VisibilityCheck.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 "VisibilityCheck.h"
 
#include <algorithm>
#include <cmath>
#include <limits>
 
namespace armarx::fake_object_detector
{
 
    namespace
    {
        /// Below this, a direction component is treated as parallel to a slab.
        constexpr float parallelEpsilon = 1e-9F;
 
        /// The local axis a face is perpendicular to.
        int
        faceAxis(int faceIndex)
        {
            return faceIndex / 2;
        }
 
        /// +1 for the positive face of an axis, -1 for the negative one.
        float
        faceSign(int faceIndex)
        {
            return (faceIndex % 2 == 0) ? 1.0F : -1.0F;
        }
    } // namespace
 
    Eigen::Vector3f
    Box::center() const
    {
        return centerPose.topRightCorner<3, 1>();
    }
 
    Eigen::Matrix3f
    Box::axes() const
    {
        return centerPose.topLeftCorner<3, 3>();
    }
 
    std::array<Eigen::Vector3f, 8>
    Box::corners() const
    {
        const Eigen::Matrix3f r = axes();
        const Eigen::Vector3f c = center();
 
        std::array<Eigen::Vector3f, 8> result;
        for (int i = 0; i < 8; ++i)
        {
            // Bit i of the index selects the sign along local axis i.
            const Eigen::Vector3f signs{
                (i & 1) ? 1.0F : -1.0F, (i & 2) ? 1.0F : -1.0F, (i & 4) ? 1.0F : -1.0F};
            result[static_cast<std::size_t>(i)] = c + r * signs.cwiseProduct(halfExtents);
        }
        return result;
    }
 
    Eigen::Vector3f
    Frustum::position() const
    {
        return cameraPose.topRightCorner<3, 1>();
    }
 
    Eigen::Vector3f
    Frustum::toViewFrame(const Eigen::Vector3f& globalPoint) const
    {
        const Eigen::Matrix3f r = cameraPose.topLeftCorner<3, 3>();
        const Eigen::Vector3f local = r.transpose() * (globalPoint - position());
 
        const Eigen::Vector3f forward = forwardLocal.normalized();
        const Eigen::Vector3f up = upLocal.normalized();
        // With forward = +Z and up = -Y this yields right = +X, the usual image convention.
        const Eigen::Vector3f right = forward.cross(up);
 
        return {local.dot(forward), local.dot(right), local.dot(up)};
    }
 
    bool
    Frustum::contains(const Eigen::Vector3f& globalPoint) const
    {
        const Eigen::Vector3f view = toViewFrame(globalPoint);
        const float depth = view.x();
 
        // Also rejects everything behind the camera, as long as minDistance >= 0.
        if (depth < minDistance or depth > maxDistance)
        {
            return false;
        }
 
        const float horizontalAngle = std::atan2(view.y(), depth);
        const float verticalAngle = std::atan2(view.z(), depth);
 
        return std::abs(horizontalAngle) <= horizontalFov / 2 and
               std::abs(verticalAngle) <= verticalFov / 2;
    }
 
    bool
    isInFieldOfView(const Frustum& frustum, const Box& box, bool requireFullyInside)
    {
        const std::array<Eigen::Vector3f, 8> corners = box.corners();
 
        if (requireFullyInside)
        {
            return std::all_of(corners.begin(),
                               corners.end(),
                               [&frustum](const Eigen::Vector3f& c)
                               { return frustum.contains(c); });
        }
 
        if (std::any_of(corners.begin(),
                        corners.end(),
                        [&frustum](const Eigen::Vector3f& c) { return frustum.contains(c); }))
        {
            return true;
        }
 
        // Catches a box that is large relative to the frustum and whose corners all fall outside.
        return frustum.contains(box.center());
    }
 
    Eigen::Vector3f
    faceNormal(const Box& box, int faceIndex)
    {
        return faceSign(faceIndex) * box.axes().col(faceAxis(faceIndex)).normalized();
    }
 
    Eigen::Vector3f
    faceCenter(const Box& box, int faceIndex)
    {
        const int axis = faceAxis(faceIndex);
        return box.center() + faceSign(faceIndex) * box.axes().col(axis) * box.halfExtents(axis);
    }
 
    int
    frontFaceIndex(const Frustum& frustum, const Box& box)
    {
        const Eigen::Vector3f cameraPosition = frustum.position();
 
        int bestFace = 0;
        float bestScore = -std::numeric_limits<float>::infinity();
 
        for (int face = 0; face < 6; ++face)
        {
            const Eigen::Vector3f toFace = faceCenter(box, face) - cameraPosition;
            const float distance = toFace.norm();
            if (distance < parallelEpsilon)
            {
                // Camera sits on the face; treat it as perfectly front facing.
                return face;
            }
 
            // The more the face normal opposes the viewing direction, the more it faces us.
            const float score = -(toFace / distance).dot(faceNormal(box, face));
            if (score > bestScore)
            {
                bestScore = score;
                bestFace = face;
            }
        }
        return bestFace;
    }
 
    std::vector<Eigen::Vector3f>
    sampleFace(const Box& box, int faceIndex, int gridN)
    {
        std::vector<Eigen::Vector3f> samples;
        if (gridN < 1)
        {
            return samples;
        }
 
        const int axis = faceAxis(faceIndex);
        const int axisU = (axis + 1) % 3;
        const int axisV = (axis + 2) % 3;
 
        const Eigen::Matrix3f r = box.axes();
        const Eigen::Vector3f center = faceCenter(box, faceIndex);
 
        samples.reserve(static_cast<std::size_t>(gridN) * static_cast<std::size_t>(gridN));
        for (int i = 0; i < gridN; ++i)
        {
            // Cell centres in [-1, 1], so samples stay clear of the face edges.
            const float u = ((i + 0.5F) / gridN) * 2.0F - 1.0F;
            for (int j = 0; j < gridN; ++j)
            {
                const float v = ((j + 0.5F) / gridN) * 2.0F - 1.0F;
                samples.push_back(center + r.col(axisU) * (u * box.halfExtents(axisU)) +
                                  r.col(axisV) * (v * box.halfExtents(axisV)));
            }
        }
        return samples;
    }
 
    std::optional<float>
    intersectRayBox(const Eigen::Vector3f& origin,
                    const Eigen::Vector3f& unitDirection,
                    const Box& box)
    {
        const Eigen::Matrix3f r = box.axes();
 
        // Work in the box's local frame, where the slabs are axis aligned.
        const Eigen::Vector3f localOrigin = r.transpose() * (origin - box.center());
        const Eigen::Vector3f localDirection = r.transpose() * unitDirection;
 
        float tMin = -std::numeric_limits<float>::infinity();
        float tMax = std::numeric_limits<float>::infinity();
 
        for (int i = 0; i < 3; ++i)
        {
            const float half = box.halfExtents(i);
            if (std::abs(localDirection(i)) < parallelEpsilon)
            {
                // Parallel to this slab: a miss unless the origin is already between its planes.
                if (std::abs(localOrigin(i)) > half)
                {
                    return std::nullopt;
                }
                continue;
            }
 
            float tEnter = (-half - localOrigin(i)) / localDirection(i);
            float tExit = (half - localOrigin(i)) / localDirection(i);
            if (tEnter > tExit)
            {
                std::swap(tEnter, tExit);
            }
 
            tMin = std::max(tMin, tEnter);
            tMax = std::min(tMax, tExit);
 
            if (tMin > tMax)
            {
                return std::nullopt;
            }
        }
 
        if (tMax < 0.0F)
        {
            // The box lies entirely behind the ray origin.
            return std::nullopt;
        }
 
        return std::max(tMin, 0.0F);
    }
 
    float
    visibleFraction(const Frustum& frustum,
                    const Box& target,
                    const std::vector<Box>& occluders,
                    int gridN,
                    float epsilon)
    {
        const std::vector<Eigen::Vector3f> samples =
            sampleFace(target, frontFaceIndex(frustum, target), gridN);
        if (samples.empty())
        {
            return 0.0F;
        }
 
        const Eigen::Vector3f cameraPosition = frustum.position();
 
        std::size_t visible = 0;
        for (const Eigen::Vector3f& sample : samples)
        {
            const Eigen::Vector3f toSample = sample - cameraPosition;
            const float distance = toSample.norm();
            if (distance < parallelEpsilon)
            {
                // The camera is on the sample point - nothing can be in between.
                ++visible;
                continue;
            }
 
            const Eigen::Vector3f direction = toSample / distance;
            const bool blocked = std::any_of(
                occluders.begin(),
                occluders.end(),
                [&](const Box& occluder)
                {
                    const std::optional<float> hit =
                        intersectRayBox(cameraPosition, direction, occluder);
                    // A hit at ~0 means the camera is inside the occluder (e.g. the
                    // head link the camera is mounted in). That must not block.
                    return hit.has_value() and *hit > epsilon and *hit < distance - epsilon;
                });
 
            if (not blocked)
            {
                ++visible;
            }
        }
 
        return static_cast<float>(visible) / static_cast<float>(samples.size());
    }
 
} // namespace armarx::fake_object_detector