MRPT  2.0.1
CICP.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 "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/config/CConfigFileBase.h> // MRPT_LOAD_*()
13 #include <mrpt/math/TPose2D.h>
15 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/poses/CPose2D.h>
17 #include <mrpt/poses/CPose3DPDF.h>
19 #include <mrpt/poses/CPosePDF.h>
21 #include <mrpt/poses/CPosePDFSOG.h>
23 #include <mrpt/slam/CICP.h>
24 #include <mrpt/system/CTicTac.h>
25 #include <mrpt/tfest.h>
26 #include <Eigen/Dense>
27 
28 using namespace mrpt::slam;
29 using namespace mrpt::maps;
30 using namespace mrpt::math;
31 using namespace mrpt::tfest;
32 using namespace mrpt::system;
33 using namespace mrpt::poses;
34 using namespace std;
35 
37  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
38  const CPosePDFGaussian& initialEstimationPDF,
40 {
42 
43  CTicTac tictac;
44  TReturnInfo outInfoVal;
45  CPosePDF::Ptr resultPDF;
46 
47  if (outInfo) tictac.Tic();
48 
49  switch (options.ICP_algorithm)
50  {
51  case icpClassic:
52  resultPDF =
53  ICP_Method_Classic(m1, mm2, initialEstimationPDF, outInfoVal);
54  break;
56  resultPDF =
57  ICP_Method_LM(m1, mm2, initialEstimationPDF, outInfoVal);
58  break;
59  default:
61  "Invalid value for ICP_algorithm: %i",
62  static_cast<int>(options.ICP_algorithm));
63  } // end switch
64 
65  if (outInfo) outInfoVal.executionTime = tictac.Tac();
66 
67  // Copy the output info if requested:
68  if (outInfo)
69  if (auto* o = dynamic_cast<TReturnInfo*>(&outInfo.value().get()); o)
70  *o = outInfoVal;
71 
72  return resultPDF;
73 
74  MRPT_END
75 }
76 
77 /*---------------------------------------------------------------
78  loadFromConfigFile
79  ---------------------------------------------------------------*/
81  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
82 {
83  MRPT_LOAD_CONFIG_VAR(maxIterations, int, iniFile, section);
84  MRPT_LOAD_CONFIG_VAR(minAbsStep_trans, float, iniFile, section);
85  MRPT_LOAD_CONFIG_VAR(minAbsStep_rot, float, iniFile, section);
86 
87  ICP_algorithm = iniFile.read_enum<TICPAlgorithm>(
88  section, "ICP_algorithm", ICP_algorithm);
89  ICP_covariance_method = iniFile.read_enum<TICPCovarianceMethod>(
90  section, "ICP_covariance_method", ICP_covariance_method);
91 
92  MRPT_LOAD_CONFIG_VAR(thresholdDist, float, iniFile, section);
93  thresholdAng = DEG2RAD(iniFile.read_float(
94  section.c_str(), "thresholdAng_DEG", RAD2DEG(thresholdAng)));
95 
96  MRPT_LOAD_CONFIG_VAR(ALFA, float, iniFile, section);
97  MRPT_LOAD_CONFIG_VAR(smallestThresholdDist, float, iniFile, section);
98  MRPT_LOAD_CONFIG_VAR(onlyUniqueRobust, bool, iniFile, section);
99  MRPT_LOAD_CONFIG_VAR(doRANSAC, bool, iniFile, section);
100  MRPT_LOAD_CONFIG_VAR(covariance_varPoints, float, iniFile, section);
101 
102  MRPT_LOAD_CONFIG_VAR(ransac_minSetSize, int, iniFile, section);
103  MRPT_LOAD_CONFIG_VAR(ransac_maxSetSize, int, iniFile, section);
106  MRPT_LOAD_CONFIG_VAR(ransac_nSimulations, int, iniFile, section);
108  MRPT_LOAD_CONFIG_VAR(ransac_fuseByCorrsMatch, bool, iniFile, section);
109  MRPT_LOAD_CONFIG_VAR(ransac_fuseMaxDiffXY, float, iniFile, section);
110  ransac_fuseMaxDiffPhi = DEG2RAD(iniFile.read_float(
111  section.c_str(), "ransac_fuseMaxDiffPhi_DEG",
112  RAD2DEG(ransac_fuseMaxDiffPhi)));
113 
114  MRPT_LOAD_CONFIG_VAR(kernel_rho, float, iniFile, section);
115  MRPT_LOAD_CONFIG_VAR(use_kernel, bool, iniFile, section);
116  MRPT_LOAD_CONFIG_VAR(Axy_aprox_derivatives, float, iniFile, section);
117  MRPT_LOAD_CONFIG_VAR(LM_initial_lambda, float, iniFile, section);
118 
119  MRPT_LOAD_CONFIG_VAR(skip_cov_calculation, bool, iniFile, section);
120  MRPT_LOAD_CONFIG_VAR(skip_quality_calculation, bool, iniFile, section);
121 
123  corresponding_points_decimation, int, iniFile, section);
124 }
125 
127  mrpt::config::CConfigFileBase& c, const std::string& s) const
128 {
130  ICP_algorithm, "The ICP algorithm to use (see enum TICPAlgorithm)");
132  ICP_covariance_method,
133  "Method to use for covariance estimation (see enum "
134  "TICPCovarianceMethod)");
136  onlyUniqueRobust,
137  "Only the closest correspondence for each reference point will be "
138  "kept");
139  MRPT_SAVE_CONFIG_VAR_COMMENT(maxIterations, "Iterations");
140  MRPT_SAVE_CONFIG_VAR_COMMENT(minAbsStep_trans, "Termination criteria");
141  MRPT_SAVE_CONFIG_VAR_COMMENT(minAbsStep_rot, "Termination criteria");
143  thresholdDist,
144  "Initial threshold distance for two points to be a match");
146  thresholdAng,
147  "Initial threshold distance for two points to be a match");
149  ALFA,
150  "The scale factor for thresholds every time convergence is achieved");
152  smallestThresholdDist,
153  "The size for threshold such that iterations will stop, "
154  "since it is considered precise enough.");
155  MRPT_SAVE_CONFIG_VAR_COMMENT(covariance_varPoints, "");
156  MRPT_SAVE_CONFIG_VAR_COMMENT(doRANSAC, "Perform a RANSAC step");
157  MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_minSetSize, "");
158  MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_maxSetSize, "");
159  MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_nSimulations, "");
162  MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_fuseByCorrsMatch, "");
163  MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_fuseMaxDiffXY, "");
164  MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_fuseMaxDiffPhi, "");
165  MRPT_SAVE_CONFIG_VAR_COMMENT(kernel_rho, "");
166  MRPT_SAVE_CONFIG_VAR_COMMENT(use_kernel, "");
167  MRPT_SAVE_CONFIG_VAR_COMMENT(Axy_aprox_derivatives, "");
168  MRPT_SAVE_CONFIG_VAR_COMMENT(LM_initial_lambda, "");
169  MRPT_SAVE_CONFIG_VAR_COMMENT(skip_cov_calculation, "");
170  MRPT_SAVE_CONFIG_VAR_COMMENT(skip_quality_calculation, "");
171  MRPT_SAVE_CONFIG_VAR_COMMENT(corresponding_points_decimation, "");
172 }
173 
174 float CICP::kernel(float x2, float rho2)
175 {
176  return options.use_kernel ? (x2 / (x2 + rho2)) : x2;
177 }
178 
179 /*----------------------------------------------------------------------------
180 
181  ICP_Method_Classic
182 
183  ----------------------------------------------------------------------------*/
185  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
186  const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
187 {
188  MRPT_START
189 
190  // The result can be either a Gaussian or a SOG:
191  CPosePDF::Ptr resultPDF;
192  CPosePDFGaussian::Ptr gaussPdf;
193  CPosePDFSOG::Ptr SOG;
194 
195  size_t nCorrespondences = 0;
196  bool keepApproaching;
197  CPose2D grossEst = initialEstimationPDF.mean;
198  mrpt::tfest::TMatchingPairList correspondences, old_correspondences;
199  CPose2D lastMeanPose;
200 
201  // Assure the class of the maps:
202  const mrpt::maps::CMetricMap* m2 = mm2;
203 
204  // Asserts:
205  // -----------------
206  ASSERT_(options.ALFA >= 0 && options.ALFA < 1);
207 
208  // The algorithm output auxiliar info:
209  // -------------------------------------------------
210  outInfo.nIterations = 0;
211  outInfo.goodness = 1;
212  outInfo.quality = 0;
213 
214  // The gaussian PDF to estimate:
215  // ------------------------------------------------------
216  gaussPdf = std::make_shared<CPosePDFGaussian>();
217 
218  // First gross approximation:
219  gaussPdf->mean = grossEst;
220 
221  // Initial thresholds:
222  mrpt::maps::TMatchingParams matchParams;
223  mrpt::maps::TMatchingExtraResults matchExtraResults;
224 
225  matchParams.maxDistForCorrespondence =
226  options.thresholdDist; // Distance threshold.
227  matchParams.maxAngularDistForCorrespondence =
228  options.thresholdAng; // Angular threshold.
229  // Option onlyClosestCorrespondences removed in MRPT 2.0.
230  matchParams.onlyKeepTheClosest = true;
231  matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
232  matchParams.decimation_other_map_points =
233  options.corresponding_points_decimation;
234 
235  // Ensure maps are not empty!
236  // ------------------------------------------------------
237  if (!m2->isEmpty())
238  {
239  matchParams.offset_other_map_points = 0;
240 
241  // ------------------------------------------------------
242  // The ICP loop
243  // ------------------------------------------------------
244  do
245  {
246  // ------------------------------------------------------
247  // Find the matching (for a points map)
248  // ------------------------------------------------------
249  matchParams.angularDistPivotPoint = TPoint3D(
250  gaussPdf->mean.x(), gaussPdf->mean.y(),
251  0); // Pivot point for angular measurements
252 
254  m2, // The other map
255  gaussPdf->mean, // The other map pose
256  correspondences, matchParams, matchExtraResults);
257 
258  nCorrespondences = correspondences.size();
259 
260  // ***DEBUG***
261  // correspondences.dumpToFile("debug_correspondences.txt");
262 
263  if (!nCorrespondences)
264  {
265  // Nothing we can do !!
266  keepApproaching = false;
267  }
268  else
269  {
270  // Compute the estimated pose.
271  // (Method from paper of J.Gonzalez, Martinez y Morales)
272  // ----------------------------------------------------------------------
273  mrpt::math::TPose2D est_mean;
274  mrpt::tfest::se2_l2(correspondences, est_mean);
275 
276  gaussPdf->mean = mrpt::poses::CPose2D(est_mean);
277 
278  // If matching has not changed, decrease the thresholds:
279  // --------------------------------------------------------
280  keepApproaching = true;
281  if (!(fabs(lastMeanPose.x() - gaussPdf->mean.x()) >
282  options.minAbsStep_trans ||
283  fabs(lastMeanPose.y() - gaussPdf->mean.y()) >
284  options.minAbsStep_trans ||
285  fabs(math::wrapToPi(
286  lastMeanPose.phi() - gaussPdf->mean.phi())) >
287  options.minAbsStep_rot))
288  {
289  matchParams.maxDistForCorrespondence *= options.ALFA;
290  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
291  if (matchParams.maxDistForCorrespondence <
292  options.smallestThresholdDist)
293  keepApproaching = false;
294 
295  if (++matchParams.offset_other_map_points >=
296  options.corresponding_points_decimation)
297  matchParams.offset_other_map_points = 0;
298  }
299 
300  lastMeanPose = gaussPdf->mean;
301 
302  } // end of "else, there are correspondences"
303 
304  // Next iteration:
305  outInfo.nIterations++;
306 
307  if (outInfo.nIterations >= options.maxIterations &&
308  matchParams.maxDistForCorrespondence >
309  options.smallestThresholdDist)
310  {
311  matchParams.maxDistForCorrespondence *= options.ALFA;
312  }
313 
314  } while (
315  (keepApproaching && outInfo.nIterations < options.maxIterations) ||
316  (outInfo.nIterations >= options.maxIterations &&
317  matchParams.maxDistForCorrespondence >
318  options.smallestThresholdDist));
319 
320  // -------------------------------------------------
321  // Obtain the covariance matrix of the estimation
322  // -------------------------------------------------
323  if (!options.skip_cov_calculation && nCorrespondences)
324  {
325  switch (options.ICP_covariance_method)
326  {
327  // ----------------------------------------------
328  // METHOD: MSE linear estimation
329  // ----------------------------------------------
330  case icpCovLinealMSE:
331  mrpt::tfest::se2_l2(correspondences, *gaussPdf);
332  // Scale covariance:
333  gaussPdf->cov *= options.covariance_varPoints;
334  break;
335 
336  // ----------------------------------------------
337  // METHOD: Method from Oxford MRG's "OXSMV2"
338  //
339  // It is the equivalent covariance resulting
340  // from a Levenberg-Maquardt optimization stage.
341  // ----------------------------------------------
343  {
344  CMatrixDouble D(3, nCorrespondences);
345 
346  const TPose2D transf = gaussPdf->mean.asTPose();
347 
348  double ccos = cos(transf.phi);
349  double csin = sin(transf.phi);
350 
351  double w1, w2, w3;
352  double q1, q2, q3;
353  double A, B;
354  double Axy = 0.01;
355 
356  // Fill out D:
357  double rho2 = square(options.kernel_rho);
358  mrpt::tfest::TMatchingPairList::iterator it;
359  size_t i;
360  for (i = 0, it = correspondences.begin();
361  i < nCorrespondences; ++i, ++it)
362  {
363  float other_x_trans =
364  transf.x + ccos * it->other_x - csin * it->other_y;
365  float other_y_trans =
366  transf.y + csin * it->other_x + ccos * it->other_y;
367 
368  // Jacobian: dR2_dx
369  // --------------------------------------
370  w1 = other_x_trans - Axy;
371  q1 = kernel(
372  square(it->this_x - w1) +
373  square(it->this_y - other_y_trans),
374  rho2);
375 
376  w2 = other_x_trans;
377  q2 = kernel(
378  square(it->this_x - w2) +
379  square(it->this_y - other_y_trans),
380  rho2);
381 
382  w3 = other_x_trans + Axy;
383  q3 = kernel(
384  square(it->this_x - w3) +
385  square(it->this_y - other_y_trans),
386  rho2);
387 
388  // interpolate
389  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
390  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
391  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
392  (w1 - w2);
393 
394  D(0, i) = (2 * A * other_x_trans) + B;
395 
396  // Jacobian: dR2_dy
397  // --------------------------------------
398  w1 = other_y_trans - Axy;
399  q1 = kernel(
400  square(it->this_x - other_x_trans) +
401  square(it->this_y - w1),
402  rho2);
403 
404  w2 = other_y_trans;
405  q2 = kernel(
406  square(it->this_x - other_x_trans) +
407  square(it->this_y - w2),
408  rho2);
409 
410  w3 = other_y_trans + Axy;
411  q3 = kernel(
412  square(it->this_x - other_x_trans) +
413  square(it->this_y - w3),
414  rho2);
415 
416  // interpolate
417  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
418  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
419  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
420  (w1 - w2);
421 
422  D(1, i) = (2 * A * other_y_trans) + B;
423 
424  // Jacobian: dR_dphi
425  // --------------------------------------
426  D(2, i) =
427  D(0, i) *
428  (-csin * it->other_x - ccos * it->other_y) +
429  D(1, i) * (ccos * it->other_x - csin * it->other_y);
430 
431  } // end for each corresp.
432 
433  // COV = ( D*D^T + lamba*I )^-1
434  CMatrixDouble33 DDt;
435  DDt.matProductOf_AAt(D);
436 
437  for (i = 0; i < 3; i++)
438  DDt(i, i) += 1e-6; // Just to make sure the matrix is
439  // not singular, while not changing
440  // its covariance significantly.
441 
442  gaussPdf->cov = DDt.inverse_LLt();
443  }
444  break;
445  default:
447  "Invalid value for ICP_covariance_method: %i",
448  static_cast<int>(options.ICP_covariance_method));
449  }
450  }
451 
452  outInfo.goodness = matchExtraResults.correspondencesRatio;
453 
454  if (!nCorrespondences || options.skip_quality_calculation)
455  {
456  outInfo.quality = matchExtraResults.correspondencesRatio;
457  }
458  else
459  {
460  // Compute a crude estimate of the quality of scan matching at this
461  // local minimum:
462  // -----------------------------------------------------------------------------------
463  float Axy = matchParams.maxDistForCorrespondence * 0.9f;
464 
465  CPose2D P0(gaussPdf->mean);
466  CPose2D PX1(P0);
467  PX1.x_incr(-Axy);
468  CPose2D PX2(P0);
469  PX2.x_incr(+Axy);
470  CPose2D PY1(P0);
471  PY1.y_incr(-Axy);
472  CPose2D PY2(P0);
473  PY2.y_incr(+Axy);
474 
475  matchParams.angularDistPivotPoint =
476  TPoint3D(gaussPdf->mean.x(), gaussPdf->mean.y(), 0);
478  m2, // The other map
479  P0, // The other map pose
480  correspondences, matchParams, matchExtraResults);
481  const float E0 = matchExtraResults.correspondencesRatio;
482 
484  m2, // The other map
485  PX1, // The other map pose
486  correspondences, matchParams, matchExtraResults);
487  const float EX1 = matchExtraResults.correspondencesRatio;
488 
490  m2, // The other map
491  PX2, // The other map pose
492  correspondences, matchParams, matchExtraResults);
493  const float EX2 = matchExtraResults.correspondencesRatio;
494 
496  m2, // The other map
497  PY1, // The other map pose
498  correspondences, matchParams, matchExtraResults);
499  const float EY1 = matchExtraResults.correspondencesRatio;
501  m2, // The other map
502  PY2, // The other map pose
503  correspondences, matchParams, matchExtraResults);
504  const float EY2 = matchExtraResults.correspondencesRatio;
505 
506  outInfo.quality =
507  -max(EX1 - E0, max(EX2 - E0, max(EY1 - E0, EY2 - E0))) /
508  (E0 + 1e-1);
509  }
510 
511  } // end of "if m2 is not empty"
512 
513  // We'll return a CPosePDFGaussian or a CPosePDFSOG if RANSAC is enabled:
514  // -----------------------------------------------------------------------
515 
516  // RANSAC?
517  if (options.doRANSAC)
518  {
520  params.ransac_minSetSize = options.ransac_minSetSize;
521  params.ransac_maxSetSize = options.ransac_maxSetSize;
522  params.ransac_mahalanobisDistanceThreshold =
523  options.ransac_mahalanobisDistanceThreshold;
524  params.ransac_nSimulations = options.ransac_nSimulations;
525  params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
526  params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
527  params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
528  params.ransac_algorithmForLandmarks = false;
529 
532  correspondences, options.normalizationStd, params, results);
533 
534  SOG = std::make_shared<CPosePDFSOG>();
535  *SOG = results.transformation;
536 
537  // And return the SOG:
538  resultPDF = SOG;
539  }
540  else
541  {
542  // Return the gaussian distribution:
543  resultPDF = gaussPdf;
544  }
545 
546  return resultPDF;
547 
548  MRPT_END
549 }
550 
551 /*----------------------------------------------------------------------------
552 
553  ICP_Method_LM
554 
555  ----------------------------------------------------------------------------*/
557  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* m2,
558  const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
559 {
560  MRPT_START
561 
562  // The result can be either a Gaussian or a SOG:
563  size_t nCorrespondences = 0;
564  bool keepIteratingICP;
565  CPose2D grossEst = initialEstimationPDF.mean;
566  TMatchingPairList correspondences, old_correspondences;
567  CPose2D lastMeanPose;
568  std::vector<float> other_xs_trans, other_ys_trans; // temporary container
569  // of "other" map (map2)
570  // transformed by "q"
571  CMatrixFloat dJ_dq; // The jacobian
572  CPose2D q; // The updated 2D transformation estimate
573  CPose2D q_new;
574 
575  const bool onlyUniqueRobust = options.onlyUniqueRobust;
576 
577  // Assure the class of the maps:
579  const auto* m1 = dynamic_cast<const CPointsMap*>(mm1);
580 
581  // Asserts:
582  // -----------------
583  ASSERT_(options.ALFA > 0 && options.ALFA < 1);
584 
585  // The algorithm output auxiliar info:
586  // -------------------------------------------------
587  outInfo.nIterations = 0;
588  outInfo.goodness = 1.0f;
589 
590  TMatchingParams matchParams;
591  TMatchingExtraResults matchExtraResults;
592 
593  matchParams.maxDistForCorrespondence =
594  options.thresholdDist; // Distance threshold
595  matchParams.maxAngularDistForCorrespondence =
596  options.thresholdAng; // Angular threshold
597  matchParams.angularDistPivotPoint =
598  TPoint3D(q.x(), q.y(), 0); // Pivot point for angular measurements
599  matchParams.onlyKeepTheClosest = true;
600  matchParams.onlyUniqueRobust = onlyUniqueRobust;
601  matchParams.decimation_other_map_points =
602  options.corresponding_points_decimation;
603 
604  // The gaussian PDF to estimate:
605  // ------------------------------------------------------
606  // First gross approximation:
607  q = grossEst;
608 
609  // For LM inverse
612  C_inv; // This will keep the cov. matrix at the end
613 
614  // Ensure maps are not empty!
615  // ------------------------------------------------------
616  if (!m2->isEmpty())
617  {
618  matchParams.offset_other_map_points = 0;
619  // ------------------------------------------------------
620  // The ICP loop
621  // ------------------------------------------------------
622  do
623  {
624  // ------------------------------------------------------
625  // Find the matching (for a points map)
626  // ------------------------------------------------------
627  m1->determineMatching2D(
628  m2, // The other map
629  q, // The other map pose
630  correspondences, matchParams, matchExtraResults);
631 
632  nCorrespondences = correspondences.size();
633 
634  if (!nCorrespondences)
635  {
636  // Nothing we can do !!
637  keepIteratingICP = false;
638  }
639  else
640  {
641  // Compute the estimated pose through iterative least-squares:
642  // Levenberg-Marquardt
643  // ----------------------------------------------------------------------
644  dJ_dq.setSize(3, nCorrespondences); // The jacobian of the
645  // error function wrt the
646  // transformation q
647 
648  double lambda = options.LM_initial_lambda; // The LM parameter
649 
650  double ccos = cos(q.phi());
651  double csin = sin(q.phi());
652 
653  double w1, w2, w3;
654  double q1, q2, q3;
655  double A, B;
656  const double Axy =
657  options.Axy_aprox_derivatives; // For approximating the
658  // derivatives
659 
660  // Compute at once the square errors for each point with the
661  // current "q" and the transformed points:
662  std::vector<float> sq_errors;
663  correspondences.squareErrorVector(
664  q, sq_errors, other_xs_trans, other_ys_trans);
665  double OSE_initial = math::sum(sq_errors);
666 
667  // Compute "dJ_dq"
668  // ------------------------------------
669  double rho2 = square(options.kernel_rho);
670  mrpt::tfest::TMatchingPairList::iterator it;
671  std::vector<float>::const_iterator other_x_trans, other_y_trans;
672  size_t i;
673 
674  for (i = 0, it = correspondences.begin(),
675  other_x_trans = other_xs_trans.begin(),
676  other_y_trans = other_ys_trans.begin();
677  i < nCorrespondences;
678  ++i, ++it, ++other_x_trans, ++other_y_trans)
679  {
680  // Jacobian: dJ_dx
681  // --------------------------------------
682  //#define ICP_DISTANCES_TO_LINE
683 
684 #ifndef ICP_DISTANCES_TO_LINE
685  w1 = *other_x_trans - Axy;
686  q1 = m1->squareDistanceToClosestCorrespondence(
687  w1, *other_y_trans);
688  q1 = kernel(q1, rho2);
689 
690  w2 = *other_x_trans;
691  q2 = m1->squareDistanceToClosestCorrespondence(
692  w2, *other_y_trans);
693  q2 = kernel(q2, rho2);
694 
695  w3 = *other_x_trans + Axy;
696  q3 = m1->squareDistanceToClosestCorrespondence(
697  w3, *other_y_trans);
698  q3 = kernel(q3, rho2);
699 #else
700  // The distance to the line that interpolates the TWO
701  // closest points:
702  float x1, y1, x2, y2, d1, d2;
703  m1->kdTreeTwoClosestPoint2D(
704  *other_x_trans, *other_y_trans, // The query
705  x1, y1, // Closest point #1
706  x2, y2, // Closest point #2
707  d1, d2);
708 
709  w1 = *other_x_trans - Axy;
711  w1, *other_y_trans, x1, y1, x2, y2);
712  q1 = kernel(q1, rho2);
713 
714  w2 = *other_x_trans;
716  w2, *other_y_trans, x1, y1, x2, y2);
717  q2 = kernel(q2, rho2);
718 
719  w3 = *other_x_trans + Axy;
721  w3, *other_y_trans, x1, y1, x2, y2);
722  q3 = kernel(q3, rho2);
723 #endif
724  // interpolate
725  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
726  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
727  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);
728 
729  dJ_dq(0, i) = (2 * A * *other_x_trans) + B;
730 
731  // Jacobian: dJ_dy
732  // --------------------------------------
733  w1 = *other_y_trans - Axy;
734 #ifdef ICP_DISTANCES_TO_LINE
736  *other_x_trans, w1, x1, y1, x2, y2);
737  q1 = kernel(q1, rho2);
738 #else
739  q1 = kernel(
740  square(it->this_x - *other_x_trans) +
741  square(it->this_y - w1),
742  rho2);
743 #endif
744 
745  w2 = *other_y_trans;
746  // q2 is alreay computed from above!
747  // q2 = m1->squareDistanceToClosestCorrespondence(
748  // *other_x_trans, w2 );
749  // q2= kernel( square(it->this_x - *other_x_trans)+ square(
750  // it->this_y - w2 ), rho2 );
751 
752  w3 = *other_y_trans + Axy;
753 #ifdef ICP_DISTANCES_TO_LINE
755  *other_x_trans, w3, x1, y1, x2, y2);
756  q3 = kernel(q3, rho2);
757 #else
758  q3 = kernel(
759  square(it->this_x - *other_x_trans) +
760  square(it->this_y - w3),
761  rho2);
762 #endif
763 
764  // interpolate
765  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
766  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
767  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);
768 
769  dJ_dq(1, i) = (2 * A * *other_y_trans) + B;
770 
771  // Jacobian: dR_dphi
772  // --------------------------------------
773  dJ_dq(2, i) =
774  dJ_dq(0, i) *
775  (-csin * it->other_x - ccos * it->other_y) +
776  dJ_dq(1, i) * (ccos * it->other_x - csin * it->other_y);
777 
778  } // end for each corresp.
779 
780  // Now we have the Jacobian in dJ_dq.
781 
782  // Compute the Hessian matrix H = dJ_dq * dJ_dq^T
783  CMatrixFloat H_(3, 3);
784  H_.matProductOf_AAt(dJ_dq);
785 
787  H = H_;
788 
789  bool keepIteratingLM = true;
790 
791  // ---------------------------------------------------
792  // Iterate the inner LM loop until convergence:
793  // ---------------------------------------------------
794  q_new = q;
795 
796  std::vector<float> new_sq_errors, new_other_xs_trans,
797  new_other_ys_trans;
798  size_t nLMiters = 0;
799  const size_t maxLMiters = 100;
800 
801  while (keepIteratingLM && ++nLMiters < maxLMiters)
802  {
803  // The LM heuristic is:
804  // x_{k+1} = x_k - ( H + \lambda diag(H) )^-1 * grad(J)
805  // grad(J) = dJ_dq * e (vector of errors)
806  C = H;
807  for (i = 0; i < 3; i++)
808  C(i, i) *=
809  (1 + lambda); // Levenberg-Maquardt heuristic
810 
811  C_inv = C.inverse_LLt();
812 
813  // LM_delta = C_inv * dJ_dq * sq_errors
814  const Eigen::Vector3f LM_delta =
815  (C_inv.asEigen() * dJ_dq.asEigen() *
817  &sq_errors[0], sq_errors.size()))
818  .eval();
819 
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]);
823 
824  // Compute the new square errors:
825  // ---------------------------------------
826  correspondences.squareErrorVector(
827  q_new, new_sq_errors, new_other_xs_trans,
828  new_other_ys_trans);
829 
830  float OSE_new = math::sum(new_sq_errors);
831 
832  bool improved = OSE_new < OSE_initial;
833 
834 #if 0 // Debuggin'
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 );
838  cout << "\\/J = "; utils::operator <<(cout,dJsq); cout << endl;
840 #endif
841 
842  keepIteratingLM =
843  fabs(LM_delta[0]) > options.minAbsStep_trans ||
844  fabs(LM_delta[1]) > options.minAbsStep_trans ||
845  fabs(LM_delta[2]) > options.minAbsStep_rot;
846 
847  if (improved)
848  {
849  // If resids have gone down, keep change and make lambda
850  // smaller by factor of 10
851  lambda /= 10;
852  q = q_new;
853  OSE_initial = OSE_new;
854  }
855  else
856  {
857  // Discard movement and try with larger lambda:
858  lambda *= 10;
859  }
860 
861  } // end iterative LM
862 
863 #if 0 // Debuggin'
864  cout << "ICP loop: " << lastMeanPose << " -> " << q << " LM iters: " << nLMiters << " threshold: " << matchParams.maxDistForCorrespondence << endl;
865 #endif
866  // --------------------------------------------------------
867  // now the conditions for the outer ICP loop
868  // --------------------------------------------------------
869  keepIteratingICP = true;
870  if (fabs(lastMeanPose.x() - q.x()) < options.minAbsStep_trans &&
871  fabs(lastMeanPose.y() - q.y()) < options.minAbsStep_trans &&
872  fabs(math::wrapToPi(lastMeanPose.phi() - q.phi())) <
873  options.minAbsStep_rot)
874  {
875  matchParams.maxDistForCorrespondence *= options.ALFA;
876  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
877  if (matchParams.maxDistForCorrespondence <
878  options.smallestThresholdDist)
879  keepIteratingICP = false;
880 
881  if (++matchParams.offset_other_map_points >=
882  options.corresponding_points_decimation)
883  matchParams.offset_other_map_points = 0;
884  }
885  lastMeanPose = q;
886  } // end of "else, there are correspondences"
887 
888  // Next iteration:
889  outInfo.nIterations++;
890 
891  if (outInfo.nIterations >= options.maxIterations &&
892  matchParams.maxDistForCorrespondence >
893  options.smallestThresholdDist)
894  {
895  matchParams.maxDistForCorrespondence *= options.ALFA;
896  }
897 
898  } while (
899  (keepIteratingICP && outInfo.nIterations < options.maxIterations) ||
900  (outInfo.nIterations >= options.maxIterations &&
901  matchParams.maxDistForCorrespondence >
902  options.smallestThresholdDist));
903 
904  outInfo.goodness = matchExtraResults.correspondencesRatio;
905 
906  // if (!options.skip_quality_calculation)
907  {
908  outInfo.quality = matchExtraResults.correspondencesRatio;
909  }
910 
911  } // end of "if m2 is not empty"
912 
913  return CPosePDFGaussian::Create(q, C_inv.cast_double());
914  MRPT_END
915 }
916 
918  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
919  const CPose3DPDFGaussian& initialEstimationPDF,
921 {
922  MRPT_START
923 
924  static CTicTac tictac;
925  TReturnInfo outInfoVal;
926  CPose3DPDF::Ptr resultPDF;
927 
928  if (outInfo) tictac.Tic();
929 
930  switch (options.ICP_algorithm)
931  {
932  case icpClassic:
933  resultPDF =
934  ICP3D_Method_Classic(m1, mm2, initialEstimationPDF, outInfoVal);
935  break;
937  THROW_EXCEPTION("Only icpClassic is implemented for ICP-3D");
938  break;
939  default:
941  "Invalid value for ICP_algorithm: %i",
942  static_cast<int>(options.ICP_algorithm));
943  } // end switch
944 
945  if (outInfo) outInfoVal.executionTime = tictac.Tac();
946 
947  // Copy the output info if requested:
948  if (outInfo)
949  if (auto* o = dynamic_cast<TReturnInfo*>(&outInfo.value().get()); o)
950  *o = outInfoVal;
951 
952  return resultPDF;
953 
954  MRPT_END
955 }
956 
958  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
959  const CPose3DPDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
960 {
961  MRPT_START
962 
963  // The result can be either a Gaussian or a SOG:
964  CPose3DPDF::Ptr resultPDF;
965  CPose3DPDFGaussian::Ptr gaussPdf;
966 
967  size_t nCorrespondences = 0;
968  bool keepApproaching;
969  CPose3D grossEst = initialEstimationPDF.mean;
970  mrpt::tfest::TMatchingPairList correspondences, old_correspondences;
971  CPose3D lastMeanPose;
972 
973  // Assure the class of the maps:
975  const CPointsMap* m2 = (CPointsMap*)mm2;
976 
977  // Asserts:
978  // -----------------
979  ASSERT_(options.ALFA > 0 && options.ALFA < 1);
980 
981  // The algorithm output auxiliar info:
982  // -------------------------------------------------
983  outInfo.nIterations = 0;
984  outInfo.goodness = 1;
985  outInfo.quality = 0;
986 
987  // The gaussian PDF to estimate:
988  // ------------------------------------------------------
989  gaussPdf = std::make_shared<CPose3DPDFGaussian>();
990 
991  // First gross approximation:
992  gaussPdf->mean = grossEst;
993 
994  // Initial thresholds:
995  TMatchingParams matchParams;
996  TMatchingExtraResults matchExtraResults;
997 
998  matchParams.maxDistForCorrespondence =
999  options.thresholdDist; // Distance threshold
1000  matchParams.maxAngularDistForCorrespondence =
1001  options.thresholdAng; // Angular threshold
1002  matchParams.onlyKeepTheClosest = true;
1003  matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
1004  matchParams.decimation_other_map_points =
1005  options.corresponding_points_decimation;
1006 
1007  // Ensure maps are not empty!
1008  // ------------------------------------------------------
1009  if (!m2->isEmpty())
1010  {
1011  matchParams.offset_other_map_points = 0;
1012 
1013  // ------------------------------------------------------
1014  // The ICP loop
1015  // ------------------------------------------------------
1016  do
1017  {
1018  matchParams.angularDistPivotPoint = TPoint3D(
1019  gaussPdf->mean.x(), gaussPdf->mean.y(), gaussPdf->mean.z());
1020 
1021  // ------------------------------------------------------
1022  // Find the matching (for a points map)
1023  // ------------------------------------------------------
1024  m1->determineMatching3D(
1025  m2, // The other map
1026  gaussPdf->mean, // The other map pose
1027  correspondences, matchParams, matchExtraResults);
1028 
1029  nCorrespondences = correspondences.size();
1030 
1031  if (!nCorrespondences)
1032  {
1033  // Nothing we can do !!
1034  keepApproaching = false;
1035  }
1036  else
1037  {
1038  // Compute the estimated pose, using Horn's method.
1039  // ----------------------------------------------------------------------
1040  mrpt::poses::CPose3DQuat estPoseQuat;
1041  double transf_scale;
1043  correspondences, estPoseQuat, transf_scale,
1044  false /* dont force unit scale */);
1045  gaussPdf->mean = mrpt::poses::CPose3D(estPoseQuat);
1046 
1047  // If matching has not changed, decrease the thresholds:
1048  // --------------------------------------------------------
1049  keepApproaching = true;
1050  if (!(fabs(lastMeanPose.x() - gaussPdf->mean.x()) >
1051  options.minAbsStep_trans ||
1052  fabs(lastMeanPose.y() - gaussPdf->mean.y()) >
1053  options.minAbsStep_trans ||
1054  fabs(lastMeanPose.z() - gaussPdf->mean.z()) >
1055  options.minAbsStep_trans ||
1056  fabs(math::wrapToPi(
1057  lastMeanPose.yaw() - gaussPdf->mean.yaw())) >
1058  options.minAbsStep_rot ||
1059  fabs(math::wrapToPi(
1060  lastMeanPose.pitch() - gaussPdf->mean.pitch())) >
1061  options.minAbsStep_rot ||
1062  fabs(math::wrapToPi(
1063  lastMeanPose.roll() - gaussPdf->mean.roll())) >
1064  options.minAbsStep_rot))
1065  {
1066  matchParams.maxDistForCorrespondence *= options.ALFA;
1067  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
1068  if (matchParams.maxDistForCorrespondence <
1069  options.smallestThresholdDist)
1070  keepApproaching = false;
1071 
1072  if (++matchParams.offset_other_map_points >=
1073  options.corresponding_points_decimation)
1074  matchParams.offset_other_map_points = 0;
1075  }
1076 
1077  lastMeanPose = gaussPdf->mean;
1078 
1079  } // end of "else, there are correspondences"
1080 
1081  // Next iteration:
1082  outInfo.nIterations++;
1083 
1084  if (outInfo.nIterations >= options.maxIterations &&
1085  matchParams.maxDistForCorrespondence >
1086  options.smallestThresholdDist)
1087  {
1088  matchParams.maxDistForCorrespondence *= options.ALFA;
1089  }
1090 
1091  } while (
1092  (keepApproaching && outInfo.nIterations < options.maxIterations) ||
1093  (outInfo.nIterations >= options.maxIterations &&
1094  matchParams.maxDistForCorrespondence >
1095  options.smallestThresholdDist));
1096 
1097  // -------------------------------------------------
1098  // Obtain the covariance matrix of the estimation
1099  // -------------------------------------------------
1100  if (!options.skip_cov_calculation && nCorrespondences)
1101  {
1102  // ...
1103  }
1104 
1105  //
1106  outInfo.goodness = matchExtraResults.correspondencesRatio;
1107 
1108  } // end of "if m2 is not empty"
1109 
1110  // Return the gaussian distribution:
1111  resultPDF = gaussPdf;
1112 
1113  return resultPDF;
1114 
1115  MRPT_END
1116 }
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.
Definition: CICP.h:31
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...
#define MRPT_START
Definition: exceptions.h:241
CPose2D mean
The mean value.
CPose3D mean
The mean value.
const float normalizationStd
double x
X,Y coordinates.
Definition: TPose2D.h:30
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
size_t decimation_other_map_points
(Default=1) Only consider 1 out of this number of points from the "other" map.
mrpt::poses::CPosePDF::Ptr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt) override
See base method docs.
Definition: CICP.cpp:36
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...
Definition: se2_l2.cpp:92
Parameters for se2_l2_robust().
Definition: se2.h:73
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)
Definition: CPose3D.h:552
double closestSquareDistanceFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2)
Returns the square distance from a point to a line.
Definition: geometry.cpp:100
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::vision::TStereoCalibParams params
unsigned int nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
STL namespace.
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CICP.cpp:80
mrpt::poses::CPose3DPDF::Ptr ICP3D_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:957
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matching between this and another 2D point map, which includes finding: ...
Definition: CMetricMap.cpp:119
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
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.
Definition: CObject.h:102
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
constexpr double DEG2RAD(const double x)
Degrees to radians.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
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...
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:200
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void y_incr(const double v)
Definition: CPoseOrPoint.h:174
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
mrpt::poses::CPosePDF::Ptr ICP_Method_LM(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:556
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
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. ...
Definition: CICP.cpp:126
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp.
Definition: CICP.h:19
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...
Definition: os.cpp:430
return_t square(const num_t x)
Inline function for the square of a number.
#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.
void x_incr(const double v)
Definition: CPoseOrPoint.h:170
size_t offset_other_map_points
Index of the first point in the "other" map to start checking for correspondences (Default=0) ...
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
constexpr double RAD2DEG(const double x)
Radians to degrees.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
The ICP algorithm return information.
Definition: CICP.h:190
Lightweight 2D pose.
Definition: TPose2D.h:22
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:24
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 ...
Definition: se3_l2.cpp:219
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options.
Definition: CICP.h:27
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
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...
CMatrixFixed< double, ROWS, COLS > cast_double() const
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt) override
Definition: CICP.cpp:917
Output placeholder for se2_l2_robust()
Definition: se2.h:130
double phi
Orientation (rads)
Definition: TPose2D.h:32
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
Definition: CMetricMap.cpp:131
mrpt::poses::CPosePDF::Ptr ICP_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:184
Covariance of a least-squares optimizer (includes data association uncertainty)
Definition: CICP.h:34
double quality
A measure of the &#39;quality&#39; of the local minimum of the sqr.
Definition: CICP.h:205
float kernel(float x2, float rho2)
Computes: or just return the input if options.useKernel = false.
Definition: CICP.cpp:174



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020