MRPT  2.0.4
CICP_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-2020, 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>
14 #include <mrpt/opengl/CDisk.h>
18 #include <mrpt/opengl/CSphere.h>
20 #include <mrpt/poses/CPose3DPDF.h>
21 #include <mrpt/poses/CPosePDF.h>
22 #include <mrpt/slam/CICP.h>
23 #include <Eigen/Dense>
24 
25 using namespace mrpt;
26 using namespace mrpt::slam;
27 using namespace mrpt::maps;
28 using namespace mrpt::opengl;
29 using namespace mrpt::poses;
30 using namespace mrpt::math;
31 using namespace mrpt::obs;
32 using namespace std;
33 
34 class ICPTests : public ::testing::Test
35 {
36  protected:
37  void SetUp() override {}
38  void TearDown() override {}
39  void align2scans(const TICPAlgorithm icp_method)
40  {
41  CSimplePointsMap m1, m2;
42  CICP::TReturnInfo info;
43  CICP ICP;
44 
45  // Load scans:
48 
51 
52  // Build the points maps from the scans:
53  m1.insertObservation(scan1);
54  m2.insertObservation(scan2);
55 
56  // -----------------------------------------------------
57  ICP.options.ICP_algorithm = icp_method;
58 
59  ICP.options.maxIterations = 100;
60  ICP.options.thresholdAng = DEG2RAD(10.0f);
61  ICP.options.thresholdDist = 0.75f;
62  ICP.options.ALFA = 0.5f;
63  ICP.options.smallestThresholdDist = 0.05f;
64  ICP.options.doRANSAC = false;
65  // ICP.options.dumpToConsole();
66  // -----------------------------------------------------
67  CPose2D initialPose(0.8f, 0.0f, (float)DEG2RAD(0.0f));
68 
69  CPosePDF::Ptr pdf = ICP.Align(&m1, &m2, initialPose, info);
70 
71  const CPose2D good_pose(0.820, 0.084, 8.73_deg);
72 
73  EXPECT_NEAR(good_pose.distanceTo(pdf->getMeanVal()), 0, 0.02);
74  }
75 
76  static void generateObjects(CSetOfObjects::Ptr& world)
77  {
78  CSphere::Ptr sph = std::make_shared<CSphere>(0.5);
79  sph->setLocation(0, 0, 0);
80  sph->setColor(1, 0, 0);
81  world->insert(sph);
82 
83  CDisk::Ptr pln = std::make_shared<opengl::CDisk>();
84  pln->setDiskRadius(2);
85  pln->setPose(CPose3D(0, 0, 0, 0, 5.0_deg, 5.0_deg));
86  pln->setColor(0.8, 0, 0);
87  world->insert(pln);
88 
89  {
90  CDisk::Ptr pln2 = std::make_shared<opengl::CDisk>();
91  pln2->setDiskRadius(2);
92  pln2->setPose(CPose3D(0, 0, 0, 30.0_deg, -20.0_deg, -2.0_deg));
93  pln2->setColor(0.9, 0, 0);
94  world->insert(pln2);
95  }
96  }
97 };
98 
99 TEST_F(ICPTests, AlignScans_icpClassic) { align2scans(icpClassic); }
100 TEST_F(ICPTests, AlignScans_icpLevenbergMarquardt)
101 
102 {
103  align2scans(icpLevenbergMarquardt);
104 }
105 
106 TEST_F(ICPTests, RayTracingICP3D)
107 {
108  // Increase this values to get more precision. It will also increase run
109  // time.
110  const size_t HOW_MANY_YAWS = 150;
111  const size_t HOW_MANY_PITCHS = 150;
112 
113  // The two origins for the 3D scans
114  CPose3D viewpoint1(-0.3, 0.7, 3, 5.0_deg, 80.0_deg, 3.0_deg);
115  CPose3D viewpoint2(0.5, -0.2, 2.6, -5.0_deg, 100.0_deg, -7.0_deg);
116 
117  CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1);
118 
119  // Create the reference objects:
120  COpenGLScene::Ptr scene1 = std::make_shared<COpenGLScene>();
121  COpenGLScene::Ptr scene2 = std::make_shared<COpenGLScene>();
122  COpenGLScene::Ptr scene3 = std::make_shared<COpenGLScene>();
123 
125  std::make_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
126  plane1->setColor(0.3f, 0.3f, 0.3f);
127  scene1->insert(plane1);
128  scene2->insert(plane1);
129  scene3->insert(plane1);
130 
131  CSetOfObjects::Ptr world = std::make_shared<CSetOfObjects>();
132  generateObjects(world);
133  scene1->insert(world);
134 
135  // Perform the 3D scans:
137  std::make_shared<CAngularObservationMesh>();
139  std::make_shared<CAngularObservationMesh>();
140 
141  CAngularObservationMesh::trace2DSetOfRays(
142  scene1, viewpoint1, aom1,
143  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
145  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
146  M_PI, HOW_MANY_YAWS));
147  CAngularObservationMesh::trace2DSetOfRays(
148  scene1, viewpoint2, aom2,
149  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
151  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
152  M_PI, HOW_MANY_YAWS));
153 
154  // Put the viewpoints origins:
155  {
157  origin1->setPose(viewpoint1);
158  origin1->setScale(0.6f);
159  scene1->insert(origin1);
160  scene2->insert(origin1);
161  }
162  {
164  origin2->setPose(viewpoint2);
165  origin2->setScale(0.6f);
166  scene1->insert(origin2);
167  scene2->insert(origin2);
168  }
169 
170  // Show the scanned points:
171  CSimplePointsMap M1, M2;
172 
173  aom1->generatePointCloud(&M1);
174  aom2->generatePointCloud(&M2);
175 
176  // Create the wrongly-localized M2:
177  CSimplePointsMap M2_noisy;
178  M2_noisy = M2;
180 
181  CSetOfObjects::Ptr PTNS1 = std::make_shared<CSetOfObjects>();
182  CSetOfObjects::Ptr PTNS2 = std::make_shared<CSetOfObjects>();
183 
184  M1.renderOptions.color = mrpt::img::TColorf(1, 0, 0);
185  M1.getAs3DObject(PTNS1);
186 
187  M2_noisy.renderOptions.color = mrpt::img::TColorf(0, 0, 1);
188  M2_noisy.getAs3DObject(PTNS2);
189 
190  scene2->insert(PTNS1);
191  scene2->insert(PTNS2);
192 
193  // --------------------------------------
194  // Do the ICP-3D
195  // --------------------------------------
196  CICP icp;
197  CICP::TReturnInfo icp_info;
198 
199  icp.options.thresholdDist = 0.40f;
200  icp.options.thresholdAng = 0;
201 
202  CPose3DPDF::Ptr pdf = icp.Align3D(
203  &M2_noisy, // Map to align
204  &M1, // Reference map
205  CPose3D(), // Initial gross estimate
206  icp_info);
207 
208  CPose3D mean = pdf->getMeanVal();
209 
210  // Checks:
211  EXPECT_NEAR(
212  0,
213  (mean.asVectorVal() - SCAN2_POSE_ERROR.asVectorVal())
214  .array()
215  .abs()
216  .mean(),
217  0.02)
218  << "ICP output: mean= " << mean << endl
219  << "Real displacement: " << SCAN2_POSE_ERROR << endl;
220 }
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
void TearDown() override
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:550
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:84
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:329
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:771
void example2DRangeScan(mrpt::obs::CObservation2DRangeScan &s, int i=0)
Example 2D lidar scans (form a venerable SICK LMS200).
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
void generateObjects(CSetOfObjects::Ptr &world)
TEST_F(ICPTests, AlignScans_icpClassic)
void align2scans(const TICPAlgorithm icp_method)
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:120
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
This base provides a set of functions for maths stuff.
constexpr double DEG2RAD(const double x)
Degrees to radians.
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:101
This namespace contains representation of robot actions and observations.
CPose3D viewpoint1(-0.3, 0.7, 3, 5.0_deg, 80.0_deg, 3.0_deg)
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:133
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp.
Definition: CICP.h:19
const size_t HOW_MANY_YAWS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
static void generateObjects(CSetOfObjects::Ptr &world)
TRenderOptions renderOptions
Definition: CPointsMap.h:333
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
double ALFA
The scale factor for thresholds every time convergence is achieved.
Definition: CICP.h:117
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
mrpt::poses::CPose3DPDF::Ptr Align3D(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1)
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
The ICP algorithm return information.
Definition: CICP.h:190
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
double mean(const CONTAINER &v)
Computes the mean value of a vector.
void SetUp() override
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
CPose3D viewpoint2(0.5, -0.2, 2.6, -5.0_deg, 100.0_deg, -7.0_deg)
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:114
const size_t HOW_MANY_PITCHS
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020