CPointCloudFilterByDistance_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/maps/CPointCloudFilterByDistance.h>
#include <mrpt/maps/CSimplePointsMap.h>

void run_pc_filter_test(
    const double map2_x_off, const double map2_tim_off,
    const size_t expected_m1_count, const size_t expected_m2_count)
{
    const double pts1[8][3] = {
        {1, 0, 0}, {1.03, 0, 0},     {1, 1, 0},  {1.01, 1.02, 0},
        {0, 1, 0}, {-0.01, 1.01, 0}, {-1, 0, 0}, {-1.01, 0.02, 0}};
    const mrpt::poses::CPose3D pts1_pose(0, 0, 0, 0, 0, 0);
    const mrpt::system::TTimeStamp pts1_tim = mrpt::system::now();

    const mrpt::poses::CPose3D pts2_pose(0.5, 0, 0, 0, 0, 0);
    const mrpt::system::TTimeStamp pts2_tim =
        mrpt::system::timestampAdd(pts1_tim, 0.2 + map2_tim_off);

    mrpt::maps::CSimplePointsMap map1, map2;
    for (const auto& i : pts1) map1.insertPoint(i[0], i[1], i[2]);
    for (size_t i = 0; i < sizeof(pts1) / sizeof(pts1[0]); i++)
    {
        double x, y, z;
        pts2_pose.inverseComposePoint(
            pts1[i][0], pts1[i][1], pts1[i][2], x, y, z);
        // Introduce optionally, 1 outlier:
        if (i == 1) x += map2_x_off;
        map2.insertPoint(x, y, z);
    }

    mrpt::maps::CPointCloudFilterByDistance pc_filter;
    mrpt::maps::CPointCloudFilterByDistance::TExtraFilterParams extra_params;
    std::vector<bool> deletion_mask;
    extra_params.out_deletion_mask = &deletion_mask;

    pc_filter.filter(&map1, pts1_tim, pts1_pose, &extra_params);
    EXPECT_EQ(map1.size(), expected_m1_count);

    pc_filter.filter(&map2, pts2_tim, pts2_pose, &extra_params);
    EXPECT_EQ(map2.size(), expected_m2_count);
}

TEST(CPointCloudFilterByDistance, noOutliers)
{
    run_pc_filter_test(
        .0 /*map2_x_off*/, .0 /*map2_tim_off*/, 8 /*expected_m1_count*/,
        8 /*expected_m2_count*/);
}
TEST(CPointCloudFilterByDistance, withOutliers)
{
    run_pc_filter_test(
        .35 /*map2_x_off*/, .0 /*map2_tim_off*/, 8 /*expected_m1_count*/,
        6 /*expected_m2_count*/);
}
TEST(CPointCloudFilterByDistance, tooOldMap)
{
    run_pc_filter_test(
        .35 /*map2_x_off*/, 2.0 /*map2_tim_off*/, 8 /*expected_m1_count*/,
        8 /*expected_m2_count*/);
}