26 #include <Eigen/Dense> 47 if (runningTime) tictac.Tic();
49 switch (options.ICP_algorithm)
53 ICP_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
56 resultPDF = ICP_Method_LM(m1, mm2, initialEstimationPDF, outInfo);
60 "Invalid value for ICP_algorithm: %i",
61 static_cast<int>(options.ICP_algorithm));
64 if (runningTime) *runningTime = tictac.Tac();
67 if (info) *
static_cast<TReturnInfo*
>(info) = outInfo;
85 section,
"ICP_algorithm", ICP_algorithm);
87 section,
"ICP_covariance_method", ICP_covariance_method);
91 section.c_str(),
"thresholdAng_DEG",
RAD2DEG(thresholdAng)));
108 section.c_str(),
"ransac_fuseMaxDiffPhi_DEG",
109 RAD2DEG(ransac_fuseMaxDiffPhi)));
120 corresponding_points_decimation,
int,
iniFile, section);
127 ICP_algorithm,
"The ICP algorithm to use (see enum TICPAlgorithm)");
129 ICP_covariance_method,
130 "Method to use for covariance estimation (see enum " 131 "TICPCovarianceMethod)");
134 "Only the closest correspondence for each reference point will be " 141 "Initial threshold distance for two points to be a match");
144 "Initial threshold distance for two points to be a match");
147 "The scale factor for thresholds everytime convergence is achieved");
149 smallestThresholdDist,
150 "The size for threshold such that iterations will stop, " 151 "since it is considered precise enough.");
173 return options.use_kernel ? (x2 / (x2 + rho2)) : x2;
192 size_t nCorrespondences = 0;
193 bool keepApproaching;
203 ASSERT_(options.ALFA >= 0 && options.ALFA < 1);
213 gaussPdf = std::make_shared<CPosePDFGaussian>();
216 gaussPdf->mean = grossEst;
223 options.thresholdDist;
225 options.thresholdAng;
230 options.corresponding_points_decimation;
247 gaussPdf->mean.x(), gaussPdf->mean.y(),
253 correspondences, matchParams, matchExtraResults);
255 nCorrespondences = correspondences.size();
260 if (!nCorrespondences)
263 keepApproaching =
false;
277 keepApproaching =
true;
278 if (!(fabs(lastMeanPose.
x() - gaussPdf->mean.x()) >
279 options.minAbsStep_trans ||
280 fabs(lastMeanPose.
y() - gaussPdf->mean.y()) >
281 options.minAbsStep_trans ||
283 lastMeanPose.
phi() - gaussPdf->mean.phi())) >
284 options.minAbsStep_rot))
289 options.smallestThresholdDist)
290 keepApproaching =
false;
293 options.corresponding_points_decimation)
297 lastMeanPose = gaussPdf->mean;
304 if (outInfo.
nIterations >= options.maxIterations &&
306 options.smallestThresholdDist)
312 (keepApproaching && outInfo.
nIterations < options.maxIterations) ||
315 options.smallestThresholdDist));
320 if (!options.skip_cov_calculation && nCorrespondences)
322 switch (options.ICP_covariance_method)
330 gaussPdf->cov *= options.covariance_varPoints;
343 const TPose2D transf = gaussPdf->mean.asTPose();
345 double ccos = cos(transf.
phi);
346 double csin = sin(transf.
phi);
354 double rho2 =
square(options.kernel_rho);
355 mrpt::tfest::TMatchingPairList::iterator it;
357 for (i = 0, it = correspondences.begin();
358 i < nCorrespondences; ++i, ++it)
360 float other_x_trans =
361 transf.
x + ccos * it->other_x - csin * it->other_y;
362 float other_y_trans =
363 transf.
y + csin * it->other_x + ccos * it->other_y;
367 w1 = other_x_trans - Axy;
370 square(it->this_y - other_y_trans),
376 square(it->this_y - other_y_trans),
379 w3 = other_x_trans + Axy;
382 square(it->this_y - other_y_trans),
386 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
387 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
388 B = ((q1 - q2) + (
A * ((
w2 *
w2) - (
w1 *
w1)))) /
391 D(0, i) = (2 *
A * other_x_trans) + B;
395 w1 = other_y_trans - Axy;
397 square(it->this_x - other_x_trans) +
403 square(it->this_x - other_x_trans) +
407 w3 = other_y_trans + Axy;
409 square(it->this_x - other_x_trans) +
414 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
415 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
416 B = ((q1 - q2) + (
A * ((
w2 *
w2) - (
w1 *
w1)))) /
419 D(1, i) = (2 *
A * other_y_trans) + B;
425 (-csin * it->other_x - ccos * it->other_y) +
426 D(1, i) * (ccos * it->other_x - csin * it->other_y);
434 for (i = 0; i < 3; i++)
444 "Invalid value for ICP_covariance_method: %i",
445 static_cast<int>(options.ICP_covariance_method));
451 if (!nCorrespondences || options.skip_quality_calculation)
473 TPoint3D(gaussPdf->mean.x(), gaussPdf->mean.y(), 0);
477 correspondences, matchParams, matchExtraResults);
483 correspondences, matchParams, matchExtraResults);
489 correspondences, matchParams, matchExtraResults);
495 correspondences, matchParams, matchExtraResults);
500 correspondences, matchParams, matchExtraResults);
504 -max(EX1 - E0, max(EX2 - E0, max(EY1 - E0, EY2 - E0))) /
514 if (options.doRANSAC)
517 params.ransac_minSetSize = options.ransac_minSetSize;
518 params.ransac_maxSetSize = options.ransac_maxSetSize;
519 params.ransac_mahalanobisDistanceThreshold =
520 options.ransac_mahalanobisDistanceThreshold;
521 params.ransac_nSimulations = options.ransac_nSimulations;
522 params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
523 params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
524 params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
525 params.ransac_algorithmForLandmarks =
false;
531 SOG = std::make_shared<CPosePDFSOG>();
540 resultPDF = gaussPdf;
560 size_t nCorrespondences = 0;
561 bool keepIteratingICP;
565 std::vector<float> other_xs_trans, other_ys_trans;
572 const bool onlyUniqueRobust = options.onlyUniqueRobust;
576 const auto* m1 =
static_cast<const CPointsMap*
>(mm1);
580 ASSERT_(options.ALFA > 0 && options.ALFA < 1);
591 options.thresholdDist;
593 options.thresholdAng;
599 options.corresponding_points_decimation;
624 m1->determineMatching2D(
627 correspondences, matchParams, matchExtraResults);
629 nCorrespondences = correspondences.size();
631 if (!nCorrespondences)
634 keepIteratingICP =
false;
641 dJ_dq.
setSize(3, nCorrespondences);
645 double lambda = options.LM_initial_lambda;
647 double ccos = cos(
q.phi());
648 double csin = sin(
q.phi());
654 options.Axy_aprox_derivatives;
659 std::vector<float> sq_errors;
661 q, sq_errors, other_xs_trans, other_ys_trans);
662 double OSE_initial =
math::sum(sq_errors);
666 double rho2 =
square(options.kernel_rho);
667 mrpt::tfest::TMatchingPairList::iterator it;
668 std::vector<float>::const_iterator other_x_trans, other_y_trans;
671 for (i = 0, it = correspondences.begin(),
672 other_x_trans = other_xs_trans.begin(),
673 other_y_trans = other_ys_trans.begin();
674 i < nCorrespondences;
675 ++i, ++it, ++other_x_trans, ++other_y_trans)
681 #ifndef ICP_DISTANCES_TO_LINE 682 w1 = *other_x_trans - Axy;
683 q1 = m1->squareDistanceToClosestCorrespondence(
685 q1 = kernel(q1, rho2);
688 q2 = m1->squareDistanceToClosestCorrespondence(
690 q2 = kernel(q2, rho2);
692 w3 = *other_x_trans + Axy;
693 q3 = m1->squareDistanceToClosestCorrespondence(
695 q3 = kernel(q3, rho2);
699 float x1, y1, x2, y2, d1, d2;
700 m1->kdTreeTwoClosestPoint2D(
701 *other_x_trans, *other_y_trans,
706 w1 = *other_x_trans - Axy;
708 w1, *other_y_trans, x1, y1, x2, y2);
709 q1 = kernel(q1, rho2);
713 w2, *other_y_trans, x1, y1, x2, y2);
714 q2 = kernel(q2, rho2);
716 w3 = *other_x_trans + Axy;
718 w3, *other_y_trans, x1, y1, x2, y2);
719 q3 = kernel(q3, rho2);
722 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
723 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
726 dJ_dq(0, i) = (2 *
A * *other_x_trans) + B;
730 w1 = *other_y_trans - Axy;
731 #ifdef ICP_DISTANCES_TO_LINE 733 *other_x_trans,
w1, x1, y1, x2, y2);
734 q1 = kernel(q1, rho2);
737 square(it->this_x - *other_x_trans) +
749 w3 = *other_y_trans + Axy;
750 #ifdef ICP_DISTANCES_TO_LINE 752 *other_x_trans, w3, x1, y1, x2, y2);
753 q3 = kernel(q3, rho2);
756 square(it->this_x - *other_x_trans) +
762 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
763 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
766 dJ_dq(1, i) = (2 *
A * *other_y_trans) + B;
772 (-csin * it->other_x - ccos * it->other_y) +
773 dJ_dq(1, i) * (ccos * it->other_x - csin * it->other_y);
786 bool keepIteratingLM =
true;
793 std::vector<float> new_sq_errors, new_other_xs_trans,
796 const size_t maxLMiters = 100;
798 while (keepIteratingLM && ++nLMiters < maxLMiters)
804 for (i = 0; i < 3; i++)
811 const Eigen::Vector3f LM_delta =
814 &sq_errors[0], sq_errors.size()))
817 q_new.
x(
q.x() - LM_delta[0]);
818 q_new.
y(
q.y() - LM_delta[1]);
819 q_new.
phi(
q.phi() - LM_delta[2]);
824 q_new, new_sq_errors, new_other_xs_trans,
827 float OSE_new =
math::sum(new_sq_errors);
829 bool improved = OSE_new < OSE_initial;
832 cout <<
"_____________" << endl;
833 cout <<
"q -> q_new : " <<
q <<
" -> " << q_new << endl;
834 printf(
"err: %f -> %f lambda: %e\n", OSE_initial ,OSE_new, lambda );
840 fabs(LM_delta[0]) > options.minAbsStep_trans ||
841 fabs(LM_delta[1]) > options.minAbsStep_trans ||
842 fabs(LM_delta[2]) > options.minAbsStep_rot;
850 OSE_initial = OSE_new;
861 cout <<
"ICP loop: " << lastMeanPose <<
" -> " <<
q <<
" LM iters: " << nLMiters <<
" threshold: " << matchParams.
maxDistForCorrespondence << endl;
866 keepIteratingICP =
true;
867 if (fabs(lastMeanPose.
x() -
q.x()) < options.minAbsStep_trans &&
868 fabs(lastMeanPose.
y() -
q.y()) < options.minAbsStep_trans &&
870 options.minAbsStep_rot)
875 options.smallestThresholdDist)
876 keepIteratingICP =
false;
879 options.corresponding_points_decimation)
888 if (outInfo.
nIterations >= options.maxIterations &&
890 options.smallestThresholdDist)
896 (keepIteratingICP && outInfo.
nIterations < options.maxIterations) ||
899 options.smallestThresholdDist));
910 return CPosePDFGaussian::Create(
q, C_inv.
cast_double());
925 if (runningTime) tictac.Tic();
927 switch (options.ICP_algorithm)
931 ICP3D_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
938 "Invalid value for ICP_algorithm: %i",
939 static_cast<int>(options.ICP_algorithm));
942 if (runningTime) *runningTime = tictac.Tac();
948 "Refactor `info` so it is polymorphic and can use dynamic_cast<> " 968 size_t nCorrespondences = 0;
969 bool keepApproaching;
980 ASSERT_(options.ALFA > 0 && options.ALFA < 1);
990 gaussPdf = std::make_shared<CPose3DPDFGaussian>();
993 gaussPdf->mean = grossEst;
1000 options.thresholdDist;
1002 options.thresholdAng;
1006 options.corresponding_points_decimation;
1020 gaussPdf->mean.x(), gaussPdf->mean.y(), gaussPdf->mean.z());
1028 correspondences, matchParams, matchExtraResults);
1030 nCorrespondences = correspondences.size();
1032 if (!nCorrespondences)
1035 keepApproaching =
false;
1042 double transf_scale;
1044 correspondences, estPoseQuat, transf_scale,
1050 keepApproaching =
true;
1051 if (!(fabs(lastMeanPose.
x() - gaussPdf->mean.x()) >
1052 options.minAbsStep_trans ||
1053 fabs(lastMeanPose.
y() - gaussPdf->mean.y()) >
1054 options.minAbsStep_trans ||
1055 fabs(lastMeanPose.z() - gaussPdf->mean.z()) >
1056 options.minAbsStep_trans ||
1058 lastMeanPose.
yaw() - gaussPdf->mean.yaw())) >
1059 options.minAbsStep_rot ||
1061 lastMeanPose.
pitch() - gaussPdf->mean.pitch())) >
1062 options.minAbsStep_rot ||
1064 lastMeanPose.
roll() - gaussPdf->mean.roll())) >
1065 options.minAbsStep_rot))
1070 options.smallestThresholdDist)
1071 keepApproaching =
false;
1074 options.corresponding_points_decimation)
1078 lastMeanPose = gaussPdf->mean;
1085 if (outInfo.
nIterations >= options.maxIterations &&
1087 options.smallestThresholdDist)
1093 (keepApproaching && outInfo.
nIterations < options.maxIterations) ||
1096 options.smallestThresholdDist));
1101 if (!options.skip_cov_calculation && nCorrespondences)
1112 resultPDF = gaussPdf;
std::ostream & operator<<(std::ostream &o, const TFTDIDevice &d)
Print out all the information of a FTDI device in textual form.
Use the covariance of the optimal registration, disregarding uncertainty in data association.
mrpt::math::TPoint3D angularDistPivotPoint
The point used to calculate angular distances: e.g.
float maxAngularDistForCorrespondence
Allowed "angular error" (in radians): this permits larger pairing threshold distances to more distant...
float quality
A measure of the 'quality' of the local minimum of the sqr.
CPose2D mean
The mean value.
CPose3D mean
The mean value.
const float normalizationStd
double RAD2DEG(const double x)
Radians to degrees.
float kernel(const float &x2, const float &rho2)
Computes: or just return the input if options.useKernel = false.
#define THROW_EXCEPTION(msg)
size_t decimation_other_map_points
(Default=1) Only consider 1 out of this number of points from the "other" map.
double closestSquareDistanceFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2)
Returns the square distance from a point to a line.
bool se2_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=nullptr)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames...
Parameters for se2_l2_robust().
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
This file implements several operations that operate element-wise on individual or pairs of container...
mrpt::poses::CPosePDF::Ptr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr) override
An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/...
A high-performance stopwatch, with typical resolution of nanoseconds.
const float ransac_mahalanobisDistanceThreshold
double pitch() const
Get the PITCH angle (in radians)
double yaw() const
Get the YAW angle (in radians)
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::poses::CPose3DPDF::Ptr ICP3D_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
void matProductOf_AAt(const MAT_A &A)
this = A * AT
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams ¶ms, TMatchingExtraResults &extraResults) const
Computes the matching between this and another 2D point map, which includes finding: ...
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
bool se2_l2_robust(const mrpt::tfest::TMatchingPairList &in_correspondences, const double in_normalizationStd, const TSE2RobustParams &in_ransac_params, TSE2RobustResult &out_results)
Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference ...
map< string, CVectorDouble > results
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
string iniFile(myDataDir+string("benchmark-options.ini"))
bool onlyUniqueRobust
Additional consistency filter: "onlyKeepTheClosest" allows one correspondence for each "local map" po...
float maxDistForCorrespondence
Maximum linear distance between two points to be paired (meters)
void squareErrorVector(const mrpt::poses::CPose2D &q, std::vector< float > &out_sqErrs) const
Returns a vector with the square error between each pair of correspondences in the list...
double x() const
Common members of all points & poses classes.
void y_incr(const double v)
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
mrpt::poses::CPosePDF::Ptr ICP_Method_LM(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
unsigned short nIterations
The number of executed iterations until convergence.
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp.
void pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
bool onlyKeepTheClosest
If set to true (default), only the closest correspondence will be returned.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
void x_incr(const double v)
size_t offset_other_map_points
Index of the first point in the "other" map to start checking for correspondences (Default=0) ...
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
const double & phi() const
Get the phi angle of the 2D pose (in radians)
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
The ICP algorithm return information.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
bool se3_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Parameters for the determination of matchings between point clouds, etc.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
CMatrixFixed< double, ROWS, COLS > cast_double() const
Output placeholder for se2_l2_robust()
double phi
Orientation (rads)
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams ¶ms, TMatchingExtraResults &extraResults) const
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
mrpt::poses::CPosePDF::Ptr ICP_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Covariance of a least-squares optimizer (includes data association uncertainty)
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr) override
The virtual method for aligning a pair of metric maps, aligning the full 6D pose. ...
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1