VisibilityCheck.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 <array>
#include <optional>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
/**
 * Geometric primitives for the fake object detector.
 *
 * This header is deliberately free of Ice, ArmarX component and Simox types so
 * that the visibility logic can be unit tested in isolation.
 *
 * All lengths are in millimetres and all poses are in the global frame, matching
 * the ArmarX/Simox convention.
 */
namespace armarx::fake_object_detector
{
 
    /// How strictly an object must be perceivable before it is reported.
    enum class DetectionMode
    {
        /// Report every object, regardless of where the camera looks.
        Always,
        /// Report an object once its bounding box enters the camera frustum.
        FieldOfView,
        /// Additionally require a clear line of sight to the front face of the bounding box.
        LineOfSight
    };
 
    /// An oriented bounding box in the global frame.
    struct Box
    {
        /// Rotation = box axes, translation = box centre.
        Eigen::Matrix4f centerPose = Eigen::Matrix4f::Identity();
        /// Half the extent along each local box axis.
        Eigen::Vector3f halfExtents = Eigen::Vector3f::Zero();
 
        Eigen::Vector3f center() const;
        Eigen::Matrix3f axes() const;
 
        /// The eight corners in global coordinates.
        std::array<Eigen::Vector3f, 8> corners() const;
    };
 
    /**
     * A symmetric view frustum.
     *
     * The camera looks along `forwardLocal` in its own frame. ArmarX/Simox render
     * cameras (the `*Sim` robot nodes) look along +Z with +X pointing up, see
     * `VirtualRobot::CoinVisualizationFactory`; the defaults reflect that.
     */
    struct Frustum
    {
        /// Camera -> global.
        Eigen::Matrix4f cameraPose = Eigen::Matrix4f::Identity();
 
        /// Full opening angles [rad].
        float horizontalFov = 1.0F;
        float verticalFov = 1.0F;
 
        /// Detection range along the viewing axis [mm].
        float minDistance = 0.0F;
        float maxDistance = 1e6F;
 
        /// Viewing axis in the camera's local frame.
        Eigen::Vector3f forwardLocal = Eigen::Vector3f::UnitZ();
        /// Up axis in the camera's local frame.
        Eigen::Vector3f upLocal = -Eigen::Vector3f::UnitY();
 
        Eigen::Vector3f position() const;
 
        /**
         * Express a global point in the camera's viewing frame.
         * @return (depth along forward, lateral along right, lateral along up).
         */
        Eigen::Vector3f toViewFrame(const Eigen::Vector3f& globalPoint) const;
 
        /// Whether a global point lies inside the frustum.
        bool contains(const Eigen::Vector3f& globalPoint) const;
    };
 
    /**
     * Whether `box` is inside the camera frustum.
     *
     * @param requireFullyInside  If true, all eight corners must be inside. Otherwise it is
     *                            enough that any corner or the box centre is inside.
     */
    bool isInFieldOfView(const Frustum& frustum, const Box& box, bool requireFullyInside);
 
    /**
     * The face of `box` that points most directly at the camera - the "front facing side"
     * of the bounding box.
     *
     * @return A face index in [0, 6): 0 = +X, 1 = -X, 2 = +Y, 3 = -Y, 4 = +Z, 5 = -Z.
     */
    int frontFaceIndex(const Frustum& frustum, const Box& box);
 
    /// Outward unit normal of `faceIndex` in global coordinates.
    Eigen::Vector3f faceNormal(const Box& box, int faceIndex);
 
    /// Centre of `faceIndex` in global coordinates.
    Eigen::Vector3f faceCenter(const Box& box, int faceIndex);
 
    /**
     * A `gridN` x `gridN` grid of sample points spread over `faceIndex`, in global coordinates.
     *
     * Samples sit at cell centres, so they never lie exactly on an edge shared with an
     * adjacent object. `gridN == 1` yields the face centre alone.
     */
    std::vector<Eigen::Vector3f> sampleFace(const Box& box, int faceIndex, int gridN);
 
    /**
     * Intersect a ray with an oriented box using the slab method.
     *
     * @param unitDirection  Must be normalized.
     * @return Distance from `origin` to the nearest intersection, or `std::nullopt` if the ray
     *         misses. Returns 0 when `origin` lies inside the box.
     */
    std::optional<float> intersectRayBox(const Eigen::Vector3f& origin,
                                         const Eigen::Vector3f& unitDirection,
                                         const Box& box);
 
    /**
     * The fraction of sample points on the front face of `target` that the camera can see.
     *
     * A sample is blocked when any occluder is hit closer than the sample itself. An occluder
     * containing the camera (such as the head link the camera is mounted in) never blocks.
     *
     * @param occluders  Must NOT contain `target`; otherwise every ray is blocked by the
     *                   target's own front face.
     * @param epsilon    Tolerance [mm] to absorb the hit exactly on the target's surface.
     * @return A value in [0, 1]; 0 if the face has no samples.
     */
    float visibleFraction(const Frustum& frustum,
                          const Box& target,
                          const std::vector<Box>& occluders,
                          int gridN,
                          float epsilon);
 
} // namespace armarx::fake_object_detector