sensor_error.hpp

 
#ifndef UFO_MAP_INTEGRATOR_SENSOR_ERROR_HPP
#define UFO_MAP_INTEGRATOR_SENSOR_ERROR_HPP
 
// STL
#include <cmath>
 
namespace ufo
{
struct FixedSensorError {
    float error_distance{};
 
    [[nodiscard]] float operator()(float /* distance */) const { return error_distance; }
};
 
struct LinearSensorError {
    float factor{};
 
    [[nodiscard]] float operator()(float distance) const { return factor * distance; }
};
 
struct PolynomialSensorError {
    float a = 0.002797f;
    float b = -0.004249f;
    float c = 0.007311f;
 
    [[nodiscard]] float operator()(float distance) const
    {
        return a + b * distance + c * distance * distance;
    }
};
 
struct ExponentialSensorError {
    float a = 0.0005877f;
    float b = 0.9925f;
 
    [[nodiscard]] float operator()(float distance) const
    {
        return a * std::exp(b * distance);
    }
};
 
static constexpr PolynomialSensorError const  KinectV1ErrorFunction{0.002797f, -0.004249f,
                                                                   0.007311};
static constexpr ExponentialSensorError const KinectV2ErrorFunction{0.0005877f, 0.9925f};
static constexpr ExponentialSensorError const ZEDErrorFunction{0.007437f, 0.3855f};
}  // namespace ufo
 
#endif  // UFO_MAP_INTEGRATOR_SENSOR_ERROR_HPP