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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019