robust_kernels_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <gtest/gtest.h>
#include <mrpt/math/robust_kernels.h>

using namespace mrpt;
using namespace mrpt::math;
using namespace std;

// Test data tables:
// Lists of: E2 (squared error) ;  PARAM2 (squared kernel parameter)  ;  R2
// (expected robustified output)   ; R' (1st derivative)   ; R'' (2nd
// derivative)
// =============  Kernel: None (plain least squares)
const double list_test_kernel_none[][5] = {{0.0, .0, 0.0, 1.0, 0.0},
                                           {1.0, .0, 1.0, 1.0, 0.0},
                                           {10.0, .0, 10.0, 1.0, 0.0}};

// =============  Kernel: Pseudo-Huber
const double list_test_kernel_pshb[][5] = {
    {0.0, 1.0, 0.0, 1.0, -0.5},
    {0.0, 4.0, 0.0, 1.0, -0.125},
    {0.0, 9.0, 0.0, 1.0, -0.0555556},
    {1.0, 1.0, 0.828427, 0.707107, -0.176777},
    {1.0, 4.0, 0.944272, 0.894427, -0.0894427},
    {1.0, 9.0, 0.973666, 0.948683, -0.0474342},
    {4.0, 1.0, 2.47214, 0.447214, -0.0447214},
    {4.0, 4.0, 3.31371, 0.707107, -0.0441942},
    {4.0, 9.0, 3.63331, 0.83205, -0.0320019}};

template <TRobustKernelType KERNEL_TYPE>
void tester_robust_kernel(const double table[][5], const size_t N)
{
    RobustKernel<KERNEL_TYPE> rKernel;

    for (size_t i = 0; i < N; i++)
    {
        const double e2 = table[i][0];
        const double param2 = table[i][1];

        double rhop, rhopp;
        rKernel.param_sq = param2;
        double r2 = rKernel.eval(e2, rhop, rhopp);

        const double expected_r2 = table[i][2];
        const double expected_rhop = table[i][3];
        const double expected_rhopp = table[i][4];

        EXPECT_NEAR(r2, expected_r2, 1e-5);
        EXPECT_NEAR(rhop, expected_rhop, 1e-5);
        EXPECT_NEAR(rhopp, expected_rhopp, 1e-5);
    }
}

TEST(RobustKernels, PlainLeastSquares)
{
    const size_t N =
        sizeof(list_test_kernel_none) / sizeof(list_test_kernel_none[0]);
    tester_robust_kernel<rkLeastSquares>(list_test_kernel_none, N);
}

TEST(RobustKernels, PseudoHuber)
{
    const size_t N =
        sizeof(list_test_kernel_pshb) / sizeof(list_test_kernel_pshb[0]);
    tester_robust_kernel<rkPseudoHuber>(list_test_kernel_pshb, N);
}