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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019