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 
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  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  const CPose2D good_pose(0.820, 0.084, 8.73_deg);
74 
75  EXPECT_NEAR(good_pose.distanceTo(pdf->getMeanVal()), 0, 0.02);
76  }
77 
78  static void generateObjects(CSetOfObjects::Ptr& world)
79  {
80  CSphere::Ptr sph = std::make_shared<CSphere>(0.5);
81  sph->setLocation(0, 0, 0);
82  sph->setColor(1, 0, 0);
83  world->insert(sph);
84 
85  CDisk::Ptr pln = std::make_shared<opengl::CDisk>();
86  pln->setDiskRadius(2);
87  pln->setPose(CPose3D(0, 0, 0, 0, 5.0_deg, 5.0_deg));
88  pln->setColor(0.8, 0, 0);
89  world->insert(pln);
90 
91  {
92  CDisk::Ptr pln2 = std::make_shared<opengl::CDisk>();
93  pln2->setDiskRadius(2);
94  pln2->setPose(CPose3D(0, 0, 0, 30.0_deg, -20.0_deg, -2.0_deg));
95  pln2->setColor(0.9, 0, 0);
96  world->insert(pln2);
97  }
98  }
99 };
100 
101 TEST_F(ICPTests, AlignScans_icpClassic) { align2scans(icpClassic); }
102 TEST_F(ICPTests, AlignScans_icpLevenbergMarquardt)
103 
104 {
105  align2scans(icpLevenbergMarquardt);
106 }
107 
108 TEST_F(ICPTests, RayTracingICP3D)
109 {
110  // Increase this values to get more precision. It will also increase run
111  // time.
112  const size_t HOW_MANY_YAWS = 150;
113  const size_t HOW_MANY_PITCHS = 150;
114 
115  // The two origins for the 3D scans
116  CPose3D viewpoint1(-0.3, 0.7, 3, 5.0_deg, 80.0_deg, 3.0_deg);
117  CPose3D viewpoint2(0.5, -0.2, 2.6, -5.0_deg, 100.0_deg, -7.0_deg);
118 
119  CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1);
120 
121  // Create the reference objects:
122  COpenGLScene::Ptr scene1 = std::make_shared<COpenGLScene>();
123  COpenGLScene::Ptr scene2 = std::make_shared<COpenGLScene>();
124  COpenGLScene::Ptr scene3 = std::make_shared<COpenGLScene>();
125 
127  std::make_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
128  plane1->setColor(0.3, 0.3, 0.3);
129  scene1->insert(plane1);
130  scene2->insert(plane1);
131  scene3->insert(plane1);
132 
133  CSetOfObjects::Ptr world = std::make_shared<CSetOfObjects>();
134  generateObjects(world);
135  scene1->insert(world);
136 
137  // Perform the 3D scans:
139  std::make_shared<CAngularObservationMesh>();
141  std::make_shared<CAngularObservationMesh>();
142 
143  CAngularObservationMesh::trace2DSetOfRays(
144  scene1, viewpoint1, aom1,
145  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
147  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
148  M_PI, HOW_MANY_YAWS));
149  CAngularObservationMesh::trace2DSetOfRays(
150  scene1, viewpoint2, aom2,
151  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
153  CAngularObservationMesh::TDoubleRange::CreateFromAperture(
154  M_PI, HOW_MANY_YAWS));
155 
156  // Put the viewpoints origins:
157  {
159  origin1->setPose(viewpoint1);
160  origin1->setScale(0.6f);
161  scene1->insert(origin1);
162  scene2->insert(origin1);
163  }
164  {
166  origin2->setPose(viewpoint2);
167  origin2->setScale(0.6f);
168  scene1->insert(origin2);
169  scene2->insert(origin2);
170  }
171 
172  // Show the scanned points:
173  CSimplePointsMap M1, M2;
174 
175  aom1->generatePointCloud(&M1);
176  aom2->generatePointCloud(&M2);
177 
178  // Create the wrongly-localized M2:
179  CSimplePointsMap M2_noisy;
180  M2_noisy = M2;
182 
183  CSetOfObjects::Ptr PTNS1 = std::make_shared<CSetOfObjects>();
184  CSetOfObjects::Ptr PTNS2 = std::make_shared<CSetOfObjects>();
185 
186  M1.renderOptions.color = mrpt::img::TColorf(1, 0, 0);
187  M1.getAs3DObject(PTNS1);
188 
189  M2_noisy.renderOptions.color = mrpt::img::TColorf(0, 0, 1);
190  M2_noisy.getAs3DObject(PTNS2);
191 
192  scene2->insert(PTNS1);
193  scene2->insert(PTNS2);
194 
195  // --------------------------------------
196  // Do the ICP-3D
197  // --------------------------------------
198  float run_time;
199  CICP icp;
200  CICP::TReturnInfo icp_info;
201 
202  icp.options.thresholdDist = 0.40f;
203  icp.options.thresholdAng = 0;
204 
205  CPose3DPDF::Ptr pdf = icp.Align3D(
206  &M2_noisy, // Map to align
207  &M1, // Reference map
208  CPose3D(), // Initial gross estimate
209  &run_time, &icp_info);
210 
211  CPose3D mean = pdf->getMeanVal();
212 
213  // Checks:
214  EXPECT_NEAR(
215  0,
216  (mean.asVectorVal() - SCAN2_POSE_ERROR.asVectorVal())
217  .array()
218  .abs()
219  .mean(),
220  0.02)
221  << "ICP output: mean= " << mean << endl
222  << "Real displacement: " << SCAN2_POSE_ERROR << endl;
223 }
void TearDown() override
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:547
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:328
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
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:768
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:332
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 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:78
The ICP algorithm return information.
Definition: CICP.h:190
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
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 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019