Go to the documentation of this file.
97 "ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n "
102 cout <<
"Mean of estimation: " << pdf->getMeanVal() << endl << endl;
107 cout <<
"Covariance of estimation: " << endl << gPdf.
cov << endl;
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;
120 cout <<
"-> Saving reference map as scan1.txt" << endl;
123 cout <<
"-> Saving map to align as scan2.txt" << endl;
126 cout <<
"-> Saving transformed map to align as scan2_trans.txt" << endl;
131 cout <<
"-> Saving MATLAB script for drawing 2D ellipsoid as view_ellip.m"
136 MEAN2D[0] = gPdf.
mean.
x();
137 MEAN2D[1] = gPdf.
mean.
y();
139 ofstream f(
"view_ellip.m");
144 #if MRPT_HAS_WXWIDGETS
154 vector<float> map1_xs, map1_ys, map1_zs;
156 win.plot(map1_xs, map1_ys,
"b.3",
"map1");
159 vector<float> map2_xs, map2_ys, map2_zs;
161 win.plot(map2_xs, map2_ys,
"r.3",
"map2");
164 win.plotEllipse(MEAN2D[0], MEAN2D[1], COV22, 3.0,
"b2",
"cov");
166 win.axis(-1, 10, -6, 6);
169 cout <<
"Close the window to exit" << endl;
191 cout <<
"MRPT exception caught: " << e.what() << endl;
196 printf(
"Another exception!!");
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
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.
unsigned int nIterations
The number of executed iterations until convergence.
TConfigParams options
The options employed by the ICP align.
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough.
double ALFA
The scale factor for thresholds every time convergence is achieved.
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
double thresholdDist
Initial threshold distance for two points to become a correspondence.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
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...
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
double x() const
Common members of all points & poses classes.
bool save2D_to_text_file(const std::string &file) const
Save to a text file.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This namespace contains representation of robot actions and observations.
The ICP algorithm return information.
CPose2D mean
The mean value.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
mrpt::gui::CDisplayWindow3D::Ptr win
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
constexpr double RAD2DEG(const double x)
Radians to degrees.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
constexpr double DEG2RAD(const double x)
Degrees to radians
Template for column vectors of dynamic size, compatible with Eigen.
Create a GUI window and display plots with MATLAB-like interfaces and commands.
CMatrixDynamic< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).
void example2DRangeScan(mrpt::obs::CObservation2DRangeScan &s, int i=0)
Example 2D lidar scans (form a venerable SICK LMS200).
unsigned int maxIterations
Maximum number of iterations to run.
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
This base provides a set of functions for maths stuff.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
std::shared_ptr< CPosePDF > Ptr
This template class provides the basic functionality for a general 2D any-size, resizable container o...
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |