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 
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(const float& x2, const 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
float kernel(const float &x2, const float &rho2)
Computes: or just return the input if options.useKernel = false.
Definition: CICP.cpp:171
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.
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:100
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
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: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:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
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: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:89
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.
T square(const T x)
Inline function for the square of a number.
constexpr double DEG2RAD(const double x)
Degrees to radians.
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
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...
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
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:554
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.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:5645
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:84
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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)
Lightweight 3D point.
Definition: TPoint3D.h:90
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...
GLenum const GLfloat * params
Definition: glext.h:3538
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
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: e47402b84 Wed Oct 23 01:09:07 2019 +0200 at miƩ oct 23 01:10:13 CEST 2019