CObservation3DRangeScan_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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/obs/CObservation3DRangeScan.h>

#include <gtest/gtest.h>

using namespace mrpt;
using namespace std;

// Defined in tests/test_main.cpp
namespace mrpt
{
namespace utils
{
extern std::string MRPT_GLOBAL_UNITTEST_SRC_DIR;
}
}

#define TEST_RANGEIMG_WIDTH 32
#define TEST_RANGEIMG_HEIGHT 24

void fillSampleObs(
        mrpt::obs::CObservation3DRangeScan& obs,
        mrpt::obs::T3DPointsProjectionParams& pp, int test_case)
{
        obs.hasRangeImage = true;
        obs.rangeImage_setSize(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH);

        obs.rangeImage.setZero();

        for (int r = 10; r < 16; r++)
                for (int c = 10; c <= r; c++) obs.rangeImage(r, c) = r;

        // Test case:
        pp.PROJ3D_USE_LUT = (test_case & 1) != 0;
        pp.USE_SSE2 = (test_case & 2) != 0;
        pp.takeIntoAccountSensorPoseOnRobot = (test_case & 4) != 0;
}

TEST(CObservation3DRangeScan, Project3D_noFilter)
{
        mrpt::obs::T3DPointsProjectionParams pp;
        mrpt::obs::TRangeImageFilterParams fp;

        for (int i = 0; i < 8; i++)  // test all combinations of flags
        {
                mrpt::obs::CObservation3DRangeScan o;
                fillSampleObs(o, pp, i);

                o.project3DPointsFromDepthImageInto(o, pp, fp);
                EXPECT_EQ(o.points3D_x.size(), 21U) << " testcase flags: i=" << i
                                                                                        << std::endl;
        }
}

TEST(CObservation3DRangeScan, Project3D_filterMinMax1)
{
        mrpt::math::CMatrix fMax(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH),
                fMin(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH);
        fMin(12, 12) = 11.5f;
        fMax(12, 12) = 12.5f;  // pass
        fMin(14, 14) = 15.5f;
        fMax(14, 14) = 16.5f;  // don't pass
        // Rest of points: filter=0.0f -> no filtering

        mrpt::obs::T3DPointsProjectionParams pp;
        mrpt::obs::TRangeImageFilterParams fp;
        fp.rangeMask_min = &fMin;
        fp.rangeMask_max = &fMax;

        for (int i = 0; i < 16; i++)  // test all combinations of flags
        {
                mrpt::obs::CObservation3DRangeScan o;
                fillSampleObs(o, pp, i);
                fp.rangeCheckBetween = (i & 8) != 0;

                o.project3DPointsFromDepthImageInto(o, pp, fp);
                EXPECT_EQ(o.points3D_x.size(), 20U) << " testcase flags: i=" << i
                                                                                        << std::endl;
        }
}

TEST(CObservation3DRangeScan, Project3D_filterMinMaxAllBetween)
{
        mrpt::math::CMatrix fMax(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH),
                fMin(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH);
        // Default filter=0.0f -> no filtering
        for (int r = 10; r < 16; r++)
                for (int c = 10; c < 16; c++)
                {
                        fMin(r, c) = r - 0.1f;  // All points actually lie in between
                        fMax(r, c) = r + 0.1f;
                }

        mrpt::obs::T3DPointsProjectionParams pp;
        mrpt::obs::TRangeImageFilterParams fp;
        fp.rangeMask_min = &fMin;
        fp.rangeMask_max = &fMax;

        for (int i = 0; i < 16; i++)  // test all combinations of flags
        {
                mrpt::obs::CObservation3DRangeScan o;
                fillSampleObs(o, pp, i);
                fp.rangeCheckBetween = (i & 8) != 0;

                o.project3DPointsFromDepthImageInto(o, pp, fp);
                EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 21U : 0U)
                        << " testcase flags: i=" << i << std::endl;
        }
}

TEST(CObservation3DRangeScan, Project3D_filterMinMaxNoneBetween)
{
        mrpt::math::CMatrix fMax(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH),
                fMin(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH);
        // Default filter=0.0f -> no filtering
        for (int r = 10; r < 16; r++)
                for (int c = 10; c < 16; c++)
                {
                        fMin(r, c) = r + 1.1f;  // No point lies in between
                        fMax(r, c) = r + 1.2f;
                }

        mrpt::obs::T3DPointsProjectionParams pp;
        mrpt::obs::TRangeImageFilterParams fp;
        fp.rangeMask_min = &fMin;
        fp.rangeMask_max = &fMax;

        for (int i = 0; i < 16; i++)  // test all combinations of flags
        {
                mrpt::obs::CObservation3DRangeScan o;
                fillSampleObs(o, pp, i);
                fp.rangeCheckBetween = (i & 8) != 0;

                o.project3DPointsFromDepthImageInto(o, pp, fp);
                EXPECT_EQ(o.points3D_x.size(), fp.rangeCheckBetween ? 0U : 21U)
                        << " testcase flags: i=" << i << std::endl;
        }
}

TEST(CObservation3DRangeScan, Project3D_filterMin)
{
        mrpt::math::CMatrix fMin(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH);
        // Default filter=0.0f -> no filtering
        for (int r = 10; r < 16; r++)
                for (int c = 10; c < 16; c++)
                        fMin(r, c) =
                                14.5f;  // Only last row of points should pass this filter

        mrpt::obs::T3DPointsProjectionParams pp;
        mrpt::obs::TRangeImageFilterParams fp;
        fp.rangeMask_min = &fMin;

        for (int i = 0; i < 8; i++)  // test all combinations of flags
        {
                mrpt::obs::CObservation3DRangeScan o;
                fillSampleObs(o, pp, i);

                o.project3DPointsFromDepthImageInto(o, pp, fp);
                EXPECT_EQ(o.points3D_x.size(), 6U) << " testcase flags: i=" << i
                                                                                   << std::endl;
        }
}

TEST(CObservation3DRangeScan, Project3D_filterMax)
{
        mrpt::math::CMatrix fMax(TEST_RANGEIMG_HEIGHT, TEST_RANGEIMG_WIDTH);
        // Default filter=0.0f -> no filtering
        for (int r = 10; r < 16; r++)
                for (int c = 10; c < 16; c++)
                        fMax(r, c) =
                                11.5f;  // Only first 2 rows of points should pass this filter

        mrpt::obs::T3DPointsProjectionParams pp;
        mrpt::obs::TRangeImageFilterParams fp;
        fp.rangeMask_max = &fMax;

        for (int i = 0; i < 8; i++)  // test all combinations of flags
        {
                mrpt::obs::CObservation3DRangeScan o;
                fillSampleObs(o, pp, i);

                o.project3DPointsFromDepthImageInto(o, pp, fp);
                EXPECT_EQ(o.points3D_x.size(), 3U) << " testcase flags: i=" << i
                                                                                   << std::endl;
        }
}