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-2018, 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/system/CTicTac.h>
16 #include <mrpt/serialization/CArchive.h>
17 #include <mrpt/config/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::tfest;
30 using namespace mrpt::system;
31 using namespace mrpt::poses;
32 using namespace std;
33 
34 CPosePDF::Ptr CICP::AlignPDF(
35  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
36  const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
37  void* info)
38 {
39  MRPT_START
40 
41  CTicTac tictac;
42  TReturnInfo outInfo;
43  CPosePDF::Ptr resultPDF;
44 
45  if (runningTime) tictac.Tic();
46 
47  switch (options.ICP_algorithm)
48  {
49  case icpClassic:
50  resultPDF =
51  ICP_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
52  break;
53  case icpLevenbergMarquardt:
54  resultPDF = ICP_Method_LM(m1, mm2, initialEstimationPDF, outInfo);
55  break;
56  default:
57  THROW_EXCEPTION_FMT(
58  "Invalid value for ICP_algorithm: %i",
59  static_cast<int>(options.ICP_algorithm));
60  } // end switch
61 
62  if (runningTime) *runningTime = tictac.Tac();
63 
64  // Copy the output info if requested:
65  if (info) *static_cast<TReturnInfo*>(info) = outInfo;
66 
67  return resultPDF;
68 
69  MRPT_END
70 }
71 
72 /*---------------------------------------------------------------
73  loadFromConfigFile
74  ---------------------------------------------------------------*/
75 void CICP::TConfigParams::loadFromConfigFile(
76  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
77 {
78  MRPT_LOAD_CONFIG_VAR(maxIterations, int, iniFile, section);
79  MRPT_LOAD_CONFIG_VAR(minAbsStep_trans, float, iniFile, section);
80  MRPT_LOAD_CONFIG_VAR(minAbsStep_rot, float, iniFile, section);
81 
82  ICP_algorithm = iniFile.read_enum<TICPAlgorithm>(
83  section, "ICP_algorithm", ICP_algorithm);
84  ICP_covariance_method = iniFile.read_enum<TICPCovarianceMethod>(
85  section, "ICP_covariance_method", ICP_covariance_method);
86 
87  MRPT_LOAD_CONFIG_VAR(thresholdDist, float, iniFile, section);
88  thresholdAng = DEG2RAD(
89  iniFile.read_float(
90  section.c_str(), "thresholdAng_DEG", RAD2DEG(thresholdAng)));
91 
92  MRPT_LOAD_CONFIG_VAR(ALFA, float, iniFile, section);
93  MRPT_LOAD_CONFIG_VAR(smallestThresholdDist, float, iniFile, section);
94  MRPT_LOAD_CONFIG_VAR(onlyUniqueRobust, bool, iniFile, section);
95  MRPT_LOAD_CONFIG_VAR(doRANSAC, bool, iniFile, section);
96  MRPT_LOAD_CONFIG_VAR(covariance_varPoints, float, iniFile, section);
97 
98  MRPT_LOAD_CONFIG_VAR(ransac_minSetSize, int, iniFile, section);
99  MRPT_LOAD_CONFIG_VAR(ransac_maxSetSize, int, iniFile, section);
100  MRPT_LOAD_CONFIG_VAR(
101  ransac_mahalanobisDistanceThreshold, float, iniFile, section);
102  MRPT_LOAD_CONFIG_VAR(ransac_nSimulations, int, iniFile, section);
103  MRPT_LOAD_CONFIG_VAR(normalizationStd, float, iniFile, section);
104  MRPT_LOAD_CONFIG_VAR(ransac_fuseByCorrsMatch, bool, iniFile, section);
105  MRPT_LOAD_CONFIG_VAR(ransac_fuseMaxDiffXY, float, iniFile, section);
106  ransac_fuseMaxDiffPhi = DEG2RAD(
107  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 
119  MRPT_LOAD_CONFIG_VAR(
120  corresponding_points_decimation, int, iniFile, section);
121 }
122 
123 void CICP::TConfigParams::saveToConfigFile(
124  mrpt::config::CConfigFileBase& c, const std::string& s) const
125 {
126  MRPT_SAVE_CONFIG_VAR_COMMENT(
127  ICP_algorithm, "The ICP algorithm to use (see enum TICPAlgorithm)");
128  MRPT_SAVE_CONFIG_VAR_COMMENT(
129  ICP_covariance_method,
130  "Method to use for covariance estimation (see enum "
131  "TICPCovarianceMethod)");
132  MRPT_SAVE_CONFIG_VAR_COMMENT(
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");
139  MRPT_SAVE_CONFIG_VAR_COMMENT(
140  thresholdDist,
141  "Initial threshold distance for two points to be a match");
142  MRPT_SAVE_CONFIG_VAR_COMMENT(
143  thresholdAng,
144  "Initial threshold distance for two points to be a match");
145  MRPT_SAVE_CONFIG_VAR_COMMENT(
146  ALFA,
147  "The scale factor for thresholds everytime convergence is achieved");
148  MRPT_SAVE_CONFIG_VAR_COMMENT(
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, "");
157  MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_mahalanobisDistanceThreshold, "");
158  MRPT_SAVE_CONFIG_VAR_COMMENT(normalizationStd, "");
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  ----------------------------------------------------------------------------*/
181 CPosePDF::Ptr CICP::ICP_Method_Classic(
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 = mrpt::make_aligned_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  // Asure 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 
250  m1->determineMatching2D(
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(
283  math::wrapToPi(
284  lastMeanPose.phi() - gaussPdf->mean.phi())) >
285  options.minAbsStep_rot))
286  {
287  matchParams.maxDistForCorrespondence *= options.ALFA;
288  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
289  if (matchParams.maxDistForCorrespondence <
290  options.smallestThresholdDist)
291  keepApproaching = false;
292 
293  if (++matchParams.offset_other_map_points >=
294  options.corresponding_points_decimation)
295  matchParams.offset_other_map_points = 0;
296  }
297 
298  lastMeanPose = gaussPdf->mean;
299 
300  } // end of "else, there are correspondences"
301 
302  // Next iteration:
303  outInfo.nIterations++;
304 
305  if (outInfo.nIterations >= options.maxIterations &&
306  matchParams.maxDistForCorrespondence >
307  options.smallestThresholdDist)
308  {
309  matchParams.maxDistForCorrespondence *= options.ALFA;
310  }
311 
312  } while (
313  (keepApproaching && outInfo.nIterations < options.maxIterations) ||
314  (outInfo.nIterations >= options.maxIterations &&
315  matchParams.maxDistForCorrespondence >
316  options.smallestThresholdDist));
317 
318  // -------------------------------------------------
319  // Obtain the covariance matrix of the estimation
320  // -------------------------------------------------
321  if (!options.skip_cov_calculation && nCorrespondences)
322  {
323  switch (options.ICP_covariance_method)
324  {
325  // ----------------------------------------------
326  // METHOD: MSE linear estimation
327  // ----------------------------------------------
328  case icpCovLinealMSE:
329  mrpt::tfest::se2_l2(correspondences, *gaussPdf);
330  // Scale covariance:
331  gaussPdf->cov *= options.covariance_varPoints;
332  break;
333 
334  // ----------------------------------------------
335  // METHOD: Method from Oxford MRG's "OXSMV2"
336  //
337  // It is the equivalent covariance resulting
338  // from a Levenberg-Maquardt optimization stage.
339  // ----------------------------------------------
340  case icpCovFiniteDifferences:
341  {
342  Eigen::Matrix<double, 3, Eigen::Dynamic> D(
343  3, nCorrespondences);
344 
345  const TPose2D transf = gaussPdf->mean.asTPose();
346 
347  double ccos = cos(transf.phi);
348  double csin = sin(transf.phi);
349 
350  double w1, w2, w3;
351  double q1, q2, q3;
352  double A, B;
353  double Axy = 0.01;
354 
355  // Fill out D:
356  double rho2 = square(options.kernel_rho);
357  mrpt::tfest::TMatchingPairList::iterator it;
358  size_t i;
359  for (i = 0, it = correspondences.begin();
360  i < nCorrespondences; ++i, ++it)
361  {
362  float other_x_trans =
363  transf.x + ccos * it->other_x - csin * it->other_y;
364  float other_y_trans =
365  transf.y + csin * it->other_x + ccos * it->other_y;
366 
367  // Jacobian: dR2_dx
368  // --------------------------------------
369  w1 = other_x_trans - Axy;
370  q1 = kernel(
371  square(it->this_x - w1) +
372  square(it->this_y - other_y_trans),
373  rho2);
374 
375  w2 = other_x_trans;
376  q2 = kernel(
377  square(it->this_x - w2) +
378  square(it->this_y - other_y_trans),
379  rho2);
380 
381  w3 = other_x_trans + Axy;
382  q3 = kernel(
383  square(it->this_x - w3) +
384  square(it->this_y - other_y_trans),
385  rho2);
386 
387  // interpolate
388  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
389  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
390  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
391  (w1 - w2);
392 
393  D(0, i) = (2 * A * other_x_trans) + B;
394 
395  // Jacobian: dR2_dy
396  // --------------------------------------
397  w1 = other_y_trans - Axy;
398  q1 = kernel(
399  square(it->this_x - other_x_trans) +
400  square(it->this_y - w1),
401  rho2);
402 
403  w2 = other_y_trans;
404  q2 = kernel(
405  square(it->this_x - other_x_trans) +
406  square(it->this_y - w2),
407  rho2);
408 
409  w3 = other_y_trans + Axy;
410  q3 = kernel(
411  square(it->this_x - other_x_trans) +
412  square(it->this_y - w3),
413  rho2);
414 
415  // interpolate
416  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
417  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
418  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
419  (w1 - w2);
420 
421  D(1, i) = (2 * A * other_y_trans) + B;
422 
423  // Jacobian: dR_dphi
424  // --------------------------------------
425  D(2, i) =
426  D(0, i) *
427  (-csin * it->other_x - ccos * it->other_y) +
428  D(1, i) * (ccos * it->other_x - csin * it->other_y);
429 
430  } // end for each corresp.
431 
432  // COV = ( D*D^T + lamba*I )^-1
433  CMatrixDouble33 DDt = D * D.transpose();
434 
435  for (i = 0; i < 3; i++)
436  DDt(i, i) += 1e-6; // Just to make sure the matrix is
437  // not singular, while not changing
438  // its covariance significantly.
439 
440  DDt.inv(gaussPdf->cov);
441  }
442  break;
443  default:
444  THROW_EXCEPTION_FMT(
445  "Invalid value for ICP_covariance_method: %i",
446  static_cast<int>(options.ICP_covariance_method));
447  }
448  }
449 
450  outInfo.goodness = matchExtraResults.correspondencesRatio;
451 
452  if (!nCorrespondences || options.skip_quality_calculation)
453  {
454  outInfo.quality = matchExtraResults.correspondencesRatio;
455  }
456  else
457  {
458  // Compute a crude estimate of the quality of scan matching at this
459  // local minimum:
460  // -----------------------------------------------------------------------------------
461  float Axy = matchParams.maxDistForCorrespondence * 0.9f;
462 
463  CPose2D P0(gaussPdf->mean);
464  CPose2D PX1(P0);
465  PX1.x_incr(-Axy);
466  CPose2D PX2(P0);
467  PX2.x_incr(+Axy);
468  CPose2D PY1(P0);
469  PY1.y_incr(-Axy);
470  CPose2D PY2(P0);
471  PY2.y_incr(+Axy);
472 
473  matchParams.angularDistPivotPoint =
474  TPoint3D(gaussPdf->mean.x(), gaussPdf->mean.y(), 0);
475  m1->determineMatching2D(
476  m2, // The other map
477  P0, // The other map pose
478  correspondences, matchParams, matchExtraResults);
479  const float E0 = matchExtraResults.correspondencesRatio;
480 
481  m1->determineMatching2D(
482  m2, // The other map
483  PX1, // The other map pose
484  correspondences, matchParams, matchExtraResults);
485  const float EX1 = matchExtraResults.correspondencesRatio;
486 
487  m1->determineMatching2D(
488  m2, // The other map
489  PX2, // The other map pose
490  correspondences, matchParams, matchExtraResults);
491  const float EX2 = matchExtraResults.correspondencesRatio;
492 
493  m1->determineMatching2D(
494  m2, // The other map
495  PY1, // The other map pose
496  correspondences, matchParams, matchExtraResults);
497  const float EY1 = matchExtraResults.correspondencesRatio;
498  m1->determineMatching2D(
499  m2, // The other map
500  PY2, // The other map pose
501  correspondences, matchParams, matchExtraResults);
502  const float EY2 = matchExtraResults.correspondencesRatio;
503 
504  outInfo.quality =
505  -max(EX1 - E0, max(EX2 - E0, max(EY1 - E0, EY2 - E0))) /
506  (E0 + 1e-1);
507  }
508 
509  } // end of "if m2 is not empty"
510 
511  // We'll return a CPosePDFGaussian or a CPosePDFSOG if RANSAC is enabled:
512  // -----------------------------------------------------------------------
513 
514  // RANSAC?
515  if (options.doRANSAC)
516  {
517  mrpt::tfest::TSE2RobustParams params;
518  params.ransac_minSetSize = options.ransac_minSetSize;
519  params.ransac_maxSetSize = options.ransac_maxSetSize;
520  params.ransac_mahalanobisDistanceThreshold =
521  options.ransac_mahalanobisDistanceThreshold;
522  params.ransac_nSimulations = options.ransac_nSimulations;
523  params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
524  params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
525  params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
526  params.ransac_algorithmForLandmarks = false;
527 
528  mrpt::tfest::TSE2RobustResult results;
529  mrpt::tfest::se2_l2_robust(
530  correspondences, options.normalizationStd, params, results);
531 
532  SOG = mrpt::make_aligned_shared<CPosePDFSOG>();
533  *SOG = results.transformation;
534 
535  // And return the SOG:
536  resultPDF = SOG;
537  }
538  else
539  {
540  // Return the gaussian distribution:
541  resultPDF = gaussPdf;
542  }
543 
544  return resultPDF;
545 
546  MRPT_END
547 }
548 
549 /*----------------------------------------------------------------------------
550 
551  ICP_Method_LM
552 
553  ----------------------------------------------------------------------------*/
554 CPosePDF::Ptr CICP::ICP_Method_LM(
555  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* m2,
556  const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
557 {
558  MRPT_START
559 
560  // The result can be either a Gaussian or a SOG:
561  size_t nCorrespondences = 0;
562  bool keepIteratingICP;
563  CPose2D grossEst = initialEstimationPDF.mean;
564  TMatchingPairList correspondences, old_correspondences;
565  CPose2D lastMeanPose;
566  std::vector<float> other_xs_trans, other_ys_trans; // temporary container
567  // of "other" map (map2)
568  // transformed by "q"
569  CMatrixFloat dJ_dq; // The jacobian
570  CPose2D q; // The updated 2D transformation estimate
571  CPose2D q_new;
572 
573  const bool onlyUniqueRobust = options.onlyUniqueRobust;
574 
575  // Assure the class of the maps:
576  ASSERT_(mm1->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
577  const CPointsMap* m1 = static_cast<const CPointsMap*>(mm1);
578 
579  // Asserts:
580  // -----------------
581  ASSERT_(options.ALFA > 0 && options.ALFA < 1);
582 
583  // The algorithm output auxiliar info:
584  // -------------------------------------------------
585  outInfo.nIterations = 0;
586  outInfo.goodness = 1.0f;
587 
588  TMatchingParams matchParams;
589  TMatchingExtraResults matchExtraResults;
590 
591  matchParams.maxDistForCorrespondence =
592  options.thresholdDist; // Distance threshold
593  matchParams.maxAngularDistForCorrespondence =
594  options.thresholdAng; // Angular threshold
595  matchParams.angularDistPivotPoint =
596  TPoint3D(q.x(), q.y(), 0); // Pivot point for angular measurements
597  matchParams.onlyKeepTheClosest = true;
598  matchParams.onlyUniqueRobust = onlyUniqueRobust;
599  matchParams.decimation_other_map_points =
600  options.corresponding_points_decimation;
601 
602  // The gaussian PDF to estimate:
603  // ------------------------------------------------------
604  // First gross approximation:
605  q = grossEst;
606 
607  // For LM inverse
608  CMatrixFixedNumeric<float, 3, 3> C;
609  CMatrixFixedNumeric<float, 3, 3>
610  C_inv; // This will keep the cov. matrix at the end
611 
612  // Asure maps are not empty!
613  // ------------------------------------------------------
614  if (!m2->isEmpty())
615  {
616  matchParams.offset_other_map_points = 0;
617  // ------------------------------------------------------
618  // The ICP loop
619  // ------------------------------------------------------
620  do
621  {
622  // ------------------------------------------------------
623  // Find the matching (for a points map)
624  // ------------------------------------------------------
625  m1->determineMatching2D(
626  m2, // The other map
627  q, // The other map pose
628  correspondences, matchParams, matchExtraResults);
629 
630  nCorrespondences = correspondences.size();
631 
632  if (!nCorrespondences)
633  {
634  // Nothing we can do !!
635  keepIteratingICP = false;
636  }
637  else
638  {
639  // Compute the estimated pose through iterative least-squares:
640  // Levenberg-Marquardt
641  // ----------------------------------------------------------------------
642  dJ_dq.setSize(3, nCorrespondences); // The jacobian of the
643  // error function wrt the
644  // transformation q
645 
646  double lambda = options.LM_initial_lambda; // The LM parameter
647 
648  double ccos = cos(q.phi());
649  double csin = sin(q.phi());
650 
651  double w1, w2, w3;
652  double q1, q2, q3;
653  double A, B;
654  const double Axy =
655  options.Axy_aprox_derivatives; // For approximating the
656  // derivatives
657 
658  // Compute at once the square errors for each point with the
659  // current "q" and the transformed points:
660  std::vector<float> sq_errors;
661  correspondences.squareErrorVector(
662  q, sq_errors, other_xs_trans, other_ys_trans);
663  double OSE_initial = math::sum(sq_errors);
664 
665  // Compute "dJ_dq"
666  // ------------------------------------
667  double rho2 = square(options.kernel_rho);
668  mrpt::tfest::TMatchingPairList::iterator it;
669  std::vector<float>::const_iterator other_x_trans, other_y_trans;
670  size_t i;
671 
672  for (i = 0, it = correspondences.begin(),
673  other_x_trans = other_xs_trans.begin(),
674  other_y_trans = other_ys_trans.begin();
675  i < nCorrespondences;
676  ++i, ++it, ++other_x_trans, ++other_y_trans)
677  {
678 // Jacobian: dJ_dx
679 // --------------------------------------
680 //#define ICP_DISTANCES_TO_LINE
681 
682 #ifndef ICP_DISTANCES_TO_LINE
683  w1 = *other_x_trans - Axy;
684  q1 = m1->squareDistanceToClosestCorrespondence(
685  w1, *other_y_trans);
686  q1 = kernel(q1, rho2);
687 
688  w2 = *other_x_trans;
689  q2 = m1->squareDistanceToClosestCorrespondence(
690  w2, *other_y_trans);
691  q2 = kernel(q2, rho2);
692 
693  w3 = *other_x_trans + Axy;
694  q3 = m1->squareDistanceToClosestCorrespondence(
695  w3, *other_y_trans);
696  q3 = kernel(q3, rho2);
697 #else
698  // The distance to the line that interpolates the TWO
699  // closest points:
700  float x1, y1, x2, y2, d1, d2;
701  m1->kdTreeTwoClosestPoint2D(
702  *other_x_trans, *other_y_trans, // The query
703  x1, y1, // Closest point #1
704  x2, y2, // Closest point #2
705  d1, d2);
706 
707  w1 = *other_x_trans - Axy;
708  q1 = math::closestSquareDistanceFromPointToLine(
709  w1, *other_y_trans, x1, y1, x2, y2);
710  q1 = kernel(q1, rho2);
711 
712  w2 = *other_x_trans;
713  q2 = math::closestSquareDistanceFromPointToLine(
714  w2, *other_y_trans, x1, y1, x2, y2);
715  q2 = kernel(q2, rho2);
716 
717  w3 = *other_x_trans + Axy;
718  q3 = math::closestSquareDistanceFromPointToLine(
719  w3, *other_y_trans, x1, y1, x2, y2);
720  q3 = kernel(q3, rho2);
721 #endif
722  // interpolate
723  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
724  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
725  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);
726 
727  dJ_dq.get_unsafe(0, i) = (2 * A * *other_x_trans) + B;
728 
729  // Jacobian: dJ_dy
730  // --------------------------------------
731  w1 = *other_y_trans - Axy;
732 #ifdef ICP_DISTANCES_TO_LINE
733  q1 = math::closestSquareDistanceFromPointToLine(
734  *other_x_trans, w1, x1, y1, x2, y2);
735  q1 = kernel(q1, rho2);
736 #else
737  q1 = kernel(
738  square(it->this_x - *other_x_trans) +
739  square(it->this_y - w1),
740  rho2);
741 #endif
742 
743  w2 = *other_y_trans;
744  // q2 is alreay computed from above!
745  // q2 = m1->squareDistanceToClosestCorrespondence(
746  // *other_x_trans, w2 );
747  // q2= kernel( square(it->this_x - *other_x_trans)+ square(
748  // it->this_y - w2 ), rho2 );
749 
750  w3 = *other_y_trans + Axy;
751 #ifdef ICP_DISTANCES_TO_LINE
752  q3 = math::closestSquareDistanceFromPointToLine(
753  *other_x_trans, w3, x1, y1, x2, y2);
754  q3 = kernel(q3, rho2);
755 #else
756  q3 = kernel(
757  square(it->this_x - *other_x_trans) +
758  square(it->this_y - w3),
759  rho2);
760 #endif
761 
762  // interpolate
763  A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
764  ((q1 - q2) / ((w1 - w2) * (w3 - w1)));
765  B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);
766 
767  dJ_dq.get_unsafe(1, i) = (2 * A * *other_y_trans) + B;
768 
769  // Jacobian: dR_dphi
770  // --------------------------------------
771  dJ_dq.get_unsafe(2, i) =
772  dJ_dq.get_unsafe(0, i) *
773  (-csin * it->other_x - ccos * it->other_y) +
774  dJ_dq.get_unsafe(1, i) *
775  (ccos * it->other_x - csin * it->other_y);
776 
777  } // end for each corresp.
778 
779  // Now we have the Jacobian in dJ_dq.
780 
781  // Compute the Hessian matrix H = dJ_dq * dJ_dq^T
782  CMatrixFloat H_(3, 3);
783  H_.multiply_AAt(dJ_dq);
784 
785  CMatrixFixedNumeric<float, 3, 3> H =
786  CMatrixFixedNumeric<float, 3, 3>(H_);
787 
788  bool keepIteratingLM = true;
789 
790  // ---------------------------------------------------
791  // Iterate the inner LM loop until convergence:
792  // ---------------------------------------------------
793  q_new = q;
794 
795  std::vector<float> new_sq_errors, new_other_xs_trans,
796  new_other_ys_trans;
797  size_t nLMiters = 0;
798  const size_t maxLMiters = 100;
799 
800  while (keepIteratingLM && ++nLMiters < maxLMiters)
801  {
802  // The LM heuristic is:
803  // x_{k+1} = x_k - ( H + \lambda diag(H) )^-1 * grad(J)
804  // grad(J) = dJ_dq * e (vector of errors)
805  C = H;
806  for (i = 0; i < 3; i++)
807  C(i, i) *=
808  (1 + lambda); // Levenberg-Maquardt heuristic
809 
810  C_inv = C.inv();
811 
812  // LM_delta = C_inv * dJ_dq * sq_errors
813  Eigen::VectorXf dJsq, LM_delta;
814  dJ_dq.multiply_Ab(
815  Eigen::Map<Eigen::VectorXf>(
816  &sq_errors[0], sq_errors.size()),
817  dJsq);
818  C_inv.multiply_Ab(dJsq, LM_delta);
819 
820  q_new.x(q.x() - LM_delta[0]);
821  q_new.y(q.y() - LM_delta[1]);
822  q_new.phi(q.phi() - LM_delta[2]);
823 
824  // Compute the new square errors:
825  // ---------------------------------------
826  correspondences.squareErrorVector(
827  q_new, new_sq_errors, new_other_xs_trans,
828  new_other_ys_trans);
829 
830  float OSE_new = math::sum(new_sq_errors);
831 
832  bool improved = OSE_new < OSE_initial;
833 
834 #if 0 // Debuggin'
835  cout << "_____________" << endl;
836  cout << "q -> q_new : " << q << " -> " << q_new << endl;
837  printf("err: %f -> %f lambda: %e\n", OSE_initial ,OSE_new, lambda );
838  cout << "\\/J = "; utils::operator <<(cout,dJsq); cout << endl;
839  mrpt::system::pause();
840 #endif
841 
842  keepIteratingLM =
843  fabs(LM_delta[0]) > options.minAbsStep_trans ||
844  fabs(LM_delta[1]) > options.minAbsStep_trans ||
845  fabs(LM_delta[2]) > options.minAbsStep_rot;
846 
847  if (improved)
848  {
849  // If resids have gone down, keep change and make lambda
850  // smaller by factor of 10
851  lambda /= 10;
852  q = q_new;
853  OSE_initial = OSE_new;
854  }
855  else
856  {
857  // Discard movement and try with larger lambda:
858  lambda *= 10;
859  }
860 
861  } // end iterative LM
862 
863 #if 0 // Debuggin'
864  cout << "ICP loop: " << lastMeanPose << " -> " << q << " LM iters: " << nLMiters << " threshold: " << matchParams.maxDistForCorrespondence << endl;
865 #endif
866  // --------------------------------------------------------
867  // now the conditions for the outer ICP loop
868  // --------------------------------------------------------
869  keepIteratingICP = true;
870  if (fabs(lastMeanPose.x() - q.x()) < options.minAbsStep_trans &&
871  fabs(lastMeanPose.y() - q.y()) < options.minAbsStep_trans &&
872  fabs(math::wrapToPi(lastMeanPose.phi() - q.phi())) <
873  options.minAbsStep_rot)
874  {
875  matchParams.maxDistForCorrespondence *= options.ALFA;
876  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
877  if (matchParams.maxDistForCorrespondence <
878  options.smallestThresholdDist)
879  keepIteratingICP = false;
880 
881  if (++matchParams.offset_other_map_points >=
882  options.corresponding_points_decimation)
883  matchParams.offset_other_map_points = 0;
884  }
885  lastMeanPose = q;
886  } // end of "else, there are correspondences"
887 
888  // Next iteration:
889  outInfo.nIterations++;
890 
891  if (outInfo.nIterations >= options.maxIterations &&
892  matchParams.maxDistForCorrespondence >
893  options.smallestThresholdDist)
894  {
895  matchParams.maxDistForCorrespondence *= options.ALFA;
896  }
897 
898  } while (
899  (keepIteratingICP && outInfo.nIterations < options.maxIterations) ||
900  (outInfo.nIterations >= options.maxIterations &&
901  matchParams.maxDistForCorrespondence >
902  options.smallestThresholdDist));
903 
904  outInfo.goodness = matchExtraResults.correspondencesRatio;
905 
906  // if (!options.skip_quality_calculation)
907  {
908  outInfo.quality = matchExtraResults.correspondencesRatio;
909  }
910 
911  } // end of "if m2 is not empty"
912 
913  return CPosePDFGaussian::Ptr(new CPosePDFGaussian(q, C_inv.cast<double>()));
914  MRPT_END
915 }
916 
917 CPose3DPDF::Ptr CICP::Align3DPDF(
918  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
919  const CPose3DPDFGaussian& initialEstimationPDF, float* runningTime,
920  void* info)
921 {
922  MRPT_START
923 
924  static CTicTac tictac;
925  TReturnInfo outInfo;
926  CPose3DPDF::Ptr resultPDF;
927 
928  if (runningTime) tictac.Tic();
929 
930  switch (options.ICP_algorithm)
931  {
932  case icpClassic:
933  resultPDF =
934  ICP3D_Method_Classic(m1, mm2, initialEstimationPDF, outInfo);
935  break;
936  case icpLevenbergMarquardt:
937  THROW_EXCEPTION("Only icpClassic is implemented for ICP-3D");
938  break;
939  default:
940  THROW_EXCEPTION_FMT(
941  "Invalid value for ICP_algorithm: %i",
942  static_cast<int>(options.ICP_algorithm));
943  } // end switch
944 
945  if (runningTime) *runningTime = tictac.Tac();
946 
947  // Copy the output info if requested:
948  if (info)
949  {
950  MRPT_TODO(
951  "Refactor `info` so it is polymorphic and can use dynamic_cast<> "
952  "here");
953  *static_cast<TReturnInfo*>(info) = outInfo;
954  }
955 
956  return resultPDF;
957 
958  MRPT_END
959 }
960 
961 CPose3DPDF::Ptr CICP::ICP3D_Method_Classic(
962  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
963  const CPose3DPDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
964 {
965  MRPT_START
966 
967  // The result can be either a Gaussian or a SOG:
968  CPose3DPDF::Ptr resultPDF;
969  CPose3DPDFGaussian::Ptr gaussPdf;
970 
971  size_t nCorrespondences = 0;
972  bool keepApproaching;
973  CPose3D grossEst = initialEstimationPDF.mean;
974  mrpt::tfest::TMatchingPairList correspondences, old_correspondences;
975  CPose3D lastMeanPose;
976 
977  // Assure the class of the maps:
978  ASSERT_(mm2->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
979  const CPointsMap* m2 = (CPointsMap*)mm2;
980 
981  // Asserts:
982  // -----------------
983  ASSERT_(options.ALFA > 0 && options.ALFA < 1);
984 
985  // The algorithm output auxiliar info:
986  // -------------------------------------------------
987  outInfo.nIterations = 0;
988  outInfo.goodness = 1;
989  outInfo.quality = 0;
990 
991  // The gaussian PDF to estimate:
992  // ------------------------------------------------------
993  gaussPdf = mrpt::make_aligned_shared<CPose3DPDFGaussian>();
994 
995  // First gross approximation:
996  gaussPdf->mean = grossEst;
997 
998  // Initial thresholds:
999  TMatchingParams matchParams;
1000  TMatchingExtraResults matchExtraResults;
1001 
1002  matchParams.maxDistForCorrespondence =
1003  options.thresholdDist; // Distance threshold
1004  matchParams.maxAngularDistForCorrespondence =
1005  options.thresholdAng; // Angular threshold
1006  matchParams.onlyKeepTheClosest = true;
1007  matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
1008  matchParams.decimation_other_map_points =
1009  options.corresponding_points_decimation;
1010 
1011  // Asure maps are not empty!
1012  // ------------------------------------------------------
1013  if (!m2->isEmpty())
1014  {
1015  matchParams.offset_other_map_points = 0;
1016 
1017  // ------------------------------------------------------
1018  // The ICP loop
1019  // ------------------------------------------------------
1020  do
1021  {
1022  matchParams.angularDistPivotPoint = TPoint3D(
1023  gaussPdf->mean.x(), gaussPdf->mean.y(), gaussPdf->mean.z());
1024 
1025  // ------------------------------------------------------
1026  // Find the matching (for a points map)
1027  // ------------------------------------------------------
1028  m1->determineMatching3D(
1029  m2, // The other map
1030  gaussPdf->mean, // The other map pose
1031  correspondences, matchParams, matchExtraResults);
1032 
1033  nCorrespondences = correspondences.size();
1034 
1035  if (!nCorrespondences)
1036  {
1037  // Nothing we can do !!
1038  keepApproaching = false;
1039  }
1040  else
1041  {
1042  // Compute the estimated pose, using Horn's method.
1043  // ----------------------------------------------------------------------
1044  mrpt::poses::CPose3DQuat estPoseQuat;
1045  double transf_scale;
1046  mrpt::tfest::se3_l2(
1047  correspondences, estPoseQuat, transf_scale,
1048  false /* dont force unit scale */);
1049  gaussPdf->mean = mrpt::poses::CPose3D(estPoseQuat);
1050 
1051  // If matching has not changed, decrease the thresholds:
1052  // --------------------------------------------------------
1053  keepApproaching = true;
1054  if (!(fabs(lastMeanPose.x() - gaussPdf->mean.x()) >
1055  options.minAbsStep_trans ||
1056  fabs(lastMeanPose.y() - gaussPdf->mean.y()) >
1057  options.minAbsStep_trans ||
1058  fabs(lastMeanPose.z() - gaussPdf->mean.z()) >
1059  options.minAbsStep_trans ||
1060  fabs(
1061  math::wrapToPi(
1062  lastMeanPose.yaw() - gaussPdf->mean.yaw())) >
1063  options.minAbsStep_rot ||
1064  fabs(
1065  math::wrapToPi(
1066  lastMeanPose.pitch() - gaussPdf->mean.pitch())) >
1067  options.minAbsStep_rot ||
1068  fabs(
1069  math::wrapToPi(
1070  lastMeanPose.roll() - gaussPdf->mean.roll())) >
1071  options.minAbsStep_rot))
1072  {
1073  matchParams.maxDistForCorrespondence *= options.ALFA;
1074  matchParams.maxAngularDistForCorrespondence *= options.ALFA;
1075  if (matchParams.maxDistForCorrespondence <
1076  options.smallestThresholdDist)
1077  keepApproaching = false;
1078 
1079  if (++matchParams.offset_other_map_points >=
1080  options.corresponding_points_decimation)
1081  matchParams.offset_other_map_points = 0;
1082  }
1083 
1084  lastMeanPose = gaussPdf->mean;
1085 
1086  } // end of "else, there are correspondences"
1087 
1088  // Next iteration:
1089  outInfo.nIterations++;
1090 
1091  if (outInfo.nIterations >= options.maxIterations &&
1092  matchParams.maxDistForCorrespondence >
1093  options.smallestThresholdDist)
1094  {
1095  matchParams.maxDistForCorrespondence *= options.ALFA;
1096  }
1097 
1098  } while (
1099  (keepApproaching && outInfo.nIterations < options.maxIterations) ||
1100  (outInfo.nIterations >= options.maxIterations &&
1101  matchParams.maxDistForCorrespondence >
1102  options.smallestThresholdDist));
1103 
1104  // -------------------------------------------------
1105  // Obtain the covariance matrix of the estimation
1106  // -------------------------------------------------
1107  if (!options.skip_cov_calculation && nCorrespondences)
1108  {
1109  // ...
1110  }
1111 
1112  //
1113  outInfo.goodness = matchExtraResults.correspondencesRatio;
1114 
1115  } // end of "if m2 is not empty"
1116 
1117  // Return the gaussian distribution:
1118  resultPDF = gaussPdf;
1119 
1120  return resultPDF;
1121 
1122  MRPT_END
1123 }
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
mrpt::tfest::TSE2RobustParams
Parameters for se2_l2_robust().
Definition: se2.h:73
ops_containers.h
mrpt::maps::CPointsMap::squareDistanceToClosestCorrespondence
virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const override
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
Definition: CPointsMap.cpp:908
ransac_mahalanobisDistanceThreshold
const float ransac_mahalanobisDistanceThreshold
Definition: se3_ransac_unittest.cpp:38
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:199
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:917
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:40
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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::slam::CICP::kernel
float kernel(const float &x2, const float &rho2)
Computes:
Definition: CICP.cpp:171
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
s
GLdouble s
Definition: glext.h:3676
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:459
mrpt::poses::CPoseOrPoint::x_incr
void x_incr(const double v)
Definition: CPoseOrPoint.h:167
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:534
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:19
MRPT_LOAD_CONFIG_VAR
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
Definition: config/CConfigFileBase.h:282
mrpt::poses::CPoseOrPoint::y_incr
void y_incr(const double v)
Definition: CPoseOrPoint.h:171
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: lightweight_geom_data.h:195
mrpt::slam::icpLevenbergMarquardt
@ icpLevenbergMarquardt
Definition: CICP.h:25
mrpt::slam::CICP::TConfigParams::saveToConfigFile
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
Definition: CICP.cpp:123
mrpt::slam::TICPCovarianceMethod
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options.
Definition: CICP.h:30
CPosePDFSOG.h
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:34
mrpt::tfest::TMatchingPairList::squareErrorVector
void squareErrorVector(const mrpt::poses::CPose2D &q, std::vector< float > &out_sqErrs) const
Returns a vector with the square error between each pair of correspondences in the list,...
Definition: TMatchingPair.cpp:132
mrpt::poses::CPosePDFGaussian::Ptr
std::shared_ptr< CPosePDFGaussian > Ptr
Definition: CPosePDFGaussian.h:33
c
const GLubyte * c
Definition: glext.h:6313
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
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:961
mrpt::poses::CPosePDFSOG::Ptr
std::shared_ptr< CPosePDFSOG > Ptr
Definition: CPosePDFSOG.h:39
CICP.h
CPose3DPDFGaussian.h
mrpt::maps::CPointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
Definition: CPointsMap.h:64
mrpt::math::TPose2D::y
double y
Definition: lightweight_geom_data.h:193
wrap2pi.h
mrpt::slam::CICP::ICP_Method_Classic
mrpt::poses::CPosePDF::Ptr ICP_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:181
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:16
mrpt::slam::TICPAlgorithm
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options.
Definition: CICP.h:22
w1
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:5567
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:48
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::RAD2DEG
double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:48
CPose2D.h
mrpt::maps::TMatchingExtraResults::correspondencesRatio
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
Definition: metric_map_types.h:66
mrpt::maps::TMatchingParams::onlyUniqueRobust
bool onlyUniqueRobust
Additional consistency filter: "onlyKeepTheClosest" allows one correspondence for each "local map" po...
Definition: metric_map_types.h:35
results
map< string, CVectorDouble > results
Definition: vision_stereo_rectify/test.cpp:33
mrpt::slam::icpClassic
@ icpClassic
Definition: CICP.h:24
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:192
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:46
mrpt::math::CMatrixTemplateNumeric
A matrix of dynamic size.
Definition: CMatrixTemplateNumeric.h:37
mrpt::maps::CMetricMap::determineMatching2D
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matching between this and another 2D point map, which includes finding:
Definition: CMetricMap.cpp:127
mrpt::slam::CICP::TReturnInfo::quality
float quality
A measure of the 'quality' of the local minimum of the sqr.
Definition: CICP.h:203
CConfigFileBase.h
normalizationStd
const float normalizationStd
Definition: se3_ransac_unittest.cpp:37
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
mrpt::maps::TMatchingParams::maxDistForCorrespondence
float maxDistForCorrespondence
Maximum linear distance between two points to be paired (meters)
Definition: metric_map_types.h:25
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::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:144
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
CPosePDF.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:40
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
mrpt::poses::CPose3DPDFGaussian::Ptr
std::shared_ptr< CPose3DPDFGaussian > Ptr
Definition: CPose3DPDFGaussian.h:42
mrpt::tfest::se3_l2
bool se3_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2.cpp:221
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::rtti::TRuntimeClassId::derivedFrom
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:24
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:540
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:186
tfest.h
mrpt::math::CMatrixFixedNumeric< double, 3, 3 >
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:22
w2
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:5568
mrpt::maps::CPointsMap::isEmpty
virtual bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CPointsMap.cpp:618
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
mrpt::maps::CPointsMap::determineMatching2D
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding:
Definition: CPointsMap.cpp:304
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
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
CTicTac.h
CPosePDFGaussian.h
slam-precomp.h
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
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::tfest::TSE2RobustResult
Output placeholder for se2_l2_robust()
Definition: se2.h:144
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:554
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
MRPT_TODO
#define MRPT_TODO(x)
Definition: common.h:129
mrpt::tfest::se2_l2
bool se2_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=nullptr)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.
Definition: se2_l2.cpp:45
mrpt::maps::CMetricMap::determineMatching3D
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
Definition: CMetricMap.cpp:142
mrpt::math::KDTreeCapable::kdTreeTwoClosestPoint2D
void kdTreeTwoClosestPoint2D(float x0, float y0, float &out_x1, float &out_y1, float &out_x2, float &out_y2, float &out_dist_sqr1, float &out_dist_sqr2) const
KD Tree-based search for the TWO closest point to some given 2D coordinates.
Definition: KDTreeCapable.h:226
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:45
mrpt::maps::CMetricMap::isEmpty
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
mrpt::slam::icpCovLinealMSE
@ icpCovLinealMSE
Use the covariance of the optimal registration, disregarding uncertainty in data association.
Definition: CICP.h:34
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:83
CPose3DPDF.h
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::slam::CICP::TReturnInfo::nIterations
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
mrpt::maps::TMatchingParams::angularDistPivotPoint
mrpt::math::TPoint3D angularDistPivotPoint
The point used to calculate angular distances: e.g.
Definition: metric_map_types.h:44
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:93
mrpt::maps
Definition: CBeacon.h:24
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::slam::icpCovFiniteDifferences
@ icpCovFiniteDifferences
Covariance of a least-squares optimizer (includes data association uncertainty)
Definition: CICP.h:37
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc.,...
Definition: metric_map_types.h:62
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:31
mrpt::slam::CICP::TConfigParams::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CICP.cpp:75
CArchive.h
mrpt::tfest::se2_l2_robust
bool se2_l2_robust(const mrpt::tfest::TMatchingPairList &in_correspondences, const double in_normalizationStd, const TSE2RobustParams &in_ransac_params, TSE2RobustResult &out_results)
Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference ...
Definition: se2_l2_ransac.cpp:78
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:44
mrpt::slam
Definition: CMultiMetricMapPDF.h:27
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:79
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:193
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::comms::operator<<
std::ostream & operator<<(std::ostream &o, const TFTDIDevice &d)
Print out all the information of a FTDI device in textual form.
Definition: CInterfaceFTDI_WIN.cpp:449
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:428
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
Definition: math_frwds.h:25
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST