SO_SE_average_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/wrap2pi.h>
#include <mrpt/poses/SO_SE_average.h>
#include <Eigen/Dense>

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

void run_test_so2_avrg(
    const double* angs, const size_t N, const double ang_correct_avr)
{
    SO_average<2> so_avr;
    for (size_t i = 0; i < N; i++) so_avr.append(angs[i]);
    const double calc_avr = so_avr.get_average();
    EXPECT_NEAR(mrpt::math::wrapToPi(ang_correct_avr - calc_avr), .0, 1e-6);
}

TEST(SE2_SE3_avrg, SO2_average)
{
    // Simple tests:
    {
        const double angs[] = {.1};
        const double ang_correct_avr = .1;
        run_test_so2_avrg(
            angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
    }
    {
        const double angs[] = {.0, M_PI};
        const double ang_correct_avr = .5 * M_PI;
        run_test_so2_avrg(
            angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
    }
    {
        const double angs[] = {-0.75 * M_PI, 0.75 * M_PI};
        const double ang_correct_avr = 1.0 * M_PI;
        run_test_so2_avrg(
            angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
    }
    {
        const double angs[] = {-0.75 * M_PI, 0.75 * M_PI, 0.3 * M_PI};
        // const double angs_w[] = {1.0, 1.0, 0.1 };
        const double ang_correct_avr = 2.3668403111754515;
        run_test_so2_avrg(
            angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
    }
    // Test launching an exception when there is no data:
    {
        const double dummy[] = {0.};
        try
        {
            run_test_so2_avrg(dummy, 0, 0);
            GTEST_FAIL()
                << "An exception should have been raised before this point!!";
        }
        catch (std::exception&)
        {
            // This error is expected, it's OK.
        }
    }
}

void run_test_so3_avrg(
    const double* angs, const size_t N,
    const mrpt::math::CMatrixDouble33& correct_avr)
{
    SO_average<3> so_avr;
    for (size_t i = 0; i < N; i++)
    {
        mrpt::poses::CPose3D rot(
            0, 0, 0, angs[3 * i + 0], angs[3 * i + 1], angs[3 * i + 2]);
        so_avr.append(rot.getRotationMatrix());
    }
    const auto calc_avr = so_avr.get_average();
    EXPECT_NEAR((correct_avr - calc_avr).array().abs().sum(), .0, 1e-5);
}

TEST(SE2_SE3_avrg, SO3_average)
{
    // Simple tests:
    {
        const double angs[] = {.0, .0, .0};
        const auto correct_avr =
            mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
        run_test_so3_avrg(
            angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
    }
    {
        const double angs[] = {-.75 * M_PI, .0, .0, .75 * M_PI, .0, .0};
        const auto correct_avr =
            mrpt::poses::CPose3D(0, 0, 0, M_PI, 0, 0).getRotationMatrix();
        run_test_so3_avrg(
            angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
    }
    {
        const double angs[] = {.0, -0.2, .0, .0, 0.2, .0};
        const auto correct_avr =
            mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
        run_test_so3_avrg(
            angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
    }
    {
        const double angs[] = {.0, .0, .3, .0, .0, -.3};
        const auto correct_avr =
            mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
        run_test_so3_avrg(
            angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
    }
}