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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019