CAtan2LookUpTable_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/core/bits_math.h>  // DEG2RAD
#include <mrpt/math/CAtan2LookUpTable.h>
#include <mrpt/random.h>
#include <cmath>

template <class LUT_CLASS>
void atan2_lut_test(
    const LUT_CLASS& atan2lut, const double SIZE, const double max_deg_errors,
    const double skip_area)
{
    for (int i = 0; i < 1000; i++)
    {
        const double x = mrpt::random::getRandomGenerator().drawUniform(
            -.5 * SIZE, .5 * SIZE);
        const double y = mrpt::random::getRandomGenerator().drawUniform(
            -.5 * SIZE, .5 * SIZE);

        // Avoid the central part, where accuracy is worse
        if (std::abs(x) < skip_area || std::abs(y) < skip_area) continue;

        const double atan2_good = ::atan2(y, x);
        double atan2_lut = .0;
        bool atan2_lut_valid = atan2lut.atan2(y, x, atan2_lut);

        EXPECT_TRUE(atan2_lut_valid);
        EXPECT_LT(
            std::abs(atan2_good - atan2_lut), mrpt::DEG2RAD(max_deg_errors))
            << "(x,y): (" << x << " , " << y << ")"
            << "\natan2_good:" << atan2_good << " atan2_lut:" << atan2_lut
            << std::endl;
    }
}

TEST(CAtan2LookUpTable, ValidValidTest)
{
    const double SIZE = 20.0;
    const double RES = 0.10;
    mrpt::math::CAtan2LookUpTable atan2lut(
        -.5 * SIZE, .5 * SIZE, -.5 * SIZE, .5 * SIZE, RES);
    atan2_lut_test(atan2lut, SIZE, 1.5 /*max error*/, 2.0 /* skip zone */);
}

TEST(CAtan2LookUpTable, MultiResTest)
{
    const double SIZE = 20.0;
    mrpt::math::CAtan2LookUpTableMultiRes atan2lut;

    std::map<double, double> res2extension;
    res2extension[0.001] = 0.8;  // 0.1 cm resolution
    res2extension[0.01] = 2.0;  // 1.0 cm resolution
    res2extension[0.02] = 5.0;  // 2.0 cm resolution
    res2extension[0.05] = 11.0;  // 5.0 cm resolution
    atan2lut.resize(res2extension);

    atan2_lut_test(atan2lut, SIZE, 0.5 /*max error*/, 0.4 /* skip zone */);
}