FakeObjectDetector.h

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
 */
 
#pragma once
 
#include <map>
#include <mutex>
#include <optional>
#include <set>
#include <string>
#include <vector>
 
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
 
#include <ArmarXSimulation/components/FakeObjectDetector/VisibilityCheck.h>
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
namespace armarx
{
 
    /**
     * @brief A fake object detector for simulation.
     *
     * Takes ground truth object poses from the simulator, but - unlike
     * `SimulationObjectPoseProvider` - only reports an object once the robot could plausibly
     * have perceived it:
     *
     * - `Always`      : report everything (parity with `SimulationObjectPoseProvider`).
     * - `FieldOfView` : report an object once its bounding box enters the camera frustum.
     * - `LineOfSight` : additionally require a clear line of sight to the front facing side
     *                   of the object's bounding box, tested against the other objects in the
     *                   scene and the robot's own body.
     *
     * Detected objects are published on the standard object pose topic, so consumers keep
     * reading the `Object/Instance` memory segment as usual.
     *
     * Objects that leave the view are simply no longer reported. They fade out through the
     * object memory's decay mechanism, which must be enabled for that to happen:
     * @code
     * ArmarX.ObjectMemory.mem.inst.decay.enabled = 1
     * @endcode
     */
    class FakeObjectDetector :
        virtual public armarx::Component,
        virtual public armarx::ObjectPoseProviderPluginUser
    {
    public:
        /// @see armarx::ManagedIceObject::getDefaultName()
        std::string getDefaultName() const override;
 
        /// Get the component's default name.
        static std::string GetDefaultName();
 
        // ObjectPoseProvider interface
    public:
        objpose::provider::RequestObjectsOutput
        requestObjects(const objpose::provider::RequestObjectsInput& input,
                       const Ice::Current& /* unused */) override;
 
        objpose::ProviderInfo getProviderInfo(const Ice::Current& /* unused */) override;
 
    protected:
        /// @see PropertyUser::createPropertyDefinitions()
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        /// @see armarx::ManagedIceObject::onInitComponent()
        void onInitComponent() override;
 
        /// @see armarx::ManagedIceObject::onConnectComponent()
        void onConnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onDisconnectComponent()
        void onDisconnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onExitComponent()
        void onExitComponent() override;
 
    private:
        /// An object of the simulated scene, prepared for the visibility checks.
        struct SceneObject
        {
            armarx::ObjectID objectID;
            Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
            /// In the object's local frame; `std::nullopt` if it had to be guessed.
            std::optional<simox::OrientedBoxf> localOOBB;
            fake_object_detector::Box box;
            /// Part of the static scene: reported without a visibility check.
            bool alwaysVisible = false;
        };
 
        void detectionTaskRun();
 
        /// Run one detection cycle and publish the result.
        void detectAndReport();
 
        /// The robot to take the camera pose from, or `nullptr` if it is not in the scene.
        const armarx::RobotVisuData* findRobot(const armarx::SceneVisuData& sceneData) const;
 
        /// Convert the simulated objects into boxes, skipping ignored ones.
        std::vector<SceneObject> collectSceneObjects(const armarx::SceneVisuData& sceneData);
 
        /**
         * The object's bounding box in its local frame.
         *
         * Looks the OOBB up via `ObjectFinder` and caches it per object class. Falls back to a
         * cube of `properties.fallbackOobbSize` when the object has no bounding box on disk.
         */
        std::optional<simox::OrientedBoxf> getLocalOOBB(const armarx::ObjectID& objectID);
 
        /// The camera frustum for the configured camera node, if that node exists.
        std::optional<fake_object_detector::Frustum>
        buildFrustum(const armarx::RobotVisuData& robot) const;
 
        /// Bounding boxes of the robot's links, in the global frame.
        std::vector<fake_object_detector::Box>
        collectRobotBoxes(const armarx::RobotVisuData& robotData);
 
        /// Load the robot model once, with collision models, so it can act as an occluder.
        VirtualRobot::RobotPtr loadRobotModel(const armarx::RobotVisuData& robotData);
 
        bool isVisible(const SceneObject& object,
                       const fake_object_detector::Frustum& frustum,
                       const std::vector<fake_object_detector::Box>& occluders) const;
 
        objpose::ProvidedObjectPose toProvidedObjectPose(const SceneObject& object,
                                                         const DateTime& time) const;
 
        void removeExpiredRequests(const DateTime& time);
        bool isRequested(const armarx::ObjectID& objectID) const;
 
    private:
        static const std::string defaultName;
 
        SimulatorInterfacePrx simulatorPrx;
 
        armarx::SimpleRunningTask<>::pointer_type detectionTask;
 
        armarx::ObjectFinder objectFinder;
        /// Cached OOBBs, keyed by class ID (`dataset/className`).
        std::map<std::string, std::optional<simox::OrientedBoxf>> oobbCache;
 
        /// Lazily loaded robot model used for self-occlusion, keyed by robot name.
        VirtualRobot::RobotPtr robotModel;
        std::string robotModelName;
        bool robotModelLoadFailed = false;
 
        mutable std::mutex activeRequestsMutex;
        /// Requested object IDs and when the request expires (invalid time = never).
        std::map<std::string, armarx::DateTime> activeRequests;
 
        /// Object IDs whose bounding box could not be found; used to warn only once.
        std::set<std::string> warnedMissingOOBB;
        bool warnedMissingCameraNode = false;
 
        /// Parsed from `properties.alwaysVisibleDatasets`.
        std::set<std::string> alwaysVisibleDatasets;
 
        struct Properties
        {
            std::int64_t updateFrequency = 10;
 
            std::string detectionMode = "FieldOfView";
            fake_object_detector::DetectionMode mode =
                fake_object_detector::DetectionMode::FieldOfView;
 
            std::string robotName;
            std::string cameraFrame = "DepthCameraSim";
 
            float horizontalFovDeg = 90.0F;
            float verticalFovDeg = 60.0F;
            float minDistance = 20.0F;
            float maxDistance = 5000.0F;
            std::string cameraForwardAxis = "+Z";
            // ArmarX rendering ('...Sim') camera nodes follow the Coin3D convention: looking
            // along +Z with +X pointing up (see VirtualRobot CoinVisualizationFactory).
            std::string cameraUpAxis = "+X";
            bool requireFullyInFov = false;
 
            int frontFaceSampleGrid = 3;
            float minVisibleFraction = 0.5F;
            bool occlusionByObjects = true;
            bool occlusionByRobot = true;
            float rayEpsilon = 1.0F;
 
            float fallbackOobbSize = 100.0F;
            float confidence = 1.0F;
            std::string ignoredObjects;
            std::string alwaysVisibleDatasets;
            bool requestedObjectsOnly = false;
        };
 
        Properties properties;
 
        /// Parsed from `properties.ignoredObjects`.
        std::set<std::string> ignoredObjects;
 
        /// Parsed from `properties.cameraForwardAxis` / `cameraUpAxis`.
        Eigen::Vector3f cameraForwardLocal = Eigen::Vector3f::UnitZ();
        Eigen::Vector3f cameraUpLocal = -Eigen::Vector3f::UnitY();
    };
 
} // namespace armarx