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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020