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::maps::CPointsMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:771
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align3D
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)
Definition: CMetricMapsAlignmentAlgorithm.cpp:31
mrpt::maps::CPointsMap::changeCoordinatesReference
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
Definition: CPointsMap.cpp:550
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align
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.
Definition: CMetricMapsAlignmentAlgorithm.cpp:22
mrpt::slam::CICP::options
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
mrpt::slam::CICP::TConfigParams::smallestThresholdDist
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough.
Definition: CICP.h:120
mrpt::slam::CICP::TConfigParams::ALFA
double ALFA
The scale factor for thresholds every time convergence is achieved.
Definition: CICP.h:117
mrpt::slam::CICP::TConfigParams::thresholdDist
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:114
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:31
CSphere.h
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::maps::CMetricMap::insertObservation
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93
mrpt::slam::icpLevenbergMarquardt
@ icpLevenbergMarquardt
Definition: CICP.h:22
CSetOfObjects.h
generateObjects
void generateObjects(CSetOfObjects::Ptr &world)
Definition: vision_stereo_rectify/test.cpp:107
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:244
CICP.h
mrpt::opengl::CDisk::Ptr
std::shared_ptr< mrpt::opengl ::CDisk > Ptr
Definition: CDisk.h:32
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::slam::TICPAlgorithm
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options.
Definition: CICP.h:19
stock_objects.h
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::slam::icpClassic
@ icpClassic
Definition: CICP.h:21
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:190
stock_observations.h
viewpoint1
CPose3D viewpoint1(-0.3, 0.7, 3, 5.0_deg, 80.0_deg, 3.0_deg)
ICPTests::align2scans
void align2scans(const TICPAlgorithm icp_method)
Definition: CICP_unittest.cpp:39
CDisk.h
CPosePDF.h
COpenGLScene.h
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::img::TColorf
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
mrpt::poses::CPoseOrPoint::asVectorVal
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
CAngularObservationMesh.h
mrpt::opengl::CAngularObservationMesh::Ptr
std::shared_ptr< mrpt::opengl ::CAngularObservationMesh > Ptr
Definition: CAngularObservationMesh.h:45
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
ICPTests
Definition: CICP_unittest.cpp:34
mrpt::slam::CICP::TConfigParams::thresholdAng
double thresholdAng
Definition: CICP.h:114
mrpt::maps::CPointsMap::renderOptions
TRenderOptions renderOptions
Definition: CPointsMap.h:333
mrpt::slam::CICP
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::opengl::CSphere::Ptr
std::shared_ptr< mrpt::opengl ::CSphere > Ptr
Definition: CSphere.h:31
HOW_MANY_YAWS
const size_t HOW_MANY_YAWS
Definition: vision_stereo_rectify/test.cpp:55
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
TEST_F
TEST_F(ICPTests, AlignScans_icpClassic)
Definition: CICP_unittest.cpp:99
mrpt::opengl::stock_objects::CornerXYZ
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
Definition: StockObjects.cpp:136
mrpt::obs::stock_observations::example2DRangeScan
void example2DRangeScan(mrpt::obs::CObservation2DRangeScan &s, int i=0)
Example 2D lidar scans (form a venerable SICK LMS200).
Definition: stock_observations.cpp:23
mrpt::slam::CICP::TConfigParams::maxIterations
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:101
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::slam::CICP::TConfigParams::doRANSAC
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:133
SCAN2_POSE_ERROR
CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1)
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
CPose3DPDF.h
ICPTests::TearDown
void TearDown() override
Definition: CICP_unittest.cpp:38
mrpt::maps
Definition: CBeacon.h:21
ICPTests::generateObjects
static void generateObjects(CSetOfObjects::Ptr &world)
Definition: CICP_unittest.cpp:76
mrpt::maps::CPointsMap::TRenderOptions::color
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:329
HOW_MANY_PITCHS
const size_t HOW_MANY_PITCHS
Definition: vision_stereo_rectify/test.cpp:56
CSimplePointsMap.h
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:41
CGridPlaneXY.h
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
viewpoint2
CPose3D viewpoint2(0.5, -0.2, 2.6, -5.0_deg, 100.0_deg, -7.0_deg)
ICPTests::SetUp
void SetUp() override
Definition: CICP_unittest.cpp:37
mrpt::slam::CICP::TConfigParams::ICP_algorithm
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:84



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020