MRPT  1.9.9
CPointCloudFilterByDistance_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
13 
15  const double map2_x_off, const double map2_tim_off,
16  const size_t expected_m1_count, const size_t expected_m2_count)
17 {
18  const double pts1[8][3] = {
19  {1, 0, 0}, {1.03, 0, 0}, {1, 1, 0}, {1.01, 1.02, 0},
20  {0, 1, 0}, {-0.01, 1.01, 0}, {-1, 0, 0}, {-1.01, 0.02, 0}};
21  const mrpt::poses::CPose3D pts1_pose(0, 0, 0, 0, 0, 0);
23 
24  const mrpt::poses::CPose3D pts2_pose(0.5, 0, 0, 0, 0, 0);
25  const mrpt::system::TTimeStamp pts2_tim =
26  mrpt::system::timestampAdd(pts1_tim, 0.2 + map2_tim_off);
27 
29  for (const auto& i : pts1) map1.insertPoint(i[0], i[1], i[2]);
30  for (size_t i = 0; i < sizeof(pts1) / sizeof(pts1[0]); i++)
31  {
32  double x, y, z;
33  pts2_pose.inverseComposePoint(
34  pts1[i][0], pts1[i][1], pts1[i][2], x, y, z);
35  // Introduce optionally, 1 outlier:
36  if (i == 1) x += map2_x_off;
37  map2.insertPoint(x, y, z);
38  }
39 
42  std::vector<bool> deletion_mask;
43  extra_params.out_deletion_mask = &deletion_mask;
44 
45  pc_filter.filter(&map1, pts1_tim, pts1_pose, &extra_params);
46  EXPECT_EQ(map1.size(), expected_m1_count);
47 
48  pc_filter.filter(&map2, pts2_tim, pts2_pose, &extra_params);
49  EXPECT_EQ(map2.size(), expected_m2_count);
50 }
51 
52 TEST(CPointCloudFilterByDistance, noOutliers)
53 {
55  .0 /*map2_x_off*/, .0 /*map2_tim_off*/, 8 /*expected_m1_count*/,
56  8 /*expected_m2_count*/);
57 }
58 TEST(CPointCloudFilterByDistance, withOutliers)
59 {
61  .35 /*map2_x_off*/, .0 /*map2_tim_off*/, 8 /*expected_m1_count*/,
62  6 /*expected_m2_count*/);
63 }
64 TEST(CPointCloudFilterByDistance, tooOldMap)
65 {
67  .35 /*map2_x_off*/, 2.0 /*map2_tim_off*/, 8 /*expected_m1_count*/,
68  8 /*expected_m2_count*/);
69 }
std::vector< bool > * out_deletion_mask
If a pointer is provided to a user-given container, the list of points to be deleted will be marked h...
GLdouble GLdouble z
Definition: glext.h:3879
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:625
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)
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:646
TEST(CPointCloudFilterByDistance, noOutliers)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
Implementation of pointcloud filtering based on requisities for minimum neigbouring points in both...
GLenum GLint GLint y
Definition: glext.h:3542
mrpt::system::TTimeStamp timestampAdd(const mrpt::system::TTimeStamp tim, const double num_seconds)
Shifts a timestamp the given amount of seconds (>0: forwards in time, <0: backwards) ...
Definition: datetime.h:146
GLenum GLint x
Definition: glext.h:3542
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:422
void filter(mrpt::maps::CPointsMap *inout_pointcloud, const mrpt::system::TTimeStamp pc_timestamp, const mrpt::poses::CPose3D &pc_reference_pose, TExtraFilterParams *params=nullptr) override
Apply the filtering algorithm to the pointcloud.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019