MRPT  2.0.4
test.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 /**
11  * Execute the iterating closest point algorithm (ICP) on a hardcoded pair of
12  * laser data. The algorithm computes the transformation (translation and
13  * rotation) for aligning the 2 sets of laser scans and plots the
14  */
15 
16 #include <mrpt/gui.h>
18 #include <mrpt/math/utils.h>
21 #include <mrpt/poses/CPose2D.h>
22 #include <mrpt/poses/CPosePDF.h>
24 #include <mrpt/slam/CICP.h>
25 
26 #include <fstream>
27 #include <iostream>
28 
29 using namespace mrpt;
30 using namespace mrpt::slam;
31 using namespace mrpt::maps;
32 using namespace mrpt::obs;
33 using namespace mrpt::math;
34 using namespace mrpt::poses;
35 using namespace std;
36 
37 bool skip_window = false;
38 int ICP_method = (int)icpClassic;
39 
40 // ------------------------------------------------------
41 // TestICP
42 // ------------------------------------------------------
43 void TestICP()
44 {
45  CSimplePointsMap m1, m2;
46  CICP::TReturnInfo info;
47  CICP ICP;
48 
49  // Load scans:
52 
55 
56  // Build the points maps from the scans:
57  m1.insertObservation(scan1);
58  m2.insertObservation(scan2);
59 
60  // -----------------------------------------------------
61 
62  /**
63  * user-set parameters for the icp algorithm.
64  * For a full list of the available options check the online tutorials
65  * page:
66  * https://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms
67  */
68 
69  // select which algorithm version to use
70  // ICP.options.ICP_algorithm = icpLevenbergMarquardt;
71  // ICP.options.ICP_algorithm = icpClassic;
73 
74  // configuration options for the icp algorithm
75  ICP.options.maxIterations = 100;
76  ICP.options.thresholdAng = DEG2RAD(10.0f);
77  ICP.options.thresholdDist = 0.75f;
78  ICP.options.ALFA = 0.5f;
79  ICP.options.smallestThresholdDist = 0.05f;
80  ICP.options.doRANSAC = false;
81 
82  ICP.options.dumpToConsole();
83  // -----------------------------------------------------
84 
85  /**
86  * Scans alignment procedure.
87  * Given an initial guess (initialPose) and the maps to be aligned, the
88  * algorithm returns the probability density function (pdf) of the alignment
89  * Additional arguments are provided to investigate the performance of the
90  * algorithm
91  */
92  CPose2D initialPose(0.8f, 0.0f, (float)DEG2RAD(0.0f));
93 
94  CPosePDF::Ptr pdf = ICP.Align(&m1, &m2, initialPose, info);
95 
96  printf(
97  "ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n "
98  "-> ",
99  info.executionTime * 1000, info.nIterations,
100  info.executionTime * 1000.0f / info.nIterations, info.goodness * 100);
101 
102  cout << "Mean of estimation: " << pdf->getMeanVal() << endl << endl;
103 
104  CPosePDFGaussian gPdf;
105  gPdf.copyFrom(*pdf);
106 
107  cout << "Covariance of estimation: " << endl << gPdf.cov << endl;
108 
109  cout << " std(x): " << sqrt(gPdf.cov(0, 0)) << endl;
110  cout << " std(y): " << sqrt(gPdf.cov(1, 1)) << endl;
111  cout << " std(phi): " << RAD2DEG(sqrt(gPdf.cov(2, 2))) << " (deg)" << endl;
112 
113  // cout << "Covariance of estimation (MATLAB format): " << endl <<
114  // gPdf.cov.inMatlabFormat() << endl;
115 
116  /**
117  * Save the results for potential postprocessing (in txt and in matlab
118  * format)
119  */
120  cout << "-> Saving reference map as scan1.txt" << endl;
121  m1.save2D_to_text_file("scan1.txt");
122 
123  cout << "-> Saving map to align as scan2.txt" << endl;
124  m2.save2D_to_text_file("scan2.txt");
125 
126  cout << "-> Saving transformed map to align as scan2_trans.txt" << endl;
127  CSimplePointsMap m2_trans = m2;
128  m2_trans.changeCoordinatesReference(gPdf.mean);
129  m2_trans.save2D_to_text_file("scan2_trans.txt");
130 
131  cout << "-> Saving MATLAB script for drawing 2D ellipsoid as view_ellip.m"
132  << endl;
134  COV22.setSize(2, 2);
135  CVectorFloat MEAN2D(2);
136  MEAN2D[0] = gPdf.mean.x();
137  MEAN2D[1] = gPdf.mean.y();
138  {
139  ofstream f("view_ellip.m");
140  f << math::MATLAB_plotCovariance2D(COV22, MEAN2D, 3.0f);
141  }
142 
143 // If we have 2D windows, use'em:
144 #if MRPT_HAS_WXWIDGETS
145  /**
146  * Plotting the icp results:
147  * Aligned maps + transformation uncertainty ellipsis
148  */
149  if (!skip_window)
150  {
151  gui::CDisplayWindowPlots win("ICP results");
152 
153  // Reference map:
154  vector<float> map1_xs, map1_ys, map1_zs;
155  m1.getAllPoints(map1_xs, map1_ys, map1_zs);
156  win.plot(map1_xs, map1_ys, "b.3", "map1");
157 
158  // Translated map:
159  vector<float> map2_xs, map2_ys, map2_zs;
160  m2_trans.getAllPoints(map2_xs, map2_ys, map2_zs);
161  win.plot(map2_xs, map2_ys, "r.3", "map2");
162 
163  // Uncertainty
164  win.plotEllipse(MEAN2D[0], MEAN2D[1], COV22, 3.0, "b2", "cov");
165 
166  win.axis(-1, 10, -6, 6);
167  win.axis_equal();
168 
169  cout << "Close the window to exit" << endl;
170  win.waitForKey();
171  }
172 #endif
173 }
174 
175 int main(int argc, char** argv)
176 {
177  try
178  {
179  skip_window = (argc > 2);
180  if (argc > 1)
181  {
182  ICP_method = atoi(argv[1]);
183  }
184 
185  TestICP();
186 
187  return 0;
188  }
189  catch (exception& e)
190  {
191  cout << "MRPT exception caught: " << e.what() << endl;
192  return -1;
193  }
194  catch (...)
195  {
196  printf("Another exception!!");
197  return -1;
198  }
199 }
mrpt::config::CLoadableOptions::dumpToConsole
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
Definition: CLoadableOptions.cpp:43
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::TReturnInfo::nIterations
unsigned int nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
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::TReturnInfo::goodness
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
Definition: CICP.h:200
mrpt::slam::CICP::TConfigParams::thresholdDist
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:114
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::math::MATLAB_plotCovariance2D
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, float stdCount, const std::string &style=std::string("b"), size_t nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:356
mrpt::math::CMatrixDynamic::setSize
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
Definition: CMatrixDynamic.h:352
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::slam::TMetricMapAlignmentResult::executionTime
double executionTime
Definition: CMetricMapsAlignmentAlgorithm.h:31
mrpt::maps::CPointsMap::save2D_to_text_file
bool save2D_to_text_file(const std::string &file) const
Save to a text file.
Definition: CPointsMap.cpp:64
mrpt::poses::CPosePDFGaussian::copyFrom
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPosePDFGaussian.cpp:124
mrpt::poses::CPosePDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPosePDFGaussian.h:46
CICP.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::maps::CPointsMap::getAllPoints
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
Definition: CPointsMap.h:590
mrpt::slam::TICPAlgorithm
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options.
Definition: CICP.h:19
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
CPose2D.h
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
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:44
stock_observations.h
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::math::CMatrixDouble
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixDynamic.h:589
CPosePDF.h
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
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::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
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
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
mrpt::slam::CICP::TConfigParams::thresholdAng
double thresholdAng
Definition: CICP.h:114
ICP_method
int ICP_method
Definition: vision_stereo_rectify/test.cpp:38
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
utils.h
mrpt::math::CVectorDynamic
Template for column vectors of dynamic size, compatible with Eigen.
Definition: CVectorDynamic.h:31
mrpt::gui::CDisplayWindowPlots
Create a GUI window and display plots with MATLAB-like interfaces and commands.
Definition: CDisplayWindowPlots.h:33
mrpt::math::CMatrixFloat
CMatrixDynamic< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).
Definition: CMatrixDynamic.h:583
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
TestICP
void TestICP()
Definition: vision_stereo_rectify/test.cpp:43
CPosePDFGaussian.h
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
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
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
gui.h
mrpt::maps
Definition: CBeacon.h:21
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
CSimplePointsMap.h
skip_window
bool skip_window
Definition: vision_stereo_rectify/test.cpp:37
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:41
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::math::CMatrixDynamic
This template class provides the basic functionality for a general 2D any-size, resizable container o...
Definition: CMatrixDynamic.h:39
CObservation2DRangeScan.h
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 Sat Jun 27 14:00:59 UTC 2020