robust_kernels.h

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once
 
#include <mrpt/core/bits_math.h>
#include <mrpt/typemeta/TEnumType.h>
 
#include <cstdint>
#include <functional>
 
namespace mp2p_icp
{
enum class RobustKernel : uint8_t
{
    None = 0,
 
    GemanMcClure,
 
    Cauchy,
};
 
using robust_sqrt_weight_func_t = std::function<double(double /*errSqr*/)>;
 
inline robust_sqrt_weight_func_t create_robust_kernel(
    const RobustKernel kernel, const double kernelParam)
{
    const double kernelParamSqr = mrpt::square(kernelParam);
 
    switch (kernel)
    {
        case RobustKernel::None:
            return {};  // empty
            break;
 
        case RobustKernel::GemanMcClure:
            return [kernelParamSqr, kernelParam](double errorSqr) -> double
            { return (kernelParamSqr) / mrpt::square(errorSqr + kernelParam); };
 
        case RobustKernel::Cauchy:
            return [kernelParamSqr](double errorSqr) -> double
            { return (kernelParamSqr) / (errorSqr + kernelParamSqr); };
 
        default:
            throw std::invalid_argument("Unknown kernel type");
    };
};
 
}  // namespace mp2p_icp
 
// This allows reading/writing the enum type to strings, e.g. in YAML files.
MRPT_ENUM_TYPE_BEGIN_NAMESPACE(mp2p_icp, mp2p_icp::RobustKernel)
MRPT_FILL_ENUM(RobustKernel::None);
MRPT_FILL_ENUM(RobustKernel::GemanMcClure);
MRPT_FILL_ENUM(RobustKernel::Cauchy);
MRPT_ENUM_TYPE_END()