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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020