MRPT  1.9.9
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-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 
12 #include <mrpt/poses/CPose3DPDF.h>
13 #include <mrpt/poses/CPosePDF.h>
14 #include <mrpt/slam/CICP.h>
15 
16 #include <gtest/gtest.h>
18 #include <mrpt/opengl/CDisk.h>
22 #include <mrpt/opengl/CSphere.h>
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  float runningTime;
43  CICP::TReturnInfo info;
44  CICP ICP;
45 
46  // Load scans:
49 
52 
53  // Build the points maps from the scans:
54  m1.insertObservation(&scan1);
55  m2.insertObservation(&scan2);
56 
57  // -----------------------------------------------------
58  ICP.options.ICP_algorithm = icp_method;
59 
60  ICP.options.maxIterations = 100;
61  ICP.options.thresholdAng = DEG2RAD(10.0f);
62  ICP.options.thresholdDist = 0.75f;
63  ICP.options.ALFA = 0.5f;
64  ICP.options.smallestThresholdDist = 0.05f;
65  ICP.options.doRANSAC = false;
66  // ICP.options.dumpToConsole();
67  // -----------------------------------------------------
68  CPose2D initialPose(0.8f, 0.0f, (float)DEG2RAD(0.0f));
69 
70  CPosePDF::Ptr pdf =
71  ICP.Align(&m1, &m2, initialPose, &runningTime, (void*)&info);
72 
73  /*printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%%
74  goodness\n -> ",
75  runningTime*1000,
76  info.nIterations,
77  runningTime*1000.0f/info.nIterations,
78  info.goodness*100 );*/
79 
80  // cout << "Mean of estimation: " << pdf->getEstimatedPose() << endl<<
81  // endl;
82  // Should be around: Mean of estimation: (0.820,0.084,8.73deg)
83 
84  const CPose2D good_pose(0.820, 0.084, DEG2RAD(8.73));
85 
86  EXPECT_NEAR(good_pose.distanceTo(pdf->getMeanVal()), 0, 0.02);
87  }
88 
89  static void generateObjects(CSetOfObjects::Ptr& world)
90  {
91  CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(0.5);
92  sph->setLocation(0, 0, 0);
93  sph->setColor(1, 0, 0);
94  world->insert(sph);
95 
96  CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
97  pln->setDiskRadius(2);
98  pln->setPose(CPose3D(0, 0, 0, 0, DEG2RAD(5), DEG2RAD(5)));
99  pln->setColor(0.8, 0, 0);
100  world->insert(pln);
101 
102  {
103  CDisk::Ptr pln2 = mrpt::make_aligned_shared<opengl::CDisk>();
104  pln2->setDiskRadius(2);
105  pln2->setPose(
106  CPose3D(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(-2)));
107  pln2->setColor(0.9, 0, 0);
108  world->insert(pln2);
109  }
110  }
111 };
112 
113 TEST_F(ICPTests, AlignScans_icpClassic) { align2scans(icpClassic); }
114 TEST_F(ICPTests, AlignScans_icpLevenbergMarquardt)
115 
116 {
117  align2scans(icpLevenbergMarquardt);
118 }
119 
120 TEST_F(ICPTests, RayTracingICP3D)
121 {
122  // Increase this values to get more precision. It will also increase run
123  // time.
124  const size_t HOW_MANY_YAWS = 150;
125  const size_t HOW_MANY_PITCHS = 150;
126 
127  // The two origins for the 3D scans
128  CPose3D viewpoint1(-0.3, 0.7, 3, DEG2RAD(5), DEG2RAD(80), DEG2RAD(3));
129  CPose3D viewpoint2(0.5, -0.2, 2.6, DEG2RAD(-5), DEG2RAD(100), DEG2RAD(-7));
130 
131  CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1);
132 
133  // Create the reference objects:
134  COpenGLScene::Ptr scene1 = mrpt::make_aligned_shared<COpenGLScene>();
135  COpenGLScene::Ptr scene2 = mrpt::make_aligned_shared<COpenGLScene>();
136  COpenGLScene::Ptr scene3 = mrpt::make_aligned_shared<COpenGLScene>();
137 
139  mrpt::make_aligned_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
140  plane1->setColor(0.3, 0.3, 0.3);
141  scene1->insert(plane1);
142  scene2->insert(plane1);
143  scene3->insert(plane1);
144 
145  CSetOfObjects::Ptr world = mrpt::make_aligned_shared<CSetOfObjects>();
146  generateObjects(world);
147  scene1->insert(world);
148 
149  // Perform the 3D scans:
151  mrpt::make_aligned_shared<CAngularObservationMesh>();
153  mrpt::make_aligned_shared<CAngularObservationMesh>();
154 
155  CAngularObservationMesh::trace2DSetOfRays(
156  scene1, viewpoint1, aom1,
157  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
159  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
160  M_PI, HOW_MANY_YAWS));
161  CAngularObservationMesh::trace2DSetOfRays(
162  scene1, viewpoint2, aom2,
163  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
165  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
166  M_PI, HOW_MANY_YAWS));
167 
168  // Put the viewpoints origins:
169  {
171  origin1->setPose(viewpoint1);
172  origin1->setScale(0.6f);
173  scene1->insert(origin1);
174  scene2->insert(origin1);
175  }
176  {
178  origin2->setPose(viewpoint2);
179  origin2->setScale(0.6f);
180  scene1->insert(origin2);
181  scene2->insert(origin2);
182  }
183 
184  // Show the scanned points:
185  CSimplePointsMap M1, M2;
186 
187  aom1->generatePointCloud(&M1);
188  aom2->generatePointCloud(&M2);
189 
190  // Create the wrongly-localized M2:
191  CSimplePointsMap M2_noisy;
192  M2_noisy = M2;
194 
195  CSetOfObjects::Ptr PTNS1 = mrpt::make_aligned_shared<CSetOfObjects>();
196  CSetOfObjects::Ptr PTNS2 = mrpt::make_aligned_shared<CSetOfObjects>();
197 
198  M1.renderOptions.color = mrpt::img::TColorf(1, 0, 0);
199  M1.getAs3DObject(PTNS1);
200 
201  M2_noisy.renderOptions.color = mrpt::img::TColorf(0, 0, 1);
202  M2_noisy.getAs3DObject(PTNS2);
203 
204  scene2->insert(PTNS1);
205  scene2->insert(PTNS2);
206 
207  // --------------------------------------
208  // Do the ICP-3D
209  // --------------------------------------
210  float run_time;
211  CICP icp;
212  CICP::TReturnInfo icp_info;
213 
214  icp.options.thresholdDist = 0.40f;
215  icp.options.thresholdAng = 0;
216 
217  CPose3DPDF::Ptr pdf = icp.Align3D(
218  &M2_noisy, // Map to align
219  &M1, // Reference map
220  CPose3D(), // Initial gross estimate
221  &run_time, &icp_info);
222 
223  CPose3D mean = pdf->getMeanVal();
224 
225  // Checks:
226  EXPECT_NEAR(
227  0,
228  (mean.getAsVectorVal() - SCAN2_POSE_ERROR.getAsVectorVal())
229  .array()
230  .abs()
231  .mean(),
232  0.02)
233  << "ICP output: mean= " << mean << endl
234  << "Real displacement: " << SCAN2_POSE_ERROR << endl;
235 }
void TearDown() override
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:556
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:211
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:263
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:94
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:84
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:326
mrpt::poses::CPose3DPDF::Ptr Align3D(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning the full 6D pose.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
double DEG2RAD(const double x)
Degrees to radians.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:777
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
This base provides a set of functions for maths stuff.
CPose3D viewpoint2(0.5, -0.2, 2.6, DEG2RAD(-5), DEG2RAD(100), DEG2RAD(-7))
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:101
This namespace contains representation of robot actions and observations.
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:330
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
double ALFA
The scale factor for thresholds everytime convergence is achieved.
Definition: CICP.h:117
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1)
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
The ICP algorithm return information.
Definition: CICP.h:190
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
void SetUp() override
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:114
const size_t HOW_MANY_PITCHS
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
CPose3D viewpoint1(-0.3, 0.7, 3, DEG2RAD(5), DEG2RAD(80), DEG2RAD(3))



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: bef67d9c5 Mon Apr 15 00:03:11 2019 +0200 at lun abr 15 00:10:13 CEST 2019