45 if (runningTime) tictac.Tic();
47 switch (options.ICP_algorithm)
51 ICP_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
54 resultPDF = ICP_Method_LM(m1, mm2, initialEstimationPDF, outInfo);
58 "Invalid value for ICP_algorithm: %i",
59 static_cast<int>(options.ICP_algorithm));
62 if (runningTime) *runningTime = tictac.Tac();
65 if (info) *
static_cast<TReturnInfo*
>(info) = outInfo;
83 section,
"ICP_algorithm", ICP_algorithm);
85 section,
"ICP_covariance_method", ICP_covariance_method);
90 section.c_str(),
"thresholdAng_DEG",
RAD2DEG(thresholdAng)));
106 ransac_fuseMaxDiffPhi =
DEG2RAD(
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 = mrpt::make_aligned_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 ||
284 lastMeanPose.
phi() - gaussPdf->mean.phi())) >
285 options.minAbsStep_rot))
290 options.smallestThresholdDist)
291 keepApproaching =
false;
294 options.corresponding_points_decimation)
298 lastMeanPose = gaussPdf->mean;
305 if (outInfo.
nIterations >= options.maxIterations &&
307 options.smallestThresholdDist)
313 (keepApproaching && outInfo.
nIterations < options.maxIterations) ||
316 options.smallestThresholdDist));
321 if (!options.skip_cov_calculation && nCorrespondences)
323 switch (options.ICP_covariance_method)
331 gaussPdf->cov *= options.covariance_varPoints;
342 Eigen::Matrix<double, 3, Eigen::Dynamic> D(
343 3, nCorrespondences);
345 const TPose2D transf = gaussPdf->mean.asTPose();
347 double ccos = cos(transf.
phi);
348 double csin = sin(transf.
phi);
356 double rho2 =
square(options.kernel_rho);
359 for (i = 0, it = correspondences.begin();
360 i < nCorrespondences; ++i, ++it)
362 float other_x_trans =
363 transf.
x + ccos * it->other_x - csin * it->other_y;
364 float other_y_trans =
365 transf.
y + csin * it->other_x + ccos * it->other_y;
369 w1 = other_x_trans - Axy;
372 square(it->this_y - other_y_trans),
378 square(it->this_y - other_y_trans),
381 w3 = other_x_trans + Axy;
384 square(it->this_y - other_y_trans),
388 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
389 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
390 B = ((q1 - q2) + (A * ((
w2 *
w2) - (
w1 *
w1)))) /
393 D(0, i) = (2 * A * other_x_trans) + B;
397 w1 = other_y_trans - Axy;
399 square(it->this_x - other_x_trans) +
405 square(it->this_x - other_x_trans) +
409 w3 = other_y_trans + Axy;
411 square(it->this_x - other_x_trans) +
416 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
417 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
418 B = ((q1 - q2) + (A * ((
w2 *
w2) - (
w1 *
w1)))) /
421 D(1, i) = (2 * A * other_y_trans) + B;
427 (-csin * it->other_x - ccos * it->other_y) +
428 D(1, i) * (ccos * it->other_x - csin * it->other_y);
435 for (i = 0; i < 3; i++)
440 DDt.inv(gaussPdf->cov);
445 "Invalid value for ICP_covariance_method: %i",
446 static_cast<int>(options.ICP_covariance_method));
452 if (!nCorrespondences || options.skip_quality_calculation)
474 TPoint3D(gaussPdf->mean.x(), gaussPdf->mean.y(), 0);
478 correspondences, matchParams, matchExtraResults);
484 correspondences, matchParams, matchExtraResults);
490 correspondences, matchParams, matchExtraResults);
496 correspondences, matchParams, matchExtraResults);
501 correspondences, matchParams, matchExtraResults);
505 -max(EX1 - E0, max(EX2 - E0, max(EY1 - E0, EY2 - E0))) /
515 if (options.doRANSAC)
518 params.ransac_minSetSize = options.ransac_minSetSize;
519 params.ransac_maxSetSize = options.ransac_maxSetSize;
520 params.ransac_mahalanobisDistanceThreshold =
521 options.ransac_mahalanobisDistanceThreshold;
522 params.ransac_nSimulations = options.ransac_nSimulations;
523 params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
524 params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
525 params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
526 params.ransac_algorithmForLandmarks =
false;
532 SOG = mrpt::make_aligned_shared<CPosePDFSOG>();
541 resultPDF = gaussPdf;
561 size_t nCorrespondences = 0;
562 bool keepIteratingICP;
566 std::vector<float> other_xs_trans, other_ys_trans;
573 const bool onlyUniqueRobust = options.onlyUniqueRobust;
581 ASSERT_(options.ALFA > 0 && options.ALFA < 1);
592 options.thresholdDist;
594 options.thresholdAng;
600 options.corresponding_points_decimation;
625 m1->determineMatching2D(
628 correspondences, matchParams, matchExtraResults);
630 nCorrespondences = correspondences.size();
632 if (!nCorrespondences)
635 keepIteratingICP =
false;
642 dJ_dq.setSize(3, nCorrespondences);
646 double lambda = options.LM_initial_lambda;
648 double ccos = cos(
q.phi());
649 double csin = sin(
q.phi());
655 options.Axy_aprox_derivatives;
660 std::vector<float> sq_errors;
662 q, sq_errors, other_xs_trans, other_ys_trans);
663 double OSE_initial =
math::sum(sq_errors);
667 double rho2 =
square(options.kernel_rho);
672 for (i = 0, it = correspondences.begin(),
673 other_x_trans = other_xs_trans.begin(),
674 other_y_trans = other_ys_trans.begin();
675 i < nCorrespondences;
676 ++i, ++it, ++other_x_trans, ++other_y_trans)
682 #ifndef ICP_DISTANCES_TO_LINE 683 w1 = *other_x_trans - Axy;
684 q1 = m1->squareDistanceToClosestCorrespondence(
686 q1 = kernel(q1, rho2);
689 q2 = m1->squareDistanceToClosestCorrespondence(
691 q2 = kernel(q2, rho2);
693 w3 = *other_x_trans + Axy;
694 q3 = m1->squareDistanceToClosestCorrespondence(
696 q3 = kernel(q3, rho2);
700 float x1, y1, x2, y2, d1, d2;
701 m1->kdTreeTwoClosestPoint2D(
702 *other_x_trans, *other_y_trans,
707 w1 = *other_x_trans - Axy;
709 w1, *other_y_trans, x1, y1, x2, y2);
710 q1 = kernel(q1, rho2);
714 w2, *other_y_trans, x1, y1, x2, y2);
715 q2 = kernel(q2, rho2);
717 w3 = *other_x_trans + Axy;
719 w3, *other_y_trans, x1, y1, x2, y2);
720 q3 = kernel(q3, rho2);
723 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
724 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
725 B = ((q1 - q2) + (A * ((
w2 *
w2) - (
w1 *
w1)))) / (
w1 -
w2);
727 dJ_dq.get_unsafe(0, i) = (2 * A * *other_x_trans) + B;
731 w1 = *other_y_trans - Axy;
732 #ifdef ICP_DISTANCES_TO_LINE 734 *other_x_trans,
w1, x1, y1, x2, y2);
735 q1 = kernel(q1, rho2);
738 square(it->this_x - *other_x_trans) +
750 w3 = *other_y_trans + Axy;
751 #ifdef ICP_DISTANCES_TO_LINE 753 *other_x_trans, w3, x1, y1, x2, y2);
754 q3 = kernel(q3, rho2);
757 square(it->this_x - *other_x_trans) +
763 A = ((q3 - q2) / ((w3 -
w2) * (w3 -
w1))) -
764 ((q1 - q2) / ((
w1 -
w2) * (w3 -
w1)));
765 B = ((q1 - q2) + (A * ((
w2 *
w2) - (
w1 *
w1)))) / (
w1 -
w2);
767 dJ_dq.get_unsafe(1, i) = (2 * A * *other_y_trans) + B;
771 dJ_dq.get_unsafe(2, i) =
772 dJ_dq.get_unsafe(0, i) *
773 (-csin * it->other_x - ccos * it->other_y) +
774 dJ_dq.get_unsafe(1, i) *
775 (ccos * it->other_x - csin * it->other_y);
783 H_.multiply_AAt(dJ_dq);
788 bool keepIteratingLM =
true;
795 std::vector<float> new_sq_errors, new_other_xs_trans,
798 const size_t maxLMiters = 100;
800 while (keepIteratingLM && ++nLMiters < maxLMiters)
806 for (i = 0; i < 3; i++)
813 Eigen::VectorXf dJsq, LM_delta;
815 Eigen::Map<Eigen::VectorXf>(
816 &sq_errors[0], sq_errors.size()),
818 C_inv.multiply_Ab(dJsq, LM_delta);
820 q_new.
x(
q.x() - LM_delta[0]);
821 q_new.
y(
q.y() - LM_delta[1]);
822 q_new.
phi(
q.phi() - LM_delta[2]);
827 q_new, new_sq_errors, new_other_xs_trans,
830 float OSE_new =
math::sum(new_sq_errors);
832 bool improved = OSE_new < OSE_initial;
835 cout <<
"_____________" << endl;
836 cout <<
"q -> q_new : " <<
q <<
" -> " << q_new << endl;
837 printf(
"err: %f -> %f lambda: %e\n", OSE_initial ,OSE_new, lambda );
843 fabs(LM_delta[0]) > options.minAbsStep_trans ||
844 fabs(LM_delta[1]) > options.minAbsStep_trans ||
845 fabs(LM_delta[2]) > options.minAbsStep_rot;
853 OSE_initial = OSE_new;
864 cout <<
"ICP loop: " << lastMeanPose <<
" -> " <<
q <<
" LM iters: " << nLMiters <<
" threshold: " << matchParams.
maxDistForCorrespondence << endl;
869 keepIteratingICP =
true;
870 if (fabs(lastMeanPose.
x() -
q.x()) < options.minAbsStep_trans &&
871 fabs(lastMeanPose.
y() -
q.y()) < options.minAbsStep_trans &&
873 options.minAbsStep_rot)
878 options.smallestThresholdDist)
879 keepIteratingICP =
false;
882 options.corresponding_points_decimation)
891 if (outInfo.
nIterations >= options.maxIterations &&
893 options.smallestThresholdDist)
899 (keepIteratingICP && outInfo.
nIterations < options.maxIterations) ||
902 options.smallestThresholdDist));
928 if (runningTime) tictac.Tic();
930 switch (options.ICP_algorithm)
934 ICP3D_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
941 "Invalid value for ICP_algorithm: %i",
942 static_cast<int>(options.ICP_algorithm));
945 if (runningTime) *runningTime = tictac.Tac();
951 "Refactor `info` so it is polymorphic and can use dynamic_cast<> " 971 size_t nCorrespondences = 0;
972 bool keepApproaching;
983 ASSERT_(options.ALFA > 0 && options.ALFA < 1);
993 gaussPdf = mrpt::make_aligned_shared<CPose3DPDFGaussian>();
996 gaussPdf->mean = grossEst;
1003 options.thresholdDist;
1005 options.thresholdAng;
1009 options.corresponding_points_decimation;
1023 gaussPdf->mean.x(), gaussPdf->mean.y(), gaussPdf->mean.z());
1031 correspondences, matchParams, matchExtraResults);
1033 nCorrespondences = correspondences.size();
1035 if (!nCorrespondences)
1038 keepApproaching =
false;
1045 double transf_scale;
1047 correspondences, estPoseQuat, transf_scale,
1053 keepApproaching =
true;
1054 if (!(fabs(lastMeanPose.
x() - gaussPdf->mean.x()) >
1055 options.minAbsStep_trans ||
1056 fabs(lastMeanPose.
y() - gaussPdf->mean.y()) >
1057 options.minAbsStep_trans ||
1058 fabs(lastMeanPose.z() - gaussPdf->mean.z()) >
1059 options.minAbsStep_trans ||
1062 lastMeanPose.
yaw() - gaussPdf->mean.yaw())) >
1063 options.minAbsStep_rot ||
1066 lastMeanPose.
pitch() - gaussPdf->mean.pitch())) >
1067 options.minAbsStep_rot ||
1070 lastMeanPose.
roll() - gaussPdf->mean.roll())) >
1071 options.minAbsStep_rot))
1076 options.smallestThresholdDist)
1077 keepApproaching =
false;
1080 options.corresponding_points_decimation)
1084 lastMeanPose = gaussPdf->mean;
1091 if (outInfo.
nIterations >= options.maxIterations &&
1093 options.smallestThresholdDist)
1099 (keepApproaching && outInfo.
nIterations < options.maxIterations) ||
1102 options.smallestThresholdDist));
1107 if (!options.skip_cov_calculation && nCorrespondences)
1118 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...
double x() const
Common members of all points & poses classes.
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.
void y_incr(const double v)
#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...
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)
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.
void x_incr(const double v)
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...
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)
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
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)
The ICP algorithm return information.
A matrix of dynamic size.
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
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)
An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/...
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.
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
const Scalar * const_iterator
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::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr)
The virtual method for aligning a pair of metric maps, aligning the full 6D pose. ...
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)
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1