Main MRPT website > C++ reference for MRPT 1.5.7
se2_l2_ransac.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 "tfest-precomp.h" // Precompiled headers
11 
12 #include <mrpt/tfest/se2.h>
15 #include <mrpt/random.h>
16 #include <mrpt/math/geometry.h>
18 #include <mrpt/utils/CTimeLogger.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::tfest;
22 using namespace mrpt::random;
23 using namespace mrpt::utils;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 using namespace std;
27 
28 //#define AVOID_MULTIPLE_CORRESPONDENCES
29 
30 // mark this pair as "selected" so it won't be picked again:
32  const TMatchingPair & c,
33  std::vector<bool> &alreadySelectedThis,
34  std::vector<bool> &alreadySelectedOther
35 #ifdef AVOID_MULTIPLE_CORRESPONDENCES
36  , const std::vector<vector_int> &listDuplicatedLandmarksThis
37 #endif
38  )
39 {
40  ASSERTDEB_( c.this_idx < alreadySelectedThis.size() );
41  ASSERTDEB_( c.other_idx < alreadySelectedOther.size() );
42 
43 #ifndef AVOID_MULTIPLE_CORRESPONDENCES
44  alreadySelectedThis[ c.this_idx ]= true;
45  alreadySelectedOther[ c.other_idx ] = true;
46 #else
47  for (vector_int::iterator it1 = listDuplicatedLandmarksThis[c.this_idx].begin();it1!=listDuplicatedLandmarksThis[c.this_idx].end();it1++)
48  alreadySelectedThis[ *it1 ] = true;
49  for (vector_int::iterator it2 = listDuplicatedLandmarksOther[c.other_idx].begin();it2!=listDuplicatedLandmarksOther[c.other_idx].end();it2++)
50  alreadySelectedOther[ *it2 ] = true;
51 #endif
52 }
53 
54 
55 /*---------------------------------------------------------------
56 
57  robustRigidTransformation
58 
59  The technique was described in the paper:
60  J.L. Blanco, J. Gonzalez-Jimenez and J.A. Fernandez-Madrigal.
61  "A robust, multi-hypothesis approach to matching occupancy grid maps".
62  Robotica, available on CJO2013. doi:10.1017/S0263574712000732.
63  http://journals.cambridge.org/action/displayAbstract?aid=8815308
64 
65  This works as follows:
66  - Repeat "results.ransac_iters" times:
67  - Randomly pick TWO correspondences from the set "in_correspondences".
68  - Compute the associated rigid transformation.
69  - For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group:
70  - If if is compatible (ransac_maxErrorXY, ransac_maxErrorPHI), grow the "consensus set"
71  - If not, do not add it.
72  ---------------------------------------------------------------*/
74  const mrpt::utils::TMatchingPairList &in_correspondences,
75  const double normalizationStd,
76  const TSE2RobustParams & params,
77  TSE2RobustResult & results
78  )
79 {
80 //#define DO_PROFILING
81 
82 #ifdef DO_PROFILING
83  CTimeLogger timlog;
84 #endif
85 
86  const size_t nCorrs = in_correspondences.size();
87 
88  // Default: 2 * normalizationStd ("noise level")
89  const double MAX_RMSE_TO_END = (params.max_rmse_to_end<=0 ? 2*normalizationStd : params.max_rmse_to_end);
90 
92 
93  // Asserts:
94  if( nCorrs < params.ransac_minSetSize )
95  {
96  // Nothing to do!
97  results.transformation.clear();
98  results.largestSubSet = TMatchingPairList();
99  return false;
100  }
101 
102 
103 #ifdef DO_PROFILING
104  timlog.enter("ransac.find_max*");
105 #endif
106  // Find the max. index of "this" and "other:
107  unsigned int maxThis=0, maxOther=0;
108  for (TMatchingPairList::const_iterator matchIt=in_correspondences.begin();matchIt!=in_correspondences.end(); ++matchIt)
109  {
110  maxThis = max(maxThis , matchIt->this_idx );
111  maxOther= max(maxOther, matchIt->other_idx );
112  }
113 #ifdef DO_PROFILING
114  timlog.leave("ransac.find_max*");
115 #endif
116 
117 
118 #ifdef DO_PROFILING
119  timlog.enter("ransac.count_unique_corrs");
120 #endif
121 
122  // Fill out 2 arrays indicating whether each element has a correspondence:
123  std::vector<bool> hasCorrThis(maxThis+1,false);
124  std::vector<bool> hasCorrOther(maxOther+1,false);
125  unsigned int howManyDifCorrs = 0;
126  for (TMatchingPairList::const_iterator matchIt=in_correspondences.begin();matchIt!=in_correspondences.end(); ++matchIt)
127  {
128  if (!hasCorrThis[matchIt->this_idx] &&
129  !hasCorrOther[matchIt->other_idx] )
130  {
131  hasCorrThis[matchIt->this_idx] = true;
132  hasCorrOther[matchIt->other_idx] = true;
133  howManyDifCorrs++;
134  }
135  }
136 #ifdef DO_PROFILING
137  timlog.leave("ransac.count_unique_corrs");
138 #endif
139 
140  // Clear the set of output particles:
141  results.transformation.clear();
142 
143  // If there are less different correspondences than the minimum required, quit:
144  if ( howManyDifCorrs < params.ransac_minSetSize )
145  {
146  // Nothing we can do here!!! :~$
147  results.transformation.clear();
148  results.largestSubSet = TMatchingPairList();
149  return false;
150  }
151 
152 
153 #ifdef AVOID_MULTIPLE_CORRESPONDENCES
154  unsigned k;
155  // Find duplicated landmarks (from SIFT features with different descriptors,etc...)
156  // this is to avoid establishing multiple correspondences for the same physical point!
157  // ------------------------------------------------------------------------------------------------
158  std::vector<vector_int> listDuplicatedLandmarksThis(maxThis+1);
159  ASSERT_(nCorrs>=1);
160  for (k=0;k<nCorrs-1;k++)
161  {
162  vector_int duplis;
163  for (unsigned j=k;j<nCorrs-1;j++)
164  {
165  if ( in_correspondences[k].this_x == in_correspondences[j].this_x &&
166  in_correspondences[k].this_y == in_correspondences[j].this_y &&
167  in_correspondences[k].this_z == in_correspondences[j].this_z )
168  duplis.push_back(in_correspondences[j].this_idx);
169  }
170  listDuplicatedLandmarksThis[in_correspondences[k].this_idx] = duplis;
171  }
172 
173  std::vector<vector_int> listDuplicatedLandmarksOther(maxOther+1);
174  for (k=0;k<nCorrs-1;k++)
175  {
176  vector_int duplis;
177  for (unsigned j=k;j<nCorrs-1;j++)
178  {
179  if ( in_correspondences[k].other_x == in_correspondences[j].other_x &&
180  in_correspondences[k].other_y == in_correspondences[j].other_y &&
181  in_correspondences[k].other_z == in_correspondences[j].other_z )
182  duplis.push_back(in_correspondences[j].other_idx);
183  }
184  listDuplicatedLandmarksOther[in_correspondences[k].other_idx] = duplis;
185  }
186 #endif
187 
188  std::deque<TMatchingPairList> alreadyAddedSubSets;
189 
190  CPosePDFGaussian referenceEstimation;
191  CPoint2DPDFGaussian pt_this;
192 
193  const double ransac_consistency_test_chi2_quantile = 0.99;
194  const double chi2_thres_dim1 = mrpt::math::chi2inv(ransac_consistency_test_chi2_quantile, 1);
195 
196  // -------------------------
197  // The RANSAC loop
198  // -------------------------
199  size_t largest_consensus_yet = 0; // Used for dynamic # of steps
200  double largestSubSet_RMSE = std::numeric_limits<double>::max();
201 
202  results.ransac_iters = params.ransac_nSimulations;
203  const bool use_dynamic_iter_number = results.ransac_iters==0;
204  if (use_dynamic_iter_number)
205  {
206  ASSERT_(params.probability_find_good_model>0 && params.probability_find_good_model<1);
207  // Set an initial # of iterations:
208  results.ransac_iters = 10; // It doesn't matter actually, since will be changed in the first loop
209  }
210 
211  std::vector<bool> alreadySelectedThis, alreadySelectedOther;
212 
213  if (!params.ransac_algorithmForLandmarks)
214  {
215  alreadySelectedThis.assign(maxThis+1,false);
216  alreadySelectedOther.assign(maxOther+1, false);
217  }
218  // else -> It will be done anyway inside the for() below
219 
220  // First: Build a permutation of the correspondences to pick from it sequentially:
221  std::vector<size_t> corrsIdxs(nCorrs), corrsIdxsPermutation;
222  for (size_t i=0;i<nCorrs;i++) corrsIdxs[i]= i;
223 
224  size_t iter_idx;
225  for (iter_idx = 0;iter_idx<results.ransac_iters; iter_idx++) // results.ransac_iters can be dynamic
226  {
227 #ifdef DO_PROFILING
228  CTimeLoggerEntry tle(timlog,"ransac.iter");
229 #endif
230 
231 #ifdef DO_PROFILING
232  timlog.enter("ransac.permute");
233 #endif
234  randomGenerator.permuteVector(corrsIdxs,corrsIdxsPermutation );
235 
236 #ifdef DO_PROFILING
237  timlog.leave("ransac.permute");
238 #endif
239 
240  TMatchingPairList subSet;
241 
242  // Select a subset of correspondences at random:
243  if (params.ransac_algorithmForLandmarks)
244  {
245 #ifdef DO_PROFILING
246  timlog.enter("ransac.reset_selection_marks");
247 #endif
248  alreadySelectedThis.assign(maxThis+1,false);
249  alreadySelectedOther.assign(maxOther+1, false);
250 #ifdef DO_PROFILING
251  timlog.leave("ransac.reset_selection_marks");
252 #endif
253  }
254  else
255  {
256  // For points: Do not repeat the corrs, and take the number of corrs as weights
257  }
258 
259  // Try to build a subsetof "ransac_maxSetSize" (maximum) elements that achieve consensus:
260  // ------------------------------------------------------------------------------------------
261 #ifdef DO_PROFILING
262  timlog.enter("ransac.inner_loops");
263 #endif
264  for (unsigned int j=0;j<nCorrs && subSet.size()<params.ransac_maxSetSize;j++)
265  {
266  const size_t idx = corrsIdxsPermutation[j];
267 
268  const TMatchingPair & corr_j = in_correspondences[idx];
269 
270  // Don't pick the same features twice!
271  if (alreadySelectedThis [corr_j.this_idx] || alreadySelectedOther[corr_j.other_idx])
272  continue;
273 
274  // Additional user-provided filter:
275  if (params.user_individual_compat_callback)
276  {
278  pm.idx_this = corr_j.this_idx;
279  pm.idx_other = corr_j.other_idx;
280  if (! (*params.user_individual_compat_callback)(pm,params.user_individual_compat_callback_userdata))
281  continue; // Skip this one!
282  }
283 
284  if (subSet.size()<2)
285  {
286  // ------------------------------------------------------------------------------------------------------
287  // If we are within the first two correspondences, just add them to the subset:
288  // ------------------------------------------------------------------------------------------------------
289  subSet.push_back( corr_j );
290  markAsPicked(corr_j, alreadySelectedThis,alreadySelectedOther);
291 
292  if (subSet.size()==2)
293  {
294  // Consistency Test: From
295 
296  // Check the feasibility of this pair "idx1"-"idx2":
297  // The distance between the pair of points in MAP1 must be very close
298  // to that of their correspondences in MAP2:
299  const double corrs_dist1 = mrpt::math::distanceBetweenPoints(
300  subSet[0].this_x, subSet[0].this_y,
301  subSet[1].this_x, subSet[1].this_y );
302 
303  const double corrs_dist2 = mrpt::math::distanceBetweenPoints(
304  subSet[0].other_x, subSet[0].other_y,
305  subSet[1].other_x, subSet[1].other_y );
306 
307  // Is is a consistent possibility?
308  // We use a chi2 test (see paper for the derivation)
309  const double corrs_dist_chi2 =
310  square( square(corrs_dist1)-square(corrs_dist2) ) /
311  (8.0* square(normalizationStd) * (square(corrs_dist1)+square(corrs_dist2)) );
312 
313  bool is_acceptable = (corrs_dist_chi2 < chi2_thres_dim1 );
314 
315  if (is_acceptable)
316  {
317  // Perform estimation:
318  tfest::se2_l2( subSet, referenceEstimation );
319  // Normalized covariance: scale!
320  referenceEstimation.cov *= square(normalizationStd);
321 
322  // Additional filter:
323  // If the correspondences as such the transformation has a high ambiguity, we discard it!
324  is_acceptable = ( referenceEstimation.cov(2,2)<square(DEG2RAD(5.0f)) );
325  }
326 
327  if (!is_acceptable)
328  {
329  // Remove this correspondence & try again with a different pair:
330  subSet.erase( subSet.begin() + (subSet.size() -1) );
331  }
332  else
333  {
334  // Only mark as picked if we're really keeping it:
335  markAsPicked(corr_j, alreadySelectedThis,alreadySelectedOther);
336  }
337  }
338  }
339  else
340  {
341 #ifdef DO_PROFILING
342  timlog.enter("ransac.test_consistency");
343 #endif
344 
345  // ------------------------------------------------------------------------------------------------------
346  // The normal case:
347  // - test for "consensus" with the current group:
348  // - If it is compatible (ransac_maxErrorXY, ransac_maxErrorPHI), grow the "consensus set"
349  // - If not, do not add it.
350  // ------------------------------------------------------------------------------------------------------
351 
352  // Test for the mahalanobis distance between:
353  // "referenceEstimation (+) point_other" AND "point_this"
354  referenceEstimation.composePoint( mrpt::math::TPoint2D(corr_j.other_x,corr_j.other_y), pt_this);
355 
356  const double maha_dist = pt_this.mahalanobisDistanceToPoint(corr_j.this_x,corr_j.this_y);
357 
358  const bool passTest = maha_dist < params.ransac_mahalanobisDistanceThreshold;
359 
360  if ( passTest )
361  {
362  // OK, consensus passed:
363  subSet.push_back( corr_j );
364  markAsPicked(corr_j, alreadySelectedThis,alreadySelectedOther);
365  }
366  // else -> Test failed
367 
368 #ifdef DO_PROFILING
369  timlog.leave("ransac.test_consistency");
370 #endif
371  } // end else "normal case"
372 
373  } // end for j
374 #ifdef DO_PROFILING
375  timlog.leave("ransac.inner_loops");
376 #endif
377 
378 
379  const bool has_to_eval_RMSE = (subSet.size()>=params.ransac_minSetSize);
380 
381  // Compute the RMSE of this matching and the corresponding transformation (only if we'll use this value below)
382  double this_subset_RMSE = 0;
383  if (has_to_eval_RMSE)
384  {
385 #ifdef DO_PROFILING
386  CTimeLoggerEntry tle(timlog,"ransac.comp_rmse");
387 #endif
388 
389  // Recompute referenceEstimation from all the corrs:
390  tfest::se2_l2(subSet,referenceEstimation);
391  // Normalized covariance: scale!
392  referenceEstimation.cov *= square(normalizationStd);
393 
394  for (size_t k=0;k<subSet.size();k++)
395  {
396  double gx,gy;
397  referenceEstimation.mean.composePoint(
398  subSet[k].other_x, subSet[k].other_y,
399  gx,gy);
400 
401  this_subset_RMSE += mrpt::math::distanceSqrBetweenPoints<double>( subSet[k].this_x,subSet[k].this_y, gx,gy );
402  }
403  this_subset_RMSE /= std::max( static_cast<size_t>(1), subSet.size() );
404  }
405  else
406  {
407  this_subset_RMSE = std::numeric_limits<double>::max();
408  }
409 
410  // Save the estimation result as a "particle", only if the subSet contains
411  // "ransac_minSetSize" elements at least:
412  if (subSet.size()>=params.ransac_minSetSize)
413  {
414  // If this subset was previously added to the SOG, just increment its weight
415  // and do not add a new mode:
416  int indexFound = -1;
417 
418  // JLBC Added DEC-2007: An alternative (optional) method to fuse Gaussian modes:
419  if (!params.ransac_fuseByCorrsMatch)
420  {
421  // Find matching by approximate match in the X,Y,PHI means
422  // -------------------------------------------------------------------
423  for (size_t i=0;i<results.transformation.size();i++)
424  {
425  double diffXY = results.transformation.get(i).mean.distanceTo( referenceEstimation.mean );
426  double diffPhi = fabs( math::wrapToPi( results.transformation.get(i).mean.phi() - referenceEstimation.mean.phi() ) );
427  if ( diffXY < params.ransac_fuseMaxDiffXY && diffPhi < params.ransac_fuseMaxDiffPhi )
428  {
429  //printf("Match by distance found: distXY:%f distPhi=%f deg\n",diffXY,RAD2DEG(diffPhi));
430  indexFound = i;
431  break;
432  }
433  }
434  }
435  else
436  {
437  // Find matching mode by exact match in the list of correspondences:
438  // -------------------------------------------------------------------
439  // Sort "subSet" in order to compare them easily!
440  //std::sort( subSet.begin(), subSet.end() );
441 
442  // Try to find matching corrs:
443  for (size_t i=0;i<alreadyAddedSubSets.size();i++)
444  {
445  if ( subSet == alreadyAddedSubSets[i] )
446  {
447  indexFound = i;
448  break;
449  }
450  }
451  }
452 
453  if (indexFound!=-1)
454  {
455  // This is an already added mode:
456  if (params.ransac_algorithmForLandmarks)
457  results.transformation.get(indexFound).log_w = log(1+ exp(results.transformation.get(indexFound).log_w));
458  else results.transformation.get(indexFound).log_w = log(subSet.size()+ exp(results.transformation.get(indexFound).log_w));
459  }
460  else
461  {
462  // Add a new mode to the SOG:
463  alreadyAddedSubSets.push_back( subSet );
464 
465  CPosePDFSOG::TGaussianMode newSOGMode;
466  if (params.ransac_algorithmForLandmarks)
467  newSOGMode.log_w = 0; //log(1);
468  else newSOGMode.log_w = log(static_cast<double>(subSet.size()));
469 
470  newSOGMode.mean = referenceEstimation.mean;
471  newSOGMode.cov = referenceEstimation.cov;
472 
473  // Add a new mode to the SOG!
474  results.transformation.push_back(newSOGMode);
475  }
476  } // end if subSet.size()>=ransac_minSetSize
477 
478  const size_t ninliers = subSet.size();
479  if (largest_consensus_yet<ninliers )
480  {
481  largest_consensus_yet = ninliers;
482 
483  // Dynamic # of steps:
484  if (use_dynamic_iter_number)
485  {
486  // Update estimate of nCorrs, the number of trials to ensure we pick,
487  // with probability p, a data set with no outliers.
488  const double fracinliers = ninliers/static_cast<double>(howManyDifCorrs); // corrsIdxs.size());
489  double pNoOutliers = 1 - pow(fracinliers,static_cast<double>(2.0 /*minimumSizeSamplesToFit*/ ));
490 
491  pNoOutliers = std::max( std::numeric_limits<double>::epsilon(), pNoOutliers); // Avoid division by -Inf
492  pNoOutliers = std::min(1.0 - std::numeric_limits<double>::epsilon() , pNoOutliers); // Avoid division by 0.
493  // Number of
494  results.ransac_iters = log(1-params.probability_find_good_model)/log(pNoOutliers);
495 
496  results.ransac_iters = std::max(results.ransac_iters, params.ransac_min_nSimulations);
497 
498  if (params.verbose)
499  cout << "[tfest::RANSAC] Iter #" << iter_idx << ":est. # iters=" << results.ransac_iters << " pNoOutliers=" << pNoOutliers << " #inliers: " << ninliers << endl;
500  }
501 
502  }
503 
504  // Save the largest subset:
505  if (subSet.size()>=params.ransac_minSetSize && this_subset_RMSE<largestSubSet_RMSE )
506  {
507  if (params.verbose)
508  cout << "[tfest::RANSAC] Iter #" << iter_idx << " Better subset: " << subSet.size() << " inliers, RMSE=" << this_subset_RMSE << endl;
509 
510  results.largestSubSet = subSet;
511  largestSubSet_RMSE = this_subset_RMSE;
512  }
513 
514  // Is the found subset good enough?
515  if (subSet.size()>=params.ransac_minSetSize &&
516  this_subset_RMSE<MAX_RMSE_TO_END)
517  {
518  break; // end RANSAC iterations.
519  }
520 
521 #ifdef DO_PROFILING
522  timlog.leave("ransac.iter");
523 #endif
524  } // end for each iteration
525 
526  if (params.verbose)
527  cout << "[tfest::RANSAC] Finished after " << iter_idx << " iterations.\n";
528 
529 
530  // Set the weights of the particles to sum the unity:
532 
533  // Done!
534 
536  printf("nCorrs=%u\n",static_cast<unsigned int>(nCorrs)); \
537  printf("Saving '_debug_in_correspondences.txt'..."); \
538  in_correspondences.dumpToFile("_debug_in_correspondences.txt"); \
539  printf("Ok\n"); \
540  printf("Saving '_debug_results.transformation.txt'..."); \
541  results.transformation.saveToTextFile("_debug_results.transformation.txt"); \
542  printf("Ok\n"); );
543 
544  return true;
545 }
A namespace of pseudo-random numbers genrators of diferent distributions.
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void permuteVector(const VEC &in_vector, VEC &out_result)
Returns a random permutation of a vector: all the elements of the input vector are in the output but ...
void clear()
Clear the list of modes.
Definition: CPosePDFSOG.cpp:43
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.
const float normalizationStd
#define min(a, b)
The struct for each mode:
Definition: CPosePDFSOG.h:45
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
#define MRPT_END_WITH_CLEAN_UP(stuff)
A gaussian distribution for 2D points.
void markAsPicked(const TMatchingPair &c, std::vector< bool > &alreadySelectedThis, std::vector< bool > &alreadySelectedOther)
Parameters for se2_l2_robust().
Definition: se2.h:62
mrpt::utils::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:112
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
const TGaussianMode & get(size_t i) const
Access to individual beacons.
Definition: CPosePDFSOG.h:105
Scalar * iterator
Definition: eigen_plugins.h:23
STL namespace.
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
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPosePDFSOG.h:116
void composePoint(const mrpt::math::TPoint2D &l, CPoint2DPDFGaussian &g) const
Returns the PDF of the 2D point with "q"=this pose and "l" a point without uncertainty.
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:178
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
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 ...
const GLubyte * c
Definition: glext.h:5590
A list of TMatchingPair.
Definition: TMatchingPair.h:78
mrpt::math::CMatrixDouble33 cov
Definition: CPosePDFSOG.h:54
double mahalanobisDistanceToPoint(const double x, const double y) const
Returns the Mahalanobis distance from this PDF to some point.
#define DEG2RAD
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned int ransac_iters
Number of actual iterations executed.
Definition: se2.h:113
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
double BASE_IMPEXP chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:967
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
Definition: CTimeLogger.h:127
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define ASSERT_(f)
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:111
std::vector< int32_t > vector_int
Definition: types_simple.h:23
size_t size() const
Return the number of Gaussian modes.
Definition: CPosePDFSOG.h:87
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
Definition: geometry.h:754
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
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
Lightweight 2D point.
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
Definition: glext.h:3514
For each individual-compatibility (IC) test, the indices of the candidate match between elements in b...
Output placeholder for se2_l2_robust()
Definition: se2.h:109
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31



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