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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019