gaze_residual.cpp

Go to the documentation of this file.

#include "gaze_residual.h"
 
#include <algorithm>
#include <cmath>
 
namespace armarx::view_selection::gaze_controller
{
 
    GazeResidual
    computeGazeResidual(const Eigen::Vector3f& currentGazeDirection,
                        const Eigen::Vector3f& targetDirection,
                        const float targetDistance)
    {
        const float currentNorm = currentGazeDirection.norm();
        const float targetNorm = targetDirection.norm();
 
        // Either direction is degenerate: the angle between them is not defined.  Report no error
        // rather than a NaN that would silently poison the scheduler's thresholds.
        if (currentNorm < 1e-6F or targetNorm < 1e-6F)
        {
            return GazeResidual{.angularError = 0.F, .lateralError = 0.F};
        }
 
        const float cosAngle =
            std::clamp((currentGazeDirection / currentNorm).dot(targetDirection / targetNorm),
                       -1.F,
                       1.F);
        const float angularError = std::acos(cosAngle);
 
        return GazeResidual{.angularError = angularError,
                            .lateralError = std::sin(angularError) * std::abs(targetDistance)};
    }
 
} // namespace armarx::view_selection::gaze_controller