FakeObjectDetector.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXSimulation::components::FakeObjectDetector
 * @author     Timo Birr ( timo dot birr at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FakeObjectDetector.h"
 
#include <algorithm>
#include <experimental/map>
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
namespace armarx
{
    // `armarx::Box` already exists, so the geometry types stay explicitly qualified.
    namespace fod = armarx::fake_object_detector;
 
    namespace
    {
        using fod::DetectionMode;
 
        /// Parse an axis specification such as "+Z" or "-y".
        std::optional<Eigen::Vector3f>
        parseAxis(const std::string& text)
        {
            const std::string trimmed = simox::alg::trim_copy(text);
            if (trimmed.size() != 2)
            {
                return std::nullopt;
            }
 
            float sign = 0;
            switch (trimmed.front())
            {
                case '+':
                    sign = 1.0F;
                    break;
                case '-':
                    sign = -1.0F;
                    break;
                default:
                    return std::nullopt;
            }
 
            switch (std::toupper(static_cast<unsigned char>(trimmed.back())))
            {
                case 'X':
                    return sign * Eigen::Vector3f::UnitX();
                case 'Y':
                    return sign * Eigen::Vector3f::UnitY();
                case 'Z':
                    return sign * Eigen::Vector3f::UnitZ();
                default:
                    return std::nullopt;
            }
        }
 
        std::optional<DetectionMode>
        parseDetectionMode(const std::string& text)
        {
            const std::string lower = simox::alg::to_lower(simox::alg::trim_copy(text));
            if (lower == "always")
            {
                return DetectionMode::Always;
            }
            if (lower == "fieldofview")
            {
                return DetectionMode::FieldOfView;
            }
            if (lower == "lineofsight")
            {
                return DetectionMode::LineOfSight;
            }
            return std::nullopt;
        }
 
        /// Turn an object-local OOBB plus the object's global pose into a global box.
        fod::Box
        toGlobalBox(const simox::OrientedBoxf& localOOBB, const Eigen::Matrix4f& globalPose)
        {
            fod::Box box;
            box.centerPose = globalPose * localOOBB.transformation_centered();
            box.halfExtents = localOOBB.dimensions() / 2;
            return box;
        }
    } // namespace
 
    const std::string FakeObjectDetector::defaultName = "FakeObjectDetector";
 
    armarx::PropertyDefinitionsPtr
    FakeObjectDetector::createPropertyDefinitions()
    {
        const armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->component(simulatorPrx, "Simulator");
 
        def->optional(properties.updateFrequency,
                      "UpdateFrequency",
                      "Frequency at which objects are detected and reported [Hz].");
 
        def->optional(properties.detectionMode,
                      "p.detectionMode",
                      "How strictly an object must be perceivable before it is reported: "
                      "'Always' (report everything), "
                      "'FieldOfView' (report once the bounding box enters the camera frustum), "
                      "'LineOfSight' (additionally require a clear line of sight to the front "
                      "facing side of the bounding box).");
 
        def->optional(properties.robotName,
                      "p.robotName",
                      "Name of the robot to take the camera pose from. "
                      "If empty, the first robot in the simulated scene is used.");
        def->optional(properties.cameraFrame,
                      "p.cameraFrame",
                      "Robot node used as the camera. Use a rendering ('...Sim') node so that "
                      "the viewing axis matches the simulated cameras.");
 
        def->optional(properties.horizontalFovDeg,
                      "p.horizontalFovDeg",
                      "Horizontal opening angle of the camera frustum [deg].");
        def->optional(properties.verticalFovDeg,
                      "p.verticalFovDeg",
                      "Vertical opening angle of the camera frustum [deg].");
        def->optional(properties.minDistance,
                      "p.minDistance",
                      "Minimal distance along the viewing axis at which objects are detected "
                      "[mm].");
        def->optional(properties.maxDistance,
                      "p.maxDistance",
                      "Maximal distance along the viewing axis at which objects are detected "
                      "[mm].");
        def->optional(properties.cameraForwardAxis,
                      "p.cameraForwardAxis",
                      "Viewing axis of the camera node, e.g. '+Z' or '-X'.");
        def->optional(
            properties.cameraUpAxis, "p.cameraUpAxis", "Up axis of the camera node, e.g. '-Y'.");
        def->optional(properties.requireFullyInFov,
                      "p.requireFullyInFov",
                      "If true, the whole bounding box must be inside the frustum. "
                      "Otherwise it is enough that a part of it is.");
 
        def->optional(properties.frontFaceSampleGrid,
                      "p.frontFaceSampleGrid",
                      "Line of sight is tested against an N x N grid of points on the front "
                      "facing side of the bounding box. This is N. 1 tests the face centre "
                      "only.");
        def->optional(properties.minVisibleFraction,
                      "p.minVisibleFraction",
                      "Fraction of the front face sample points that must have a clear line of "
                      "sight for the object to be detected.");
        def->optional(properties.occlusionByObjects,
                      "p.occlusionByObjects",
                      "If true, other objects in the scene can occlude an object.");
        def->optional(properties.occlusionByRobot,
                      "p.occlusionByRobot",
                      "If true, the robot's own body can occlude an object.");
        def->optional(properties.rayEpsilon,
                      "p.rayEpsilon",
                      "Numerical tolerance for the line of sight rays [mm].");
 
        def->optional(properties.fallbackOobbSize,
                      "p.fallbackOobbSize",
                      "Edge length of the cube used as bounding box for objects that have no "
                      "bounding box on disk [mm].");
        def->optional(
            properties.confidence, "p.confidence", "Confidence reported for a detected object.");
        def->optional(properties.ignoredObjects,
                      "p.ignoredObjects",
                      "Comma separated object IDs (e.g. 'Kitchen/green-cup/0') that are never "
                      "reported.");
        def->optional(properties.alwaysVisibleDatasets,
                      "p.alwaysVisibleDatasets",
                      "Comma separated object datasets (e.g. 'Interior') that make up the static "
                      "scene. Their objects are reported without any visibility check and marked "
                      "as static, so they never decay out of the object memory. All other "
                      "objects are gated by 'p.detectionMode'.");
        def->optional(properties.alwaysVisibleObjects,
                      "p.alwaysVisibleObjects",
                      "Comma separated object IDs (e.g. 'Kitchen/mobile-dishwasher/0') that are "
                      "part of the static scene even though their dataset is not. Same effect as "
                      "'p.alwaysVisibleDatasets', per object: needed where one dataset holds both "
                      "room furniture and the small objects the robot manipulates, because only "
                      "the furniture should be known without looking at it.");
        def->optional(properties.externallyOwnedObjects,
                      "p.externallyOwnedObjects",
                      "Comma separated object IDs that another component is the authority on, "
                      "e.g. an articulated object whose joint state a skill provider writes. "
                      "They are never reported, so this detector cannot overwrite that state "
                      "-- but unlike 'p.ignoredObjects' they are still used as occluders, "
                      "because they are physically there.");
        def->optional(properties.staticSceneRepublishSeconds,
                      "p.staticSceneRepublishSeconds",
                      "How often the always-visible static scene is reported even though it "
                      "has not moved [s]. Between those, a static object is reported only "
                      "when its pose changes. 0 reports it every cycle.");
        def->optional(properties.requestedObjectsOnly,
                      "p.requestedObjectsOnly",
                      "If true, only report visible objects that were requested via "
                      "`requestObjects()`. If false, all visible objects are reported.");
 
        return def;
    }
 
    void
    FakeObjectDetector::onInitComponent()
    {
        if (const std::optional<DetectionMode> mode = parseDetectionMode(properties.detectionMode))
        {
            properties.mode = *mode;
        }
        else
        {
            ARMARX_WARNING << "Unknown detection mode '" << properties.detectionMode
                           << "'. Expected one of 'Always', 'FieldOfView', 'LineOfSight'. "
                           << "Falling back to 'FieldOfView'.";
            properties.mode = DetectionMode::FieldOfView;
        }
 
        if (const std::optional<Eigen::Vector3f> axis = parseAxis(properties.cameraForwardAxis))
        {
            cameraForwardLocal = *axis;
        }
        else
        {
            ARMARX_WARNING << "Could not parse camera forward axis '"
                           << properties.cameraForwardAxis << "'. Falling back to '+Z'.";
            cameraForwardLocal = Eigen::Vector3f::UnitZ();
        }
 
        if (const std::optional<Eigen::Vector3f> axis = parseAxis(properties.cameraUpAxis))
        {
            cameraUpLocal = *axis;
        }
        else
        {
            ARMARX_WARNING << "Could not parse camera up axis '" << properties.cameraUpAxis
                           << "'. Falling back to '-Y'.";
            cameraUpLocal = -Eigen::Vector3f::UnitY();
        }
 
        if (std::abs(cameraForwardLocal.dot(cameraUpLocal)) > 1e-3F)
        {
            ARMARX_WARNING << "Camera forward axis '" << properties.cameraForwardAxis
                           << "' and up axis '" << properties.cameraUpAxis
                           << "' are not perpendicular. The frustum will be skewed.";
        }
 
        if (properties.frontFaceSampleGrid < 1)
        {
            ARMARX_WARNING << "p.frontFaceSampleGrid must be at least 1, but is "
                           << properties.frontFaceSampleGrid << ". Using 1.";
            properties.frontFaceSampleGrid = 1;
        }
 
        for (const std::string& id : simox::alg::split(properties.ignoredObjects, ","))
        {
            const std::string trimmed = simox::alg::trim_copy(id);
            if (not trimmed.empty())
            {
                ignoredObjects.insert(trimmed);
            }
        }
        if (not ignoredObjects.empty())
        {
            ARMARX_INFO << "Ignoring objects: "
                        << simox::alg::join(std::vector<std::string>(ignoredObjects.begin(),
                                                                     ignoredObjects.end()),
                                            ", ");
        }
 
        for (const std::string& dataset : simox::alg::split(properties.alwaysVisibleDatasets, ","))
        {
            const std::string trimmed = simox::alg::trim_copy(dataset);
            if (not trimmed.empty())
            {
                alwaysVisibleDatasets.insert(trimmed);
            }
        }
        for (const std::string& id : simox::alg::split(properties.alwaysVisibleObjects, ","))
        {
            const std::string trimmed = simox::alg::trim_copy(id);
            if (not trimmed.empty())
            {
                alwaysVisibleObjects.insert(trimmed);
            }
        }
 
        for (const std::string& id : simox::alg::split(properties.externallyOwnedObjects, ","))
        {
            const std::string trimmed = simox::alg::trim_copy(id);
            if (not trimmed.empty())
            {
                externallyOwnedObjects.insert(trimmed);
            }
        }
        if (not externallyOwnedObjects.empty())
        {
            ARMARX_INFO << "Not reporting these objects (another component owns their state, "
                        << "they are still used as occluders): "
                        << simox::alg::join(std::vector<std::string>(
                                                externallyOwnedObjects.begin(),
                                                externallyOwnedObjects.end()),
                                            ", ");
        }
        if (not alwaysVisibleObjects.empty())
        {
            ARMARX_INFO << "Reporting these objects as part of the always visible static scene: "
                        << simox::alg::join(std::vector<std::string>(alwaysVisibleObjects.begin(),
                                                                     alwaysVisibleObjects.end()),
                                            ", ");
        }
 
        if (not alwaysVisibleDatasets.empty())
        {
            ARMARX_INFO << "Reporting these datasets as the always visible static scene: "
                        << simox::alg::join(std::vector<std::string>(alwaysVisibleDatasets.begin(),
                                                                     alwaysVisibleDatasets.end()),
                                            ", ");
        }
    }
 
    void
    FakeObjectDetector::onConnectComponent()
    {
        detectionTask = new SimpleRunningTask<>([this]() { this->detectionTaskRun(); });
        detectionTask->start();
    }
 
    void
    FakeObjectDetector::onDisconnectComponent()
    {
        if (detectionTask)
        {
            detectionTask->stop();
            detectionTask = nullptr;
        }
    }
 
    void
    FakeObjectDetector::onExitComponent()
    {
    }
 
    std::string
    FakeObjectDetector::getDefaultName() const
    {
        return FakeObjectDetector::defaultName;
    }
 
    std::string
    FakeObjectDetector::GetDefaultName()
    {
        return FakeObjectDetector::defaultName;
    }
 
    objpose::provider::RequestObjectsOutput
    FakeObjectDetector::requestObjects(const objpose::provider::RequestObjectsInput& input,
                                       const Ice::Current& /*unused*/)
    {
        objpose::provider::RequestObjectsOutput output;
 
        const DateTime now = DateTime::Now();
        const std::scoped_lock lock(activeRequestsMutex);
 
        for (const auto& id : input.objectIDs)
        {
            const std::string entityID = id.dataset + "/" + id.className + "/" + id.instanceName;
 
            activeRequests[entityID] =
                now + armarx::core::time::Duration::MilliSeconds(input.relativeTimeoutMS);
 
            // Whether the object is actually reported depends on visibility, which is decided
            // per cycle - so the request itself always succeeds.
            output.results[id].success = true;
        }
 
        if (not properties.requestedObjectsOnly and not input.objectIDs.empty())
        {
            ARMARX_INFO << deactivateSpam(10)
                        << "All visible objects are reported by default. Requesting an object "
                           "has no effect unless 'p.requestedObjectsOnly' is enabled.";
        }
 
        return output;
    }
 
    objpose::ProviderInfo
    FakeObjectDetector::getProviderInfo(const Ice::Current& /*unused*/)
    {
        objpose::ProviderInfo info;
        info.objectType = objpose::KnownObject;
        info.proxy = getProxy<objpose::ObjectPoseProviderPrx>();
        info.supportedObjects = {};
        return info;
    }
 
    void
    FakeObjectDetector::detectionTaskRun()
    {
        Metronome metronome(Frequency::Hertz(properties.updateFrequency));
 
        while (detectionTask and not detectionTask->isStopped())
        {
            metronome.waitForNextTick();
            detectAndReport();
        }
    }
 
    void
    FakeObjectDetector::detectAndReport()
    {
        armarx::SceneVisuData sceneData;
        try
        {
            sceneData = simulatorPrx->getScene();
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << "Could not get the scene from the simulator: " << e.what();
            return;
        }
 
        const DateTime now = DateTime::Now();
        removeExpiredRequests(now);
 
        const std::vector<SceneObject> objects = collectSceneObjects(sceneData);
        std::vector<objpose::ProvidedObjectPose> providedObjects;
 
        const armarx::RobotVisuData* robotData = findRobot(sceneData);
 
        // The static scene is known to the robot at all times, in every mode, and even if the
        // camera cannot be resolved. It is still used as an occluder for the gated objects.
        //
        // It is NOT reported every cycle, though. A static pose does not decay, so one report
        // is enough until it moves -- and reporting ~25 unchanging objects at the update
        // frequency has two costs that are easy to miss:
        //   * distance_to_obstacle_costmap_provider subscribes per provider and rebuilds the
        //     whole costmap on every commit that touches a "relevant" dataset (which is every
        //     dataset except the manipulable ones). A rebuild takes ~670 ms, so a 10 Hz
        //     report keeps it permanently rebuilding and permanently behind.
        //   * every commit is a snapshot in the object memory's LTM export.
        // The pose is still compared each cycle, so a static object that IS moved (e.g. by a
        // failure-induction script) is reported immediately.
        const bool staticSceneDue =
            properties.staticSceneRepublishSeconds <= 0 or not lastStaticSceneReport.isValid() or
            (now - lastStaticSceneReport).toSecondsDouble() >=
                properties.staticSceneRepublishSeconds;
 
        for (const SceneObject& object : objects)
        {
            if (not(object.alwaysVisible and isRequested(object.objectID)))
            {
                continue;
            }
 
            if (externallyOwnedObjects.count(object.objectID.str()) > 0)
            {
                continue;
            }
 
            const std::string id = object.objectID.str();
            const auto it = lastReportedStaticPose.find(id);
            const bool moved =
                it == lastReportedStaticPose.end() or not it->second.isApprox(object.globalPose);
 
            if (staticSceneDue or moved)
            {
                lastReportedStaticPose[id] = object.globalPose;
                providedObjects.push_back(toProvidedObjectPose(object, now));
            }
        }
 
        if (staticSceneDue)
        {
            lastStaticSceneReport = now;
        }
 
        if (properties.mode == DetectionMode::Always)
        {
            for (const SceneObject& object : objects)
            {
                if (not object.alwaysVisible and isRequested(object.objectID) and
                    externallyOwnedObjects.count(object.objectID.str()) == 0)
                {
                    providedObjects.push_back(toProvidedObjectPose(object, now));
                }
            }
        }
        else if (robotData != nullptr)
        {
            if (const std::optional<fod::Frustum> frustum = buildFrustum(*robotData))
            {
                std::vector<fod::Box> robotBoxes;
                if (properties.mode == DetectionMode::LineOfSight and properties.occlusionByRobot)
                {
                    robotBoxes = collectRobotBoxes(*robotData);
                }
 
                for (std::size_t i = 0; i < objects.size(); ++i)
                {
                    // Already reported above as part of the static scene.
                    if (objects[i].alwaysVisible or not isRequested(objects[i].objectID) or
                        externallyOwnedObjects.count(objects[i].objectID.str()) > 0)
                    {
                        continue;
                    }
 
                    std::vector<fod::Box> occluders;
                    if (properties.mode == DetectionMode::LineOfSight)
                    {
                        occluders.reserve(objects.size() + robotBoxes.size());
                        if (properties.occlusionByObjects)
                        {
                            for (std::size_t j = 0; j < objects.size(); ++j)
                            {
                                // The target must never occlude itself.
                                if (j != i)
                                {
                                    occluders.push_back(objects[j].box);
                                }
                            }
                        }
                        occluders.insert(occluders.end(), robotBoxes.begin(), robotBoxes.end());
                    }
 
                    if (isVisible(objects[i], *frustum, occluders))
                    {
                        providedObjects.push_back(toProvidedObjectPose(objects[i], now));
                    }
                }
            }
        }
        else
        {
            ARMARX_WARNING << deactivateSpam(10) << "No robot '" << properties.robotName
                           << "' in the simulated scene. Reporting no objects.";
        }
 
        ARMARX_DEBUG << deactivateSpam(10) << "Detected " << providedObjects.size() << " of "
                     << objects.size() << " objects.";
 
        objpose::data::ProvidedObjectPoseSeq providedObjectsIce = objpose::toIce(providedObjects);
 
        // `robotName` only exists on the Ice struct. The object memory needs it to resolve the
        // robot and fill in `objectPoseRobot`; without it, it logs "Failed to retrieve robot".
        const std::string robotName = robotData != nullptr ? robotData->name : properties.robotName;
        for (objpose::data::ProvidedObjectPose& pose : providedObjectsIce)
        {
            pose.robotName = robotName;
        }
 
        try
        {
            objectPoseTopic->reportObjectPoses(getName(), providedObjectsIce);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_INFO << deactivateSpam(10)
                        << "Could not report object poses to the object memory: " << e.what();
        }
    }
 
    const armarx::RobotVisuData*
    FakeObjectDetector::findRobot(const armarx::SceneVisuData& sceneData) const
    {
        if (sceneData.robots.empty())
        {
            return nullptr;
        }
        if (properties.robotName.empty())
        {
            return &sceneData.robots.front();
        }
 
        const auto it = std::find_if(sceneData.robots.begin(),
                                     sceneData.robots.end(),
                                     [this](const armarx::RobotVisuData& robot)
                                     { return robot.name == properties.robotName; });
 
        return it != sceneData.robots.end() ? &(*it) : nullptr;
    }
 
    std::vector<FakeObjectDetector::SceneObject>
    FakeObjectDetector::collectSceneObjects(const armarx::SceneVisuData& sceneData)
    {
        std::vector<SceneObject> objects;
        objects.reserve(sceneData.objects.size());
 
        for (const armarx::ObjectVisuData& visuData : sceneData.objects)
        {
            if (ignoredObjects.count(visuData.name) > 0)
            {
                continue;
            }
 
            const auto poseIt = visuData.objectPoses.find(visuData.name);
            if (poseIt == visuData.objectPoses.end())
            {
                ARMARX_WARNING << deactivateSpam(10) << "Simulated object '" << visuData.name
                               << "' has no pose. Skipping it.";
                continue;
            }
 
            SceneObject object;
            object.objectID = armarx::ObjectID(visuData.name);
            object.globalPose = armarx::fromIce(poseIt->second);
            object.localOOBB = getLocalOOBB(object.objectID);
            object.alwaysVisible = alwaysVisibleDatasets.count(object.objectID.dataset()) > 0 or
                                   alwaysVisibleObjects.count(object.objectID.str()) > 0;
 
            const Eigen::Matrix4f identity = Eigen::Matrix4f::Identity();
            const Eigen::Vector3f fallbackExtents =
                Eigen::Vector3f::Constant(properties.fallbackOobbSize);
            const simox::OrientedBoxf oobb =
                object.localOOBB.value_or(simox::OrientedBoxf(identity, fallbackExtents));
            object.box = toGlobalBox(oobb, object.globalPose);
 
            objects.push_back(std::move(object));
        }
 
        return objects;
    }
 
    std::optional<simox::OrientedBoxf>
    FakeObjectDetector::getLocalOOBB(const armarx::ObjectID& objectID)
    {
        // The OOBB is a property of the object class, so cache it per class.
        const std::string classID = objectID.getClassID().str();
 
        if (const auto it = oobbCache.find(classID); it != oobbCache.end())
        {
            return it->second;
        }
 
        std::optional<simox::OrientedBoxf> oobb;
        if (const std::optional<ObjectInfo> info = objectFinder.findObject(objectID))
        {
            oobb = info->loadOOBB();
        }
 
        if (not oobb and warnedMissingOOBB.insert(classID).second)
        {
            ARMARX_WARNING << "No bounding box found for object class '" << classID
                           << "'. Using a cube of " << properties.fallbackOobbSize
                           << " mm instead.";
        }
 
        oobbCache[classID] = oobb;
        return oobb;
    }
 
    std::optional<fod::Frustum>
    FakeObjectDetector::buildFrustum(const armarx::RobotVisuData& robot) const
    {
        const auto it = robot.robotNodePoses.find(properties.cameraFrame);
        if (it == robot.robotNodePoses.end())
        {
            if (not warnedMissingCameraNode)
            {
                std::vector<std::string> nodeNames;
                nodeNames.reserve(robot.robotNodePoses.size());
                for (const auto& [name, _] : robot.robotNodePoses)
                {
                    nodeNames.push_back(name);
                }
                ARMARX_WARNING << "Robot '" << robot.name << "' has no node '"
                               << properties.cameraFrame << "' to use as camera. "
                               << "Available nodes are: " << nodeNames;
                const_cast<FakeObjectDetector*>(this)->warnedMissingCameraNode = true;
            }
            return std::nullopt;
        }
 
        fod::Frustum frustum;
        frustum.cameraPose = armarx::fromIce(it->second);
        frustum.horizontalFov = simox::math::deg_to_rad(properties.horizontalFovDeg);
        frustum.verticalFov = simox::math::deg_to_rad(properties.verticalFovDeg);
        frustum.minDistance = properties.minDistance;
        frustum.maxDistance = properties.maxDistance;
        frustum.forwardLocal = cameraForwardLocal;
        frustum.upLocal = cameraUpLocal;
        return frustum;
    }
 
    VirtualRobot::RobotPtr
    FakeObjectDetector::loadRobotModel(const armarx::RobotVisuData& robotData)
    {
        if (robotModel and robotModelName == robotData.name)
        {
            return robotModel;
        }
        if (robotModelLoadFailed)
        {
            return nullptr;
        }
 
        if (robotData.robotFile.empty())
        {
            ARMARX_WARNING << "Robot '" << robotData.name
                           << "' has no model file. It cannot occlude objects.";
            robotModelLoadFailed = true;
            return nullptr;
        }
 
        if (not robotData.project.empty())
        {
            const CMakePackageFinder finder(robotData.project);
            if (finder.packageFound())
            {
                ArmarXDataPath::addDataPaths(finder.getDataDir());
            }
            else
            {
                ARMARX_WARNING << "ArmarX package '" << robotData.project << "' was not found.";
            }
        }
 
        std::string filename = robotData.robotFile;
        ArmarXDataPath::getAbsolutePath(filename, filename);
 
        try
        {
            // Collision models are required - the default (eStructure) has no geometry at all.
            robotModel =
                VirtualRobot::RobotIO::loadRobot(filename, VirtualRobot::RobotIO::eCollisionModel);
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "Could not load the robot model from '" << filename
                           << "': " << e.what() << ". The robot will not occlude objects.";
        }
 
        if (not robotModel)
        {
            robotModelLoadFailed = true;
            return nullptr;
        }
 
        robotModelName = robotData.name;
        ARMARX_INFO << "Loaded robot model '" << robotData.name << "' from '" << filename
                    << "' for self-occlusion checks.";
        return robotModel;
    }
 
    std::vector<fod::Box>
    FakeObjectDetector::collectRobotBoxes(const armarx::RobotVisuData& robotData)
    {
        const VirtualRobot::RobotPtr robot = loadRobotModel(robotData);
        if (not robot)
        {
            return {};
        }
 
        std::map<std::string, float> jointValues;
        for (const auto& [name, value] : robotData.jointValues)
        {
            if (robot->hasRobotNode(name))
            {
                jointValues[name] = value;
            }
        }
        robot->setJointValues(jointValues);
        if (robotData.pose)
        {
            robot->setGlobalPose(armarx::fromIce(robotData.pose));
        }
 
        std::vector<fod::Box> boxes;
        for (const VirtualRobot::RobotNodePtr& node : robot->getRobotNodes())
        {
            const VirtualRobot::CollisionModelPtr collisionModel = node->getCollisionModel();
            if (not collisionModel)
            {
                continue;
            }
 
            // Take the bounding box in the link's own frame and orient it with the link, rather
            // than using getBoundingBox(true). A global axis aligned box around a link lying
            // diagonally is far larger than the link and would occlude much of the scene.
            const VirtualRobot::BoundingBox boundingBox = collisionModel->getBoundingBox(false);
            const Eigen::Vector3f min = boundingBox.getMin();
            const Eigen::Vector3f max = boundingBox.getMax();
            const Eigen::Vector3f extents = max - min;
            if (extents.minCoeff() <= 0)
            {
                continue;
            }
 
            Eigen::Matrix4f localCenter = Eigen::Matrix4f::Identity();
            localCenter.topRightCorner<3, 1>() = (min + max) / 2;
 
            fod::Box box;
            box.centerPose = collisionModel->getGlobalPose() * localCenter;
            box.halfExtents = extents / 2;
            boxes.push_back(box);
        }
 
        return boxes;
    }
 
    bool
    FakeObjectDetector::isVisible(const SceneObject& object,
                                  const fod::Frustum& frustum,
                                  const std::vector<fod::Box>& occluders) const
    {
        if (not fod::isInFieldOfView(frustum, object.box, properties.requireFullyInFov))
        {
            return false;
        }
 
        if (properties.mode != DetectionMode::LineOfSight)
        {
            return true;
        }
 
        const float fraction = fod::visibleFraction(
            frustum, object.box, occluders, properties.frontFaceSampleGrid, properties.rayEpsilon);
 
        return fraction >= properties.minVisibleFraction;
    }
 
    objpose::ProvidedObjectPose
    FakeObjectDetector::toProvidedObjectPose(const SceneObject& object, const DateTime& time) const
    {
        objpose::ProvidedObjectPose pose;
 
        pose.providerName = getName();
        pose.objectType = objpose::ObjectType::KnownObject;
        // Static poses never decay. That is what keeps the static scene permanently in memory,
        // and exactly why a detected object must not be marked static.
        pose.isStatic = object.alwaysVisible;
 
        pose.objectID = object.objectID;
        pose.objectPose = object.globalPose;
        pose.objectPoseFrame = armarx::GlobalFrame;
 
        pose.localOOBB = object.localOOBB;
        pose.confidence = properties.confidence;
        // Must advance every cycle, otherwise the memory discards the update.
        pose.timestamp = time;
 
        return pose;
    }
 
    void
    FakeObjectDetector::removeExpiredRequests(const DateTime& time)
    {
        const std::scoped_lock lock(activeRequestsMutex);
 
        std::experimental::erase_if(activeRequests,
                                    [&time](const auto& request)
                                    {
                                        const DateTime& until = request.second;
                                        return (not until.isInvalid()) and time > until;
                                    });
    }
 
    bool
    FakeObjectDetector::isRequested(const armarx::ObjectID& objectID) const
    {
        if (not properties.requestedObjectsOnly)
        {
            return true;
        }
 
        const std::scoped_lock lock(activeRequestsMutex);
        return activeRequests.count(objectID.str()) > 0;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::FakeObjectDetector,
                                         ::armarx::FakeObjectDetector::GetDefaultName());
 
} // namespace armarx