MRPT  2.0.3
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 }
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:221
mrpt::tfest::TSE2RobustParams
Parameters for se2_l2_robust().
Definition: se2.h:73
ops_containers.h
ransac_mahalanobisDistanceThreshold
const float ransac_mahalanobisDistanceThreshold
Definition: se3_ransac_unittest.cpp:38
mrpt::slam::CICP::TReturnInfo::nIterations
unsigned int nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
mrpt::slam::CICP::TReturnInfo::goodness
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
Definition: CICP.h:200
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::math::CMatrixFixed::cast_double
CMatrixFixed< double, ROWS, COLS > cast_double() const
Definition: CMatrixFixed_impl.h:25
mrpt::maps::TMatchingParams::decimation_other_map_points
size_t decimation_other_map_points
(Default=1) Only consider 1 out of this number of points from the "other" map.
Definition: metric_map_types.h:36
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:480
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::slam::icpLevenbergMarquardt
@ icpLevenbergMarquardt
Definition: CICP.h:22
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
mrpt::slam::CICP::TConfigParams::saveToConfigFile
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
mrpt::slam::TICPCovarianceMethod
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options.
Definition: CICP.h:27
CPosePDFSOG.h
mrpt::poses::CPoseOrPoint::x_incr
void x_incr(const double v)
Definition: CPoseOrPoint.h:170
mrpt::math::CMatrixDynamic::setSize
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
Definition: CMatrixDynamic.h:352
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::tfest::TMatchingPairList::squareErrorVector
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,...
Definition: TMatchingPair.cpp:132
mrpt::slam::TMetricMapAlignmentResult::executionTime
double executionTime
Definition: CMetricMapsAlignmentAlgorithm.h:31
mrpt::slam::CICP::Align3DPDF
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
mrpt::slam::CICP::ICP3D_Method_Classic
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
TPose2D.h
CICP.h
mrpt::poses::CPoseOrPoint::y_incr
void y_incr(const double v)
Definition: CPoseOrPoint.h:174
CPose3DPDFGaussian.h
mrpt::maps::CPointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
Definition: CPointsMap.h:67
mrpt::math::TPose2D::y
double y
Definition: TPose2D.h:30
wrap2pi.h
mrpt::slam::CICP::ICP_Method_Classic
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
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
mrpt::slam::TICPAlgorithm
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options.
Definition: CICP.h:19
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:45
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
CPose2D.h
mrpt::maps::TMatchingExtraResults::correspondencesRatio
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
Definition: metric_map_types.h:55
mrpt::maps::TMatchingParams::onlyUniqueRobust
bool onlyUniqueRobust
Additional consistency filter: "onlyKeepTheClosest" allows one correspondence for each "local map" po...
Definition: metric_map_types.h:33
results
map< string, CVectorDouble > results
Definition: vision_stereo_rectify/test.cpp:33
mrpt::slam::icpClassic
@ icpClassic
Definition: CICP.h:21
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:190
mrpt::poses::CPose3DPDFGaussian::Ptr
std::shared_ptr< mrpt::poses ::CPose3DPDFGaussian > Ptr
Definition: CPose3DPDFGaussian.h:41
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:44
mrpt::math::MatrixBase::inverse_LLt
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
Definition: MatrixBase_impl.h:195
mrpt::maps::CMetricMap::determineMatching2D
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
CConfigFileBase.h
normalizationStd
const float normalizationStd
Definition: se3_ransac_unittest.cpp:37
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
Eigen::Map
Definition: math_frwds.h:24
mrpt::slam::CICP::kernel
float kernel(float x2, float rho2)
Computes:
Definition: CICP.cpp:174
mrpt::maps::TMatchingParams::maxDistForCorrespondence
float maxDistForCorrespondence
Maximum linear distance between two points to be paired (meters)
Definition: metric_map_types.h:23
mrpt::math::closestSquareDistanceFromPointToLine
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
mrpt::math::CMatrixFixed< double, 3, 3 >
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
CPosePDF.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::tfest::se3_l2
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
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::math::MatrixBase::matProductOf_AAt
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::math::CMatrixDynamic::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixDynamic.h:540
mrpt::rtti::TRuntimeClassId::derivedFrom
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:24
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
A
Definition: is_defined_unittest.cpp:13
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
tfest.h
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:20
mrpt::maps::CPointsMap::isEmpty
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CPointsMap.cpp:595
mrpt::poses::CPosePDFGaussian::Ptr
std::shared_ptr< mrpt::poses ::CPosePDFGaussian > Ptr
Definition: CPosePDFGaussian.h:30
mrpt::maps::TMatchingParams::offset_other_map_points
size_t offset_other_map_points
Index of the first point in the "other" map to start checking for correspondences (Default=0)
Definition: metric_map_types.h:39
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixFixed.h:251
CTicTac.h
CPosePDFGaussian.h
slam-precomp.h
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::maps::TMatchingParams::onlyKeepTheClosest
bool onlyKeepTheClosest
If set to true (default), only the closest correspondence will be returned.
Definition: metric_map_types.h:29
mrpt::tfest::TSE2RobustResult
Output placeholder for se2_l2_robust()
Definition: se2.h:130
mrpt::slam::CICP::ICP_Method_LM
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
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::tfest::se2_l2
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
mrpt::maps::CMetricMap::determineMatching3D
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::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
mrpt::maps::CMetricMap::isEmpty
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
mrpt::slam::icpCovLinealMSE
@ icpCovLinealMSE
Use the covariance of the optimal registration, disregarding uncertainty in data association.
Definition: CICP.h:31
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
CPose3DPDF.h
mrpt::maps::TMatchingParams::angularDistPivotPoint
mrpt::math::TPoint3D angularDistPivotPoint
The point used to calculate angular distances: e.g.
Definition: metric_map_types.h:42
mrpt::maps
Definition: CBeacon.h:21
mrpt::slam::icpCovFiniteDifferences
@ icpCovFiniteDifferences
Covariance of a least-squares optimizer (includes data association uncertainty)
Definition: CICP.h:34
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc.,...
Definition: metric_map_types.h:51
mrpt::poses::CPosePDFSOG::Ptr
std::shared_ptr< mrpt::poses ::CPosePDFSOG > Ptr
Definition: CPosePDFSOG.h:36
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
mrpt::slam::CICP::TConfigParams::loadFromConfigFile
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
CArchive.h
mrpt::slam::CICP::TReturnInfo::quality
double quality
A measure of the 'quality' of the local minimum of the sqr.
Definition: CICP.h:205
mrpt::tfest::se2_l2_robust
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 ...
Definition: se2_l2_ransac.cpp:79
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:41
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:78
mrpt::math::CMatrixDynamic< double >
mrpt::optional_ref
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: TPose2D.h:30
mrpt::maps::TMatchingParams::maxAngularDistForCorrespondence
float maxAngularDistForCorrespondence
Allowed "angular error" (in radians): this permits larger pairing threshold distances to more distant...
Definition: metric_map_types.h:26
mrpt::comms::operator<<
std::ostream & operator<<(std::ostream &o, const TFTDIDevice &d)
Print out all the information of a FTDI device in textual form.
Definition: CInterfaceFTDI_WIN.cpp:451
mrpt::system::pause
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
mrpt::system
Definition: backtrace.h:14
mrpt::slam::CICP::AlignPDF
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



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020