Main MRPT website > C++ reference for MRPT 1.9.9
CICP.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/slam/CICP.h>
13 #include <mrpt/tfest.h>
14 #include <mrpt/poses/CPosePDFSOG.h>
15 #include <mrpt/utils/CTicTac.h>
16 #include <mrpt/utils/CStream.h>
17 #include <mrpt/utils/CConfigFileBase.h> // MRPT_LOAD_*()
18 #include <mrpt/math/wrap2pi.h>
19 #include <mrpt/math/ops_containers.h>
20 #include <mrpt/poses/CPose2D.h>
21 #include <mrpt/poses/CPosePDF.h>
22 #include <mrpt/poses/CPose3DPDF.h>
23 #include <mrpt/poses/CPosePDFGaussian.h>
24 #include <mrpt/poses/CPose3DPDFGaussian.h>
25 
26 using namespace mrpt::slam;
27 using namespace mrpt::maps;
28 using namespace mrpt::math;
29 using namespace mrpt::poses;
30 using namespace mrpt::utils;
31 using namespace std;
32 
33 /*---------------------------------------------------------------
34 The method for aligning a pair of 2D points map.
35 * The meaning of some parameters are implementation dependent,
36 * so look for derived classes for instructions.
37 * The target is to find a PDF for the pose displacement between
38 * maps, <b>thus the pose of m2 relative to m1</b>. This pose
39 * is returned as a PDF rather than a single value.
40 *
41 * \param m1 [IN] The first map
42 * \param m2 [IN] The second map. The pose of this map respect to m1
43 is to be estimated.
44 * \param grossEst [IN] An initial gross estimation for the displacement.
45 If a given algorithm doesn't need it, set to <code>CPose2D(0,0,0)</code> for
46 example.
47 * \param pdf [IN/OUT] A pointer to a CPosePDF pointer, initially set
48 to nullptr for this method to create the object. For greater efficiency, this
49 object can be left undeleted and passed again to the method. When <b>not used
50 anymore remember to delete it</b> using <code>delete pdf;</code>
51 * \param runningTime [OUT] A pointer to a container for obtaining the
52 algorithm running time in seconds, or nullptr if you don't need it.
53 * \param info [OUT] See derived classes for details, or nullptr if it
54 isn't needed.
55 *
56 * \sa CPointsMapAlignmentAlgorithm
57  ---------------------------------------------------------------*/
58 CPosePDF::Ptr CICP::AlignPDF(
59  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
60  const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
61  void* info)
62 {
63  MRPT_START
64 
65  CTicTac tictac;
66  TReturnInfo outInfo;
67  CPosePDF::Ptr resultPDF;
68 
69  if (runningTime) tictac.Tic();
70 
71  switch (options.ICP_algorithm)
72  {
73  case icpClassic:
74  resultPDF =
75  ICP_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
76  break;
77  case icpLevenbergMarquardt:
78  resultPDF = ICP_Method_LM(m1, mm2, initialEstimationPDF, outInfo);
79  break;
80  default:
81  THROW_EXCEPTION_FMT(
82  "Invalid value for ICP_algorithm: %i",
83  static_cast<int>(options.ICP_algorithm));
84  } // end switch
85 
86  if (runningTime) *runningTime = tictac.Tac();
87 
88  // Copy the output info if requested:
89  if (info) *static_cast<TReturnInfo*>(info) = outInfo;
90 
91  return resultPDF;
92 
93  MRPT_END
94 }
95 
96 /*---------------------------------------------------------------
97  TConfigParams
98  ---------------------------------------------------------------*/
99 CICP::TConfigParams::TConfigParams()
100  : ICP_algorithm(icpClassic),
101  ICP_covariance_method(icpCovFiniteDifferences),
102 
103  onlyClosestCorrespondences(true),
104  onlyUniqueRobust(false),
105  maxIterations(40),
106  minAbsStep_trans(1e-6f),
107  minAbsStep_rot(1e-6f),
108  thresholdDist(0.75f),
109  thresholdAng(DEG2RAD(0.15f)),
110  ALFA(0.50f),
111  smallestThresholdDist(0.10f),
112  covariance_varPoints(square(0.02f)),
113  doRANSAC(false),
114 
115  ransac_minSetSize(3),
116  ransac_maxSetSize(20),
117  ransac_nSimulations(100),
118  ransac_mahalanobisDistanceThreshold(3.0f),
119  normalizationStd(0.02f),
120  ransac_fuseByCorrsMatch(true),
121  ransac_fuseMaxDiffXY(0.01f),
122  ransac_fuseMaxDiffPhi(DEG2RAD(0.1f)),
123 
124  kernel_rho(0.07f),
125  use_kernel(true),
126  Axy_aprox_derivatives(0.05f),
127 
128  LM_initial_lambda(1e-4f),
129 
130  skip_cov_calculation(false),
131  skip_quality_calculation(true),
132 
133  corresponding_points_decimation(5)
134 {
135 }
136 
137 /*---------------------------------------------------------------
138  loadFromConfigFile
139  ---------------------------------------------------------------*/
140 void CICP::TConfigParams::loadFromConfigFile(
141  const mrpt::utils::CConfigFileBase& iniFile, const std::string& section)
142 {
143  MRPT_LOAD_CONFIG_VAR(maxIterations, int, iniFile, section);
144  MRPT_LOAD_CONFIG_VAR(minAbsStep_trans, float, iniFile, section);
145  MRPT_LOAD_CONFIG_VAR(minAbsStep_rot, float, iniFile, section);
146 
147  ICP_algorithm = iniFile.read_enum<TICPAlgorithm>(
148  section, "ICP_algorithm", ICP_algorithm);
149  ICP_covariance_method = iniFile.read_enum<TICPCovarianceMethod>(
150  section, "ICP_covariance_method", ICP_covariance_method);
151 
152  MRPT_LOAD_CONFIG_VAR(thresholdDist, float, iniFile, section);
153  thresholdAng = DEG2RAD(
154  iniFile.read_float(
155  section.c_str(), "thresholdAng_DEG", RAD2DEG(thresholdAng)));
156 
157  MRPT_LOAD_CONFIG_VAR(ALFA, float, iniFile, section);
158  MRPT_LOAD_CONFIG_VAR(smallestThresholdDist, float, iniFile, section);
159  MRPT_LOAD_CONFIG_VAR(onlyClosestCorrespondences, bool, iniFile, section);
160  MRPT_LOAD_CONFIG_VAR(onlyUniqueRobust, bool, iniFile, section);
161  MRPT_LOAD_CONFIG_VAR(doRANSAC, bool, iniFile, section);
162  MRPT_LOAD_CONFIG_VAR(covariance_varPoints, float, iniFile, section);
163 
164  MRPT_LOAD_CONFIG_VAR(ransac_minSetSize, int, iniFile, section);
165  MRPT_LOAD_CONFIG_VAR(ransac_maxSetSize, int, iniFile, section);
166  MRPT_LOAD_CONFIG_VAR(
167  ransac_mahalanobisDistanceThreshold, float, iniFile, section);
168  MRPT_LOAD_CONFIG_VAR(ransac_nSimulations, int, iniFile, section);
169  MRPT_LOAD_CONFIG_VAR(normalizationStd, float, iniFile, section);
170  MRPT_LOAD_CONFIG_VAR(ransac_fuseByCorrsMatch, bool, iniFile, section);
171  MRPT_LOAD_CONFIG_VAR(ransac_fuseMaxDiffXY, float, iniFile, section);
172  ransac_fuseMaxDiffPhi = DEG2RAD(
173  iniFile.read_float(
174  section.c_str(), "ransac_fuseMaxDiffPhi_DEG",
175  RAD2DEG(ransac_fuseMaxDiffPhi)));
176 
177  MRPT_LOAD_CONFIG_VAR(kernel_rho, float, iniFile, section);
178  MRPT_LOAD_CONFIG_VAR(use_kernel, bool, iniFile, section);
179  MRPT_LOAD_CONFIG_VAR(Axy_aprox_derivatives, float, iniFile, section);
180  MRPT_LOAD_CONFIG_VAR(LM_initial_lambda, float, iniFile, section);
181 
182  MRPT_LOAD_CONFIG_VAR(skip_cov_calculation, bool, iniFile, section);
183  MRPT_LOAD_CONFIG_VAR(skip_quality_calculation, bool, iniFile, section);
184 
185  MRPT_LOAD_CONFIG_VAR(
186  corresponding_points_decimation, int, iniFile, section);
187 }
188 
189 /*---------------------------------------------------------------
190  dumpToTextStream
191  ---------------------------------------------------------------*/
192 void CICP::TConfigParams::dumpToTextStream(mrpt::utils::CStream& out) const
193 {
194  out.printf("\n----------- [CICP::TConfigParams] ------------ \n\n");
195 
196  out.printf(
197  "ICP_algorithm = %s\n",
198  mrpt::utils::TEnumType<TICPAlgorithm>::value2name(ICP_algorithm)
199  .c_str());
200  out.printf(
201  "ICP_covariance_method = %s\n",
202  mrpt::utils::TEnumType<TICPCovarianceMethod>::value2name(
203  ICP_covariance_method)
204  .c_str());
205  out.printf("maxIterations = %i\n", maxIterations);
206  out.printf(
207  "minAbsStep_trans = %f\n", minAbsStep_trans);
208  out.printf(
209  "minAbsStep_rot = %f\n", minAbsStep_rot);
210 
211  out.printf("thresholdDist = %f\n", thresholdDist);
212  out.printf(
213  "thresholdAng = %f deg\n",
214  RAD2DEG(thresholdAng));
215  out.printf("ALFA = %f\n", ALFA);
216  out.printf(
217  "smallestThresholdDist = %f\n",
218  smallestThresholdDist);
219  out.printf(
220  "onlyClosestCorrespondences = %c\n",
221  onlyClosestCorrespondences ? 'Y' : 'N');
222  out.printf(
223  "onlyUniqueRobust = %c\n",
224  onlyUniqueRobust ? 'Y' : 'N');
225  out.printf(
226  "covariance_varPoints = %f\n", covariance_varPoints);
227  out.printf(
228  "doRANSAC = %c\n", doRANSAC ? 'Y' : 'N');
229  out.printf(
230  "ransac_minSetSize = %i\n", ransac_minSetSize);
231  out.printf(
232  "ransac_maxSetSize = %i\n", ransac_maxSetSize);
233  out.printf(
234  "ransac_mahalanobisDistanceThreshold = %f\n",
235  ransac_mahalanobisDistanceThreshold);
236  out.printf(
237  "ransac_nSimulations = %i\n", ransac_nSimulations);
238  out.printf(
239  "ransac_fuseByCorrsMatch = %c\n",
240  ransac_fuseByCorrsMatch ? 'Y' : 'N');
241  out.printf(
242  "ransac_fuseMaxDiffXY = %f\n", ransac_fuseMaxDiffXY);
243  out.printf(
244  "ransac_fuseMaxDiffPhi = %f deg\n",
245  RAD2DEG(ransac_fuseMaxDiffPhi));
246  out.printf(
247  "normalizationStd = %f\n", normalizationStd);
248  out.printf("kernel_rho = %f\n", kernel_rho);
249  out.printf(
250  "use_kernel = %c\n",
251  use_kernel ? 'Y' : 'N');
252  out.printf(
253  "Axy_aprox_derivatives = %f\n",
254  Axy_aprox_derivatives);
255  out.printf(
256  "LM_initial_lambda = %f\n", LM_initial_lambda);
257  out.printf(
258  "skip_cov_calculation = %c\n",
259  skip_cov_calculation ? 'Y' : 'N');
260  out.printf(
261  "skip_quality_calculation = %c\n",
262  skip_quality_calculation ? 'Y' : 'N');
263  out.printf(
264  "corresponding_points_decimation = %u\n",
265  (unsigned int)corresponding_points_decimation);
266  out.printf("\n");
267 }
268 
269 /*---------------------------------------------------------------
270  kernel
271  ---------------------------------------------------------------*/
272 float CICP::kernel(const float& x2, const float& rho2)
273 {
274  return options.use_kernel ? (x2 / (x2 + rho2)) : x2;
275 }
276 
277 /*----------------------------------------------------------------------------
278 
279  ICP_Method_Classic
280 
281  ----------------------------------------------------------------------------*/
282 CPosePDF::Ptr CICP::ICP_Method_Classic(
283  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
284  const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
285 {
286  MRPT_START
287 
288  // The result can be either a Gaussian or a SOG:
289  CPosePDF::Ptr resultPDF;
290  CPosePDFGaussian::Ptr gaussPdf;
291  CPosePDFSOG::Ptr SOG;
292 
293  size_t nCorrespondences = 0;
294  bool keepApproaching;
295  CPose2D grossEst = initialEstimationPDF.mean;
296  mrpt::utils::TMatchingPairList correspondences, old_correspondences;
297  CPose2D lastMeanPose;
298 
299  // Assure the class of the maps:
300  const mrpt::maps::CMetricMap* m2 = mm2;
301 
302  // Asserts:
303  // -----------------
304  ASSERT_(options.ALFA >= 0 && options.ALFA < 1);
305 
306  // The algorithm output auxiliar info:
307  // -------------------------------------------------
308  outInfo.nIterations = 0;
309  outInfo.goodness = 1;
310  outInfo.quality = 0;
311 
312  // The gaussian PDF to estimate:
313  // ------------------------------------------------------
314  gaussPdf = mrpt::make_aligned_shared<CPosePDFGaussian>();
315 
316  // First gross approximation:
317  gaussPdf->mean = grossEst;
318 
319  // Initial thresholds:
320  mrpt::maps::TMatchingParams matchParams;
321  mrpt::maps::TMatchingExtraResults matchExtraResults;
322 
323  matchParams.maxDistForCorrespondence =
324  options.thresholdDist; // Distance threshold
325  matchParams.maxAngularDistForCorrespondence =
326  options.thresholdAng; // Angular threshold
327  matchParams.onlyKeepTheClosest = options.onlyClosestCorrespondences;
328  matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
329  matchParams.decimation_other_map_points =
330  options.corresponding_points_decimation;
331 
332  // Asure maps are not empty!
333  // ------------------------------------------------------
334  if (!m2->isEmpty())
335  {
336  matchParams.offset_other_map_points = 0;
337 
338  // ------------------------------------------------------
339  // The ICP loop
340  // ------------------------------------------------------
341  do
342  {
343  // ------------------------------------------------------
344  // Find the matching (for a points map)
345  // ------------------------------------------------------
346  matchParams.angularDistPivotPoint = TPoint3D(
347  gaussPdf->mean.x(), gaussPdf->mean.y(),
348  0); // Pivot point for angular measurements
349 
350  m1->determineMatching2D(
351  m2, // The other map
352  gaussPdf->mean, // The other map pose
353  correspondences, matchParams, matchExtraResults);
354 
355  nCorrespondences = correspondences.size();
356 
357  // ***DEBUG***
358  // correspondences.dumpToFile("debug_correspondences.txt");
359 
360  if (!nCorrespondences)
361  {
362  // Nothing we can do !!
363  keepApproaching = false;
364  }
365  else
366  {
367  // Compute the estimated pose.
368  // (Method from paper of J.Gonzalez, Martinez y Morales)
369  // ----------------------------------------------------------------------
370  mrpt::math::TPose2D est_mean;
371  mrpt::tfest::se2_l2(correspondences, est_mean);
372 
373  gaussPdf->mean = mrpt::poses::CPose2D(est_mean);
374 
375  // If matching has not changed, decrease the thresholds:
376  // --------------------------------------------------------
377  keepApproaching = true;
378  if (!(fabs(lastMeanPose.x() - gaussPdf->mean.x()) >
379  options.minAbsStep_trans ||
380  fabs(lastMeanPose.y() - gaussPdf->mean.y()) >
381  options.minAbsStep_trans ||
382  fabs(
383  math::wrapToPi(
384  lastMeanPose.phi() - gaussPdf->mean.phi())) >
385  options.minAbsStep_rot))
386  {
387  matchParams.maxDistForCorrespondence *= options.ALFA;
388  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
389  if (matchParams.maxDistForCorrespondence <
390  options.smallestThresholdDist)
391  keepApproaching = false;
392 
393  if (++matchParams.offset_other_map_points >=
394  options.corresponding_points_decimation)
395  matchParams.offset_other_map_points = 0;
396  }
397 
398  lastMeanPose = gaussPdf->mean;
399 
400  } // end of "else, there are correspondences"
401 
402  // Next iteration:
403  outInfo.nIterations++;
404 
405  if (outInfo.nIterations >= options.maxIterations &&
406  matchParams.maxDistForCorrespondence >
407  options.smallestThresholdDist)
408  {
409  matchParams.maxDistForCorrespondence *= options.ALFA;
410  }
411 
412  } while (
413  (keepApproaching && outInfo.nIterations < options.maxIterations) ||
414  (outInfo.nIterations >= options.maxIterations &&
415  matchParams.maxDistForCorrespondence >
416  options.smallestThresholdDist));
417 
418  // -------------------------------------------------
419  // Obtain the covariance matrix of the estimation
420  // -------------------------------------------------
421  if (!options.skip_cov_calculation && nCorrespondences)
422  {
423  switch (options.ICP_covariance_method)
424  {
425  // ----------------------------------------------
426  // METHOD: MSE linear estimation
427  // ----------------------------------------------
428  case icpCovLinealMSE:
429  mrpt::tfest::se2_l2(correspondences, *gaussPdf);
430  // Scale covariance:
431  gaussPdf->cov *= options.covariance_varPoints;
432  break;
433 
434  // ----------------------------------------------
435  // METHOD: Method from Oxford MRG's "OXSMV2"
436  //
437  // It is the equivalent covariance resulting
438  // from a Levenberg-Maquardt optimization stage.
439  // ----------------------------------------------
440  case icpCovFiniteDifferences:
441  {
442  Eigen::Matrix<double, 3, Eigen::Dynamic> D(
443  3, nCorrespondences);
444 
445  const TPose2D transf(gaussPdf->mean);
446 
447  double ccos = cos(transf.phi);
448  double csin = sin(transf.phi);
449 
450  double w1, w2, w3;
451  double q1, q2, q3;
452  double A, B;
453  double Axy = 0.01;
454 
455  // Fill out D:
456  double rho2 = square(options.kernel_rho);
457  mrpt::utils::TMatchingPairList::iterator it;
458  size_t i;
459  for (i = 0, it = correspondences.begin();
460  i < nCorrespondences; ++i, ++it)
461  {
462  float other_x_trans =
463  transf.x + ccos * it->other_x - csin * it->other_y;
464  float other_y_trans =
465  transf.y + csin * it->other_x + ccos * it->other_y;
466 
467  // Jacobian: dR2_dx
468  // --------------------------------------
469  w1 = other_x_trans - Axy;
470  q1 = kernel(
471  square(it->this_x - w1) +
472  square(it->this_y - other_y_trans),
473  rho2);
474 
475  w2 = other_x_trans;
476  q2 = kernel(
477  square(it->this_x - w2) +
478  square(it->this_y - other_y_trans),
479  rho2);
480 
481  w3 = other_x_trans + Axy;
482  q3 = kernel(
483  square(it->this_x - w3) +
484  square(it->this_y - other_y_trans),
485  rho2);
486 
487  // interpolate
488  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
489  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
490  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
491  (w1 - w2);
492 
493  D(0, i) = (2 * A * other_x_trans) + B;
494 
495  // Jacobian: dR2_dy
496  // --------------------------------------
497  w1 = other_y_trans - Axy;
498  q1 = kernel(
499  square(it->this_x - other_x_trans) +
500  square(it->this_y - w1),
501  rho2);
502 
503  w2 = other_y_trans;
504  q2 = kernel(
505  square(it->this_x - other_x_trans) +
506  square(it->this_y - w2),
507  rho2);
508 
509  w3 = other_y_trans + Axy;
510  q3 = kernel(
511  square(it->this_x - other_x_trans) +
512  square(it->this_y - w3),
513  rho2);
514 
515  // interpolate
516  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
517  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
518  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
519  (w1 - w2);
520 
521  D(1, i) = (2 * A * other_y_trans) + B;
522 
523  // Jacobian: dR_dphi
524  // --------------------------------------
525  D(2, i) =
526  D(0, i) *
527  (-csin * it->other_x - ccos * it->other_y) +
528  D(1, i) * (ccos * it->other_x - csin * it->other_y);
529 
530  } // end for each corresp.
531 
532  // COV = ( D*D^T + lamba*I )^-1
533  CMatrixDouble33 DDt = D * D.transpose();
534 
535  for (i = 0; i < 3; i++)
536  DDt(i, i) += 1e-6; // Just to make sure the matrix is
537  // not singular, while not changing
538  // its covariance significantly.
539 
540  DDt.inv(gaussPdf->cov);
541  }
542  break;
543  default:
544  THROW_EXCEPTION_FMT(
545  "Invalid value for ICP_covariance_method: %i",
546  static_cast<int>(options.ICP_covariance_method));
547  }
548  }
549 
550  outInfo.goodness = matchExtraResults.correspondencesRatio;
551 
552  if (!nCorrespondences || options.skip_quality_calculation)
553  {
554  outInfo.quality = matchExtraResults.correspondencesRatio;
555  }
556  else
557  {
558  // Compute a crude estimate of the quality of scan matching at this
559  // local minimum:
560  // -----------------------------------------------------------------------------------
561  float Axy = matchParams.maxDistForCorrespondence * 0.9f;
562 
563  CPose2D P0(gaussPdf->mean);
564  CPose2D PX1(P0);
565  PX1.x_incr(-Axy);
566  CPose2D PX2(P0);
567  PX2.x_incr(+Axy);
568  CPose2D PY1(P0);
569  PY1.y_incr(-Axy);
570  CPose2D PY2(P0);
571  PY2.y_incr(+Axy);
572 
573  matchParams.angularDistPivotPoint =
574  TPoint3D(gaussPdf->mean.x(), gaussPdf->mean.y(), 0);
575  m1->determineMatching2D(
576  m2, // The other map
577  P0, // The other map pose
578  correspondences, matchParams, matchExtraResults);
579  const float E0 = matchExtraResults.correspondencesRatio;
580 
581  m1->determineMatching2D(
582  m2, // The other map
583  PX1, // The other map pose
584  correspondences, matchParams, matchExtraResults);
585  const float EX1 = matchExtraResults.correspondencesRatio;
586 
587  m1->determineMatching2D(
588  m2, // The other map
589  PX2, // The other map pose
590  correspondences, matchParams, matchExtraResults);
591  const float EX2 = matchExtraResults.correspondencesRatio;
592 
593  m1->determineMatching2D(
594  m2, // The other map
595  PY1, // The other map pose
596  correspondences, matchParams, matchExtraResults);
597  const float EY1 = matchExtraResults.correspondencesRatio;
598  m1->determineMatching2D(
599  m2, // The other map
600  PY2, // The other map pose
601  correspondences, matchParams, matchExtraResults);
602  const float EY2 = matchExtraResults.correspondencesRatio;
603 
604  outInfo.quality =
605  -max(EX1 - E0, max(EX2 - E0, max(EY1 - E0, EY2 - E0))) /
606  (E0 + 1e-1);
607  }
608 
609  } // end of "if m2 is not empty"
610 
611  // We'll return a CPosePDFGaussian or a CPosePDFSOG if RANSAC is enabled:
612  // -----------------------------------------------------------------------
613 
614  // RANSAC?
615  if (options.doRANSAC)
616  {
617  mrpt::tfest::TSE2RobustParams params;
618  params.ransac_minSetSize = options.ransac_minSetSize;
619  params.ransac_maxSetSize = options.ransac_maxSetSize;
620  params.ransac_mahalanobisDistanceThreshold =
621  options.ransac_mahalanobisDistanceThreshold;
622  params.ransac_nSimulations = options.ransac_nSimulations;
623  params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
624  params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
625  params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
626  params.ransac_algorithmForLandmarks = false;
627 
628  mrpt::tfest::TSE2RobustResult results;
629  mrpt::tfest::se2_l2_robust(
630  correspondences, options.normalizationStd, params, results);
631 
632  SOG = mrpt::make_aligned_shared<CPosePDFSOG>();
633  *SOG = results.transformation;
634 
635  // And return the SOG:
636  resultPDF = SOG;
637  }
638  else
639  {
640  // Return the gaussian distribution:
641  resultPDF = gaussPdf;
642  }
643 
644  return resultPDF;
645 
646  MRPT_END
647 }
648 
649 /*----------------------------------------------------------------------------
650 
651  ICP_Method_LM
652 
653  ----------------------------------------------------------------------------*/
654 CPosePDF::Ptr CICP::ICP_Method_LM(
655  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* m2,
656  const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
657 {
658  MRPT_START
659 
660  // The result can be either a Gaussian or a SOG:
661  size_t nCorrespondences = 0;
662  bool keepIteratingICP;
663  CPose2D grossEst = initialEstimationPDF.mean;
664  TMatchingPairList correspondences, old_correspondences;
665  CPose2D lastMeanPose;
666  std::vector<float> other_xs_trans, other_ys_trans; // temporary container
667  // of "other" map (map2)
668  // transformed by "q"
669  CMatrixFloat dJ_dq; // The jacobian
670  CPose2D q; // The updated 2D transformation estimate
671  CPose2D q_new;
672 
673  const bool onlyUniqueRobust = options.onlyUniqueRobust;
674  const bool onlyKeepTheClosest = options.onlyClosestCorrespondences;
675 
676  // Assure the class of the maps:
677  ASSERT_(mm1->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
678  const CPointsMap* m1 = static_cast<const CPointsMap*>(mm1);
679 
680  // Asserts:
681  // -----------------
682  ASSERT_(options.ALFA > 0 && options.ALFA < 1);
683 
684  // The algorithm output auxiliar info:
685  // -------------------------------------------------
686  outInfo.nIterations = 0;
687  outInfo.goodness = 1.0f;
688 
689  TMatchingParams matchParams;
690  TMatchingExtraResults matchExtraResults;
691 
692  matchParams.maxDistForCorrespondence =
693  options.thresholdDist; // Distance threshold
694  matchParams.maxAngularDistForCorrespondence =
695  options.thresholdAng; // Angular threshold
696  matchParams.angularDistPivotPoint =
697  TPoint3D(q.x(), q.y(), 0); // Pivot point for angular measurements
698  matchParams.onlyKeepTheClosest = onlyKeepTheClosest;
699  matchParams.onlyUniqueRobust = onlyUniqueRobust;
700  matchParams.decimation_other_map_points =
701  options.corresponding_points_decimation;
702 
703  // The gaussian PDF to estimate:
704  // ------------------------------------------------------
705  // First gross approximation:
706  q = grossEst;
707 
708  // For LM inverse
709  CMatrixFixedNumeric<float, 3, 3> C;
710  CMatrixFixedNumeric<float, 3, 3>
711  C_inv; // This will keep the cov. matrix at the end
712 
713  // Asure maps are not empty!
714  // ------------------------------------------------------
715  if (!m2->isEmpty())
716  {
717  matchParams.offset_other_map_points = 0;
718  // ------------------------------------------------------
719  // The ICP loop
720  // ------------------------------------------------------
721  do
722  {
723  // ------------------------------------------------------
724  // Find the matching (for a points map)
725  // ------------------------------------------------------
726  m1->determineMatching2D(
727  m2, // The other map
728  q, // The other map pose
729  correspondences, matchParams, matchExtraResults);
730 
731  nCorrespondences = correspondences.size();
732 
733  if (!nCorrespondences)
734  {
735  // Nothing we can do !!
736  keepIteratingICP = false;
737  }
738  else
739  {
740  // Compute the estimated pose through iterative least-squares:
741  // Levenberg-Marquardt
742  // ----------------------------------------------------------------------
743  dJ_dq.setSize(3, nCorrespondences); // The jacobian of the
744  // error function wrt the
745  // transformation q
746 
747  double lambda = options.LM_initial_lambda; // The LM parameter
748 
749  double ccos = cos(q.phi());
750  double csin = sin(q.phi());
751 
752  double w1, w2, w3;
753  double q1, q2, q3;
754  double A, B;
755  const double Axy =
756  options.Axy_aprox_derivatives; // For approximating the
757  // derivatives
758 
759  // Compute at once the square errors for each point with the
760  // current "q" and the transformed points:
761  std::vector<float> sq_errors;
762  correspondences.squareErrorVector(
763  q, sq_errors, other_xs_trans, other_ys_trans);
764  double OSE_initial = math::sum(sq_errors);
765 
766  // Compute "dJ_dq"
767  // ------------------------------------
768  double rho2 = square(options.kernel_rho);
769  mrpt::utils::TMatchingPairList::iterator it;
770  std::vector<float>::const_iterator other_x_trans, other_y_trans;
771  size_t i;
772 
773  for (i = 0, it = correspondences.begin(),
774  other_x_trans = other_xs_trans.begin(),
775  other_y_trans = other_ys_trans.begin();
776  i < nCorrespondences;
777  ++i, ++it, ++other_x_trans, ++other_y_trans)
778  {
779 // Jacobian: dJ_dx
780 // --------------------------------------
781 //#define ICP_DISTANCES_TO_LINE
782 
783 #ifndef ICP_DISTANCES_TO_LINE
784  w1 = *other_x_trans - Axy;
785  q1 = m1->squareDistanceToClosestCorrespondence(
786  w1, *other_y_trans);
787  q1 = kernel(q1, rho2);
788 
789  w2 = *other_x_trans;
790  q2 = m1->squareDistanceToClosestCorrespondence(
791  w2, *other_y_trans);
792  q2 = kernel(q2, rho2);
793 
794  w3 = *other_x_trans + Axy;
795  q3 = m1->squareDistanceToClosestCorrespondence(
796  w3, *other_y_trans);
797  q3 = kernel(q3, rho2);
798 #else
799  // The distance to the line that interpolates the TWO
800  // closest points:
801  float x1, y1, x2, y2, d1, d2;
802  m1->kdTreeTwoClosestPoint2D(
803  *other_x_trans, *other_y_trans, // The query
804  x1, y1, // Closest point #1
805  x2, y2, // Closest point #2
806  d1, d2);
807 
808  w1 = *other_x_trans - Axy;
809  q1 = math::closestSquareDistanceFromPointToLine(
810  w1, *other_y_trans, x1, y1, x2, y2);
811  q1 = kernel(q1, rho2);
812 
813  w2 = *other_x_trans;
814  q2 = math::closestSquareDistanceFromPointToLine(
815  w2, *other_y_trans, x1, y1, x2, y2);
816  q2 = kernel(q2, rho2);
817 
818  w3 = *other_x_trans + Axy;
819  q3 = math::closestSquareDistanceFromPointToLine(
820  w3, *other_y_trans, x1, y1, x2, y2);
821  q3 = kernel(q3, rho2);
822 #endif
823  // interpolate
824  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
825  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
826  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);
827 
828  dJ_dq.get_unsafe(0, i) = (2 * A * *other_x_trans) + B;
829 
830  // Jacobian: dJ_dy
831  // --------------------------------------
832  w1 = *other_y_trans - Axy;
833 #ifdef ICP_DISTANCES_TO_LINE
834  q1 = math::closestSquareDistanceFromPointToLine(
835  *other_x_trans, w1, x1, y1, x2, y2);
836  q1 = kernel(q1, rho2);
837 #else
838  q1 = kernel(
839  square(it->this_x - *other_x_trans) +
840  square(it->this_y - w1),
841  rho2);
842 #endif
843 
844  w2 = *other_y_trans;
845  // q2 is alreay computed from above!
846  // q2 = m1->squareDistanceToClosestCorrespondence(
847  // *other_x_trans, w2 );
848  // q2= kernel( square(it->this_x - *other_x_trans)+ square(
849  // it->this_y - w2 ), rho2 );
850 
851  w3 = *other_y_trans + Axy;
852 #ifdef ICP_DISTANCES_TO_LINE
853  q3 = math::closestSquareDistanceFromPointToLine(
854  *other_x_trans, w3, x1, y1, x2, y2);
855  q3 = kernel(q3, rho2);
856 #else
857  q3 = kernel(
858  square(it->this_x - *other_x_trans) +
859  square(it->this_y - w3),
860  rho2);
861 #endif
862 
863  // interpolate
864  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
865  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
866  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);
867 
868  dJ_dq.get_unsafe(1, i) = (2 * A * *other_y_trans) + B;
869 
870  // Jacobian: dR_dphi
871  // --------------------------------------
872  dJ_dq.get_unsafe(2, i) =
873  dJ_dq.get_unsafe(0, i) *
874  (-csin * it->other_x - ccos * it->other_y) +
875  dJ_dq.get_unsafe(1, i) *
876  (ccos * it->other_x - csin * it->other_y);
877 
878  } // end for each corresp.
879 
880  // Now we have the Jacobian in dJ_dq.
881 
882  // Compute the Hessian matrix H = dJ_dq * dJ_dq^T
883  CMatrixFloat H_(3, 3);
884  H_.multiply_AAt(dJ_dq);
885 
886  CMatrixFixedNumeric<float, 3, 3> H =
887  CMatrixFixedNumeric<float, 3, 3>(H_);
888 
889  bool keepIteratingLM = true;
890 
891  // ---------------------------------------------------
892  // Iterate the inner LM loop until convergence:
893  // ---------------------------------------------------
894  q_new = q;
895 
896  std::vector<float> new_sq_errors, new_other_xs_trans,
897  new_other_ys_trans;
898  size_t nLMiters = 0;
899  const size_t maxLMiters = 100;
900 
901  while (keepIteratingLM && ++nLMiters < maxLMiters)
902  {
903  // The LM heuristic is:
904  // x_{k+1} = x_k - ( H + \lambda diag(H) )^-1 * grad(J)
905  // grad(J) = dJ_dq * e (vector of errors)
906  C = H;
907  for (i = 0; i < 3; i++)
908  C(i, i) *=
909  (1 + lambda); // Levenberg-Maquardt heuristic
910 
911  C_inv = C.inv();
912 
913  // LM_delta = C_inv * dJ_dq * sq_errors
914  Eigen::VectorXf dJsq, LM_delta;
915  dJ_dq.multiply_Ab(
916  Eigen::Map<Eigen::VectorXf>(
917  &sq_errors[0], sq_errors.size()),
918  dJsq);
919  C_inv.multiply_Ab(dJsq, LM_delta);
920 
921  q_new.x(q.x() - LM_delta[0]);
922  q_new.y(q.y() - LM_delta[1]);
923  q_new.phi(q.phi() - LM_delta[2]);
924 
925  // Compute the new square errors:
926  // ---------------------------------------
927  correspondences.squareErrorVector(
928  q_new, new_sq_errors, new_other_xs_trans,
929  new_other_ys_trans);
930 
931  float OSE_new = math::sum(new_sq_errors);
932 
933  bool improved = OSE_new < OSE_initial;
934 
935 #if 0 // Debuggin'
936  cout << "_____________" << endl;
937  cout << "q -> q_new : " << q << " -> " << q_new << endl;
938  printf("err: %f -> %f lambda: %e\n", OSE_initial ,OSE_new, lambda );
939  cout << "\\/J = "; utils::operator <<(cout,dJsq); cout << endl;
940  mrpt::system::pause();
941 #endif
942 
943  keepIteratingLM =
944  fabs(LM_delta[0]) > options.minAbsStep_trans ||
945  fabs(LM_delta[1]) > options.minAbsStep_trans ||
946  fabs(LM_delta[2]) > options.minAbsStep_rot;
947 
948  if (improved)
949  {
950  // If resids have gone down, keep change and make lambda
951  // smaller by factor of 10
952  lambda /= 10;
953  q = q_new;
954  OSE_initial = OSE_new;
955  }
956  else
957  {
958  // Discard movement and try with larger lambda:
959  lambda *= 10;
960  }
961 
962  } // end iterative LM
963 
964 #if 0 // Debuggin'
965  cout << "ICP loop: " << lastMeanPose << " -> " << q << " LM iters: " << nLMiters << " threshold: " << matchParams.maxDistForCorrespondence << endl;
966 #endif
967  // --------------------------------------------------------
968  // now the conditions for the outer ICP loop
969  // --------------------------------------------------------
970  keepIteratingICP = true;
971  if (fabs(lastMeanPose.x() - q.x()) < options.minAbsStep_trans &&
972  fabs(lastMeanPose.y() - q.y()) < options.minAbsStep_trans &&
973  fabs(math::wrapToPi(lastMeanPose.phi() - q.phi())) <
974  options.minAbsStep_rot)
975  {
976  matchParams.maxDistForCorrespondence *= options.ALFA;
977  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
978  if (matchParams.maxDistForCorrespondence <
979  options.smallestThresholdDist)
980  keepIteratingICP = false;
981 
982  if (++matchParams.offset_other_map_points >=
983  options.corresponding_points_decimation)
984  matchParams.offset_other_map_points = 0;
985  }
986  lastMeanPose = q;
987  } // end of "else, there are correspondences"
988 
989  // Next iteration:
990  outInfo.nIterations++;
991 
992  if (outInfo.nIterations >= options.maxIterations &&
993  matchParams.maxDistForCorrespondence >
994  options.smallestThresholdDist)
995  {
996  matchParams.maxDistForCorrespondence *= options.ALFA;
997  }
998 
999  } while (
1000  (keepIteratingICP && outInfo.nIterations < options.maxIterations) ||
1001  (outInfo.nIterations >= options.maxIterations &&
1002  matchParams.maxDistForCorrespondence >
1003  options.smallestThresholdDist));
1004 
1005  outInfo.goodness = matchExtraResults.correspondencesRatio;
1006 
1007  // if (!options.skip_quality_calculation)
1008  {
1009  outInfo.quality = matchExtraResults.correspondencesRatio;
1010  }
1011 
1012  } // end of "if m2 is not empty"
1013 
1014  return CPosePDFGaussian::Ptr(new CPosePDFGaussian(q, C_inv.cast<double>()));
1015  MRPT_END
1016 }
1017 
1018 /*---------------------------------------------------------------
1019 The method for aligning a pair of 2D points map.
1020 * The meaning of some parameters are implementation dependant,
1021 * so look for derived classes for instructions.
1022 * The target is to find a PDF for the pose displacement between
1023 * maps, <b>thus the pose of m2 relative to m1</b>. This pose
1024 * is returned as a PDF rather than a single value.
1025 *
1026 * \param m1 [IN] The first map
1027 * \param m2 [IN] The second map. The pose of this map respect to m1
1028 is to be estimated.
1029 * \param grossEst [IN] An initial gross estimation for the displacement.
1030 If a given algorithm doesn't need it, set to <code>CPose2D(0,0,0)</code> for
1031 example.
1032 * \param pdf [IN/OUT] A pointer to a CPosePDF pointer, initially set
1033 to nullptr for this method to create the object. For greater efficiency, this
1034 object can be left undeleted and passed again to the method. When <b>not used
1035 anymore remember to delete it</b> using <code>delete pdf;</code>
1036 * \param runningTime [OUT] A pointer to a container for obtaining the
1037 algorithm running time in seconds, or nullptr if you don't need it.
1038 * \param info [OUT] See derived classes for details, or nullptr if it
1039 isn't needed.
1040 *
1041 * \sa CPointsMapAlignmentAlgorithm
1042  ---------------------------------------------------------------*/
1043 CPose3DPDF::Ptr CICP::Align3DPDF(
1044  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
1045  const CPose3DPDFGaussian& initialEstimationPDF, float* runningTime,
1046  void* info)
1047 {
1048  MRPT_START
1049 
1050  static CTicTac tictac;
1051  TReturnInfo outInfo;
1052  CPose3DPDF::Ptr resultPDF;
1053 
1054  if (runningTime) tictac.Tic();
1055 
1056  switch (options.ICP_algorithm)
1057  {
1058  case icpClassic:
1059  resultPDF =
1060  ICP3D_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
1061  break;
1062  case icpLevenbergMarquardt:
1063  THROW_EXCEPTION("Only icpClassic is implemented for ICP-3D")
1064  break;
1065  default:
1066  THROW_EXCEPTION_FMT(
1067  "Invalid value for ICP_algorithm: %i",
1068  static_cast<int>(options.ICP_algorithm));
1069  } // end switch
1070 
1071  if (runningTime) *runningTime = tictac.Tac();
1072 
1073  // Copy the output info if requested:
1074  if (info)
1075  {
1076  MRPT_TODO(
1077  "Refactor `info` so it is polymorphic and can use dynamic_cast<> "
1078  "here");
1079  *static_cast<TReturnInfo*>(info) = outInfo;
1080  }
1081 
1082  return resultPDF;
1083 
1084  MRPT_END
1085 }
1086 
1087 CPose3DPDF::Ptr CICP::ICP3D_Method_Classic(
1088  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
1089  const CPose3DPDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
1090 {
1091  MRPT_START
1092 
1093  // The result can be either a Gaussian or a SOG:
1094  CPose3DPDF::Ptr resultPDF;
1095  CPose3DPDFGaussian::Ptr gaussPdf;
1096 
1097  size_t nCorrespondences = 0;
1098  bool keepApproaching;
1099  CPose3D grossEst = initialEstimationPDF.mean;
1100  mrpt::utils::TMatchingPairList correspondences, old_correspondences;
1101  CPose3D lastMeanPose;
1102 
1103  // Assure the class of the maps:
1104  ASSERT_(mm2->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
1105  const CPointsMap* m2 = (CPointsMap*)mm2;
1106 
1107  // Asserts:
1108  // -----------------
1109  ASSERT_(options.ALFA > 0 && options.ALFA < 1);
1110 
1111  // The algorithm output auxiliar info:
1112  // -------------------------------------------------
1113  outInfo.nIterations = 0;
1114  outInfo.goodness = 1;
1115  outInfo.quality = 0;
1116 
1117  // The gaussian PDF to estimate:
1118  // ------------------------------------------------------
1119  gaussPdf = mrpt::make_aligned_shared<CPose3DPDFGaussian>();
1120 
1121  // First gross approximation:
1122  gaussPdf->mean = grossEst;
1123 
1124  // Initial thresholds:
1125  TMatchingParams matchParams;
1126  TMatchingExtraResults matchExtraResults;
1127 
1128  matchParams.maxDistForCorrespondence =
1129  options.thresholdDist; // Distance threshold
1130  matchParams.maxAngularDistForCorrespondence =
1131  options.thresholdAng; // Angular threshold
1132  matchParams.onlyKeepTheClosest = options.onlyClosestCorrespondences;
1133  matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
1134  matchParams.decimation_other_map_points =
1135  options.corresponding_points_decimation;
1136 
1137  // Asure maps are not empty!
1138  // ------------------------------------------------------
1139  if (!m2->isEmpty())
1140  {
1141  matchParams.offset_other_map_points = 0;
1142 
1143  // ------------------------------------------------------
1144  // The ICP loop
1145  // ------------------------------------------------------
1146  do
1147  {
1148  matchParams.angularDistPivotPoint = TPoint3D(
1149  gaussPdf->mean.x(), gaussPdf->mean.y(), gaussPdf->mean.z());
1150 
1151  // ------------------------------------------------------
1152  // Find the matching (for a points map)
1153  // ------------------------------------------------------
1154  m1->determineMatching3D(
1155  m2, // The other map
1156  gaussPdf->mean, // The other map pose
1157  correspondences, matchParams, matchExtraResults);
1158 
1159  nCorrespondences = correspondences.size();
1160 
1161  if (!nCorrespondences)
1162  {
1163  // Nothing we can do !!
1164  keepApproaching = false;
1165  }
1166  else
1167  {
1168  // Compute the estimated pose, using Horn's method.
1169  // ----------------------------------------------------------------------
1170  mrpt::poses::CPose3DQuat estPoseQuat;
1171  double transf_scale;
1172  mrpt::tfest::se3_l2(
1173  correspondences, estPoseQuat, transf_scale,
1174  false /* dont force unit scale */);
1175  gaussPdf->mean = mrpt::poses::CPose3D(estPoseQuat);
1176 
1177  // If matching has not changed, decrease the thresholds:
1178  // --------------------------------------------------------
1179  keepApproaching = true;
1180  if (!(fabs(lastMeanPose.x() - gaussPdf->mean.x()) >
1181  options.minAbsStep_trans ||
1182  fabs(lastMeanPose.y() - gaussPdf->mean.y()) >
1183  options.minAbsStep_trans ||
1184  fabs(lastMeanPose.z() - gaussPdf->mean.z()) >
1185  options.minAbsStep_trans ||
1186  fabs(
1187  math::wrapToPi(
1188  lastMeanPose.yaw() - gaussPdf->mean.yaw())) >
1189  options.minAbsStep_rot ||
1190  fabs(
1191  math::wrapToPi(
1192  lastMeanPose.pitch() - gaussPdf->mean.pitch())) >
1193  options.minAbsStep_rot ||
1194  fabs(
1195  math::wrapToPi(
1196  lastMeanPose.roll() - gaussPdf->mean.roll())) >
1197  options.minAbsStep_rot))
1198  {
1199  matchParams.maxDistForCorrespondence *= options.ALFA;
1200  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
1201  if (matchParams.maxDistForCorrespondence <
1202  options.smallestThresholdDist)
1203  keepApproaching = false;
1204 
1205  if (++matchParams.offset_other_map_points >=
1206  options.corresponding_points_decimation)
1207  matchParams.offset_other_map_points = 0;
1208  }
1209 
1210  lastMeanPose = gaussPdf->mean;
1211 
1212  } // end of "else, there are correspondences"
1213 
1214  // Next iteration:
1215  outInfo.nIterations++;
1216 
1217  if (outInfo.nIterations >= options.maxIterations &&
1218  matchParams.maxDistForCorrespondence >
1219  options.smallestThresholdDist)
1220  {
1221  matchParams.maxDistForCorrespondence *= options.ALFA;
1222  }
1223 
1224  } while (
1225  (keepApproaching && outInfo.nIterations < options.maxIterations) ||
1226  (outInfo.nIterations >= options.maxIterations &&
1227  matchParams.maxDistForCorrespondence >
1228  options.smallestThresholdDist));
1229 
1230  // -------------------------------------------------
1231  // Obtain the covariance matrix of the estimation
1232  // -------------------------------------------------
1233  if (!options.skip_cov_calculation && nCorrespondences)
1234  {
1235  // ...
1236  }
1237 
1238  //
1239  outInfo.goodness = matchExtraResults.correspondencesRatio;
1240 
1241  } // end of "if m2 is not empty"
1242 
1243  // Return the gaussian distribution:
1244  resultPDF = gaussPdf;
1245 
1246  return resultPDF;
1247 
1248  MRPT_END
1249 }
mrpt::slam::icpCovLinealMSE
Use the covariance of the optimal registration, disregarding uncertainty in data association.
Definition: CICP.h:34
mrpt::maps::TMatchingParams::angularDistPivotPoint
mrpt::math::TPoint3D angularDistPivotPoint
The point used to calculate angular distances: e.g.
Definition: metric_map_types.h:44
mrpt::maps::TMatchingParams::maxAngularDistForCorrespondence
float maxAngularDistForCorrespondence
Allowed "angular error" (in radians): this permits larger pairing threshold distances to more distant...
Definition: metric_map_types.h:28
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:135
mrpt::slam::CICP::TReturnInfo::quality
float quality
A measure of the &#39;quality&#39; of the local minimum of the sqr.
Definition: CICP.h:206
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:46
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:79
normalizationStd
const float normalizationStd
Definition: se3_ransac_unittest.cpp:37
mrpt::slam::CICP::TConfigParams::ransac_mahalanobisDistanceThreshold
float ransac_mahalanobisDistanceThreshold
Definition: CICP.h:146
mrpt::slam::CICP::kernel
float kernel(const float &x2, const float &rho2)
Computes: or just return the input if options.useKernel = false.
Definition: CICP.cpp:272
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:201
mrpt::poses::CPoseOrPoint::y_incr
void y_incr(const double v)
Definition: CPoseOrPoint.h:166
mrpt::utils::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:106
mrpt::slam::CICP::TConfigParams::thresholdDist
float thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:122
mrpt::poses::CPose3DPDFGaussian::Ptr
std::shared_ptr< CPose3DPDFGaussian > Ptr
Definition: CPose3DPDFGaussian.h:42
mrpt::maps::TMatchingParams::decimation_other_map_points
size_t decimation_other_map_points
(Default=1) Only consider 1 out of this number of points from the "other" map.
Definition: metric_map_types.h:38
mrpt::math::closestSquareDistanceFromPointToLine
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:103
mrpt::slam::CICP::TConfigParams::ICP_algorithm
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:89
mrpt::tfest::TSE2RobustParams
Parameters for se2_l2_robust().
Definition: se2.h:74
mrpt::maps::CMetricMap::determineMatching2D
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matching between this and another 2D point map, which includes finding: ...
Definition: CMetricMap.cpp:127
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: CColouredPointsMap.h:122
mrpt::utils::TRuntimeClassId::derivedFrom
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:32
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
ops_containers.h
This file implements several operations that operate element-wise on individual or pairs of container...
ransac_mahalanobisDistanceThreshold
const float ransac_mahalanobisDistanceThreshold
Definition: se3_ransac_unittest.cpp:38
CICP.h
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:539
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:533
mrpt::tfest::se2_l2
bool se2_l2(const mrpt::utils::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
std
STL namespace.
mrpt::slam::CICP::TConfigParams::skip_quality_calculation
bool skip_quality_calculation
Skip the (sometimes) expensive evaluation of the term &#39;quality&#39; at ICP output (Default=true) ...
Definition: CICP.h:173
mrpt::tfest::se2_l2_robust
bool se2_l2_robust(const mrpt::utils::TMatchingPairList &in_correspondences, const double in_normalizationStd, const TSE2RobustParams &in_ransac_params, TSE2RobustResult &out_results)
Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference ...
Definition: se2_l2_ransac.cpp:78
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::slam::CICP::TConfigParams::ransac_nSimulations
unsigned int ransac_nSimulations
Definition: CICP.h:145
mrpt::slam::CICP::TConfigParams::ALFA
float ALFA
The scale factor for threshold everytime convergence is achieved.
Definition: CICP.h:124
mrpt::slam::CICP::options
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:185
mrpt::slam::CICP::TConfigParams::kernel_rho
float kernel_rho
Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0...
Definition: CICP.h:157
mrpt::slam::CICP::ICP3D_Method_Classic
mrpt::poses::CPose3DPDF::Ptr ICP3D_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:1087
mrpt::slam::CICP::TConfigParams::minAbsStep_rot
float minAbsStep_rot
If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians)...
Definition: CICP.h:117
mrpt::slam::CICP::TConfigParams::corresponding_points_decimation
uint32_t corresponding_points_decimation
Decimation of the point cloud being registered against the reference one (default=5) - set to 1 to ha...
Definition: CICP.h:181
MRPT_TODO
#define MRPT_TODO(x)
Definition: CColouredPointsMap.h:83
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:39
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:55
mrpt::maps::CMetricMap::determineMatching3D
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::utils::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:142
CStream.h
mrpt::slam::CICP::TReturnInfo::goodness
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:202
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:41
mrpt::maps::CPointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:63
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:44
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFGaussian.h:31
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
Definition: metric_map_types.h:62
MRPT_END
#define MRPT_END
Definition: CColouredPointsMap.h:430
mrpt::utils::TEnumType
A helper class that can convert an enum value into its textual representation, and viceversa...
Definition: TEnumType.h:38
mrpt::utils::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:93
mrpt::slam::CICP::TConfigParams::maxIterations
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:109
mrpt::utils::CTicTac
This class implements a high-performance stopwatch.
Definition: CTicTac.h:23
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
mrpt::poses::CPoseOrPoint::x_incr
void x_incr(const double v)
Definition: CPoseOrPoint.h:162
CPose3DPDF.h
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:45
mrpt::slam::CICP::TConfigParams::covariance_varPoints
float covariance_varPoints
This is the normalization constant that is used to scale the whole 3x3 covariance.
Definition: CICP.h:136
mrpt::tfest::se3_l2
bool se3_l2(const mrpt::utils::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:223
mrpt::maps::TMatchingParams::onlyUniqueRobust
bool onlyUniqueRobust
Additional consistency filter: "onlyKeepTheClosest" allows one correspondence for each "local map" po...
Definition: metric_map_types.h:35
mrpt::maps::TMatchingParams::maxDistForCorrespondence
float maxDistForCorrespondence
Maximum linear distance between two points to be paired (meters)
Definition: metric_map_types.h:25
mrpt::slam::CICP::TConfigParams::ICP_covariance_method
TICPCovarianceMethod ICP_covariance_method
The method to use for covariance estimation (Default: icpCovFiniteDifferences)
Definition: CICP.h:92
mrpt::poses::CPosePDFGaussian::Ptr
std::shared_ptr< CPosePDFGaussian > Ptr
Definition: CPosePDFGaussian.h:33
mrpt::slam::CICP::TConfigParams::doRANSAC
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:140
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
mrpt::slam::CICP::ICP_Method_LM
mrpt::poses::CPosePDF::Ptr ICP_Method_LM(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:654
mrpt::poses::CPosePDFSOG::Ptr
std::shared_ptr< CPosePDFSOG > Ptr
Definition: CPosePDFSOG.h:38
CConfigFileBase.h
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
mrpt::slam::CICP::TConfigParams::onlyClosestCorrespondences
bool onlyClosestCorrespondences
The usual approach: to consider only the closest correspondence for each local point (Default to true...
Definition: CICP.h:99
mrpt::slam::CICP::TReturnInfo::nIterations
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:199
mrpt::slam::CICP::TConfigParams::use_kernel
bool use_kernel
Whether to use kernel_rho to smooth distances, or use distances directly (default=true) ...
Definition: CICP.h:160
mrpt::utils::operator<<
CStream & operator<<(mrpt::utils::CStream &s, const char *a)
Definition: CStream.cpp:123
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:545
mrpt::utils::CConfigFileBase::read_enum
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
Definition: CConfigFileBase.h:242
mrpt::slam::TICPAlgorithm
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp.
Definition: CICP.h:22
CPose3DPDFGaussian.h
mrpt::system::pause
void pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:427
CTicTac.h
mrpt::utils::TMatchingPairList::squareErrorVector
void squareErrorVector(const mrpt::poses::CPose2D &q, std::vector< float > &out_sqErrs) const
Returns a vector with the square error between each pair of correspondences in the list...
Definition: TMatchingPair.cpp:143
MRPT_START
#define MRPT_START
Definition: CColouredPointsMap.h:426
CPosePDFGaussian.h
RAD2DEG
#define RAD2DEG
Definition: CColouredPointsMap.h:115
mrpt::maps::TMatchingParams::onlyKeepTheClosest
bool onlyKeepTheClosest
If set to true (default), only the closest correspondence will be returned.
Definition: metric_map_types.h:31
mrpt::slam::CICP::TConfigParams::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CICP.cpp:192
w2
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:5566
mrpt::maps::TMatchingParams::offset_other_map_points
size_t offset_other_map_points
Index of the first point in the "other" map to start checking for correspondences (Default=0) ...
Definition: metric_map_types.h:41
mrpt::slam::CICP::TConfigParams::skip_cov_calculation
bool skip_cov_calculation
Skip the computation of the covariance (saves some time) (default=false)
Definition: CICP.h:170
mrpt::maps::TMatchingExtraResults::correspondencesRatio
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
Definition: metric_map_types.h:66
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:55
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
mrpt::slam::CICP::TConfigParams::TConfigParams
TConfigParams()
Initializer for default values:
Definition: CICP.cpp:99
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::slam::CICP::TConfigParams::smallestThresholdDist
float smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:127
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:91
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:195
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:194
CPosePDFSOG.h
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
mrpt::slam::CICP::TConfigParams::ransac_minSetSize
unsigned int ransac_minSetSize
Definition: CICP.h:145
mrpt::tfest::TSE2RobustResult::transformation
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:148
mrpt::slam::CICP::AlignPDF
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)
An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/...
Definition: CICP.cpp:58
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:40
mrpt::slam::CICP::TConfigParams::LM_initial_lambda
float LM_initial_lambda
[LM method only] The initial value of the lambda parameter in the LM method (default=1e-4) ...
Definition: CICP.h:166
mrpt::slam::CICP::TConfigParams::normalizationStd
float normalizationStd
RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used t...
Definition: CICP.h:150
CPosePDF.h
mrpt::slam::CICP::TConfigParams::minAbsStep_trans
float minAbsStep_trans
If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters)...
Definition: CICP.h:113
mrpt::slam::CICP::TConfigParams::ransac_maxSetSize
unsigned int ransac_maxSetSize
Definition: CICP.h:145
MRPT_LOAD_CONFIG_VAR
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
Definition: CConfigFileBase.h:279
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:381
mrpt::slam::TICPCovarianceMethod
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options.
Definition: CICP.h:30
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:22
mrpt::slam::CICP::TConfigParams::Axy_aprox_derivatives
float Axy_aprox_derivatives
[LM method only] The size of the perturbance in x & y used to estimate the Jacobians of the square er...
Definition: CICP.h:163
CPose2D.h
mrpt::maps::CMetricMap::isEmpty
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::tfest::TSE2RobustResult
Output placeholder for se2_l2_robust()
Definition: se2.h:145
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: lightweight_geom_data.h:203
mrpt::slam::CICP::Align3DPDF
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)
The virtual method for aligning a pair of metric maps, aligning the full 6D pose. ...
Definition: CICP.cpp:1043
mrpt::utils::CStream::printf
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:597
mrpt::slam::CICP::ICP_Method_Classic
mrpt::poses::CPosePDF::Ptr ICP_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:282
mrpt::slam::icpCovFiniteDifferences
Covariance of a least-squares optimizer (includes data association uncertainty)
Definition: CICP.h:37
w1
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:5566
mrpt::slam::CICP::TConfigParams::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::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:140


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019