CHeightGridMap2Ds_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include <mrpt/maps/CHeightGridMap2D_MRF.h>
#include <mrpt/maps/CHeightGridMap2D.h>
#include <gtest/gtest.h>
 
template <class MAP>
void do_test_insertCheckMapBounds()
{
        mrpt::maps::CHeightGridMap2D_Base::TPointInsertParams pt_params;
        pt_params.update_map_after_insertion = false;
 
        MAP dem;  // mrpt::maps::CHeightGridMap2D dem;
        dem.setSize(-4.0, 4.0, 0.0, 4.0, 1.0);  // x:[-10,10] * y:[0,5]
        // Inside:
        EXPECT_TRUE(dem.insertIndividualPoint(2.0, 3.0, 56.0, pt_params));
        EXPECT_TRUE(dem.insertIndividualPoint(-3.0, 0.4, 56.0, pt_params));
        EXPECT_TRUE(dem.insertIndividualPoint(3.0, 3.8, 56.0, pt_params));
        // Outside:
        EXPECT_FALSE(dem.insertIndividualPoint(-11.0, 2.0, 56.0, pt_params));
        EXPECT_FALSE(dem.insertIndividualPoint(11.0, 2.0, 56.0, pt_params));
        EXPECT_FALSE(dem.insertIndividualPoint(2.0, -1.0, 56.0, pt_params));
        EXPECT_FALSE(dem.insertIndividualPoint(2.0, 6.0, 56.0, pt_params));
}
TEST(CHeightGridMap2Ds, insertCheckMapBounds)
{
        do_test_insertCheckMapBounds<mrpt::maps::CHeightGridMap2D>();
        do_test_insertCheckMapBounds<mrpt::maps::CHeightGridMap2D_MRF>();
}
 
template <class MAP>
void do_test_insertPointsAndRead()
{
        MAP dem;
        dem.setSize(0.0, 5.0, 0.0, 5.0, 0.5);  // x:[-10,10] * y:[0,5]
        // Inside:
        const double x = 2.1, y = 3.1, z_write = 56.0;
        dem.insertIndividualPoint(x, y, z_write);
        double z_read;
        bool res = dem.dem_get_z(x, y, z_read);
        EXPECT_TRUE(res);
        EXPECT_NEAR(z_read, z_write, 1e-6);
}
TEST(CHeightGridMap2Ds, insertPointsAndRead)
{
        do_test_insertPointsAndRead<mrpt::maps::CHeightGridMap2D>();
        do_test_insertPointsAndRead<mrpt::maps::CHeightGridMap2D_MRF>();
}