33 std::vector<bool>& alreadySelectedOther
34 #ifdef AVOID_MULTIPLE_CORRESPONDENCES
36 const std::vector<std::vector<int>>& listDuplicatedLandmarksThis
40 ASSERTDEB_(
c.this_idx < alreadySelectedThis.size());
41 ASSERTDEB_(
c.other_idx < alreadySelectedOther.size());
43 #ifndef AVOID_MULTIPLE_CORRESPONDENCES 44 alreadySelectedThis[
c.this_idx] =
true;
45 alreadySelectedOther[
c.other_idx] =
true;
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;
89 const size_t nCorrs = in_correspondences.size();
92 const double MAX_RMSE_TO_END =
99 if (nCorrs <
params.ransac_minSetSize)
102 results.transformation.clear();
108 timlog.enter(
"ransac.find_max*");
111 unsigned int maxThis = 0, maxOther = 0;
112 for (
const auto& in_correspondence : in_correspondences)
114 maxThis = max(maxThis, in_correspondence.this_idx);
115 maxOther = max(maxOther, in_correspondence.other_idx);
118 timlog.leave(
"ransac.find_max*");
122 timlog.enter(
"ransac.count_unique_corrs");
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)
131 if (!hasCorrThis[in_correspondence.this_idx] &&
132 !hasCorrOther[in_correspondence.other_idx])
134 hasCorrThis[in_correspondence.this_idx] =
true;
135 hasCorrOther[in_correspondence.other_idx] =
true;
140 timlog.leave(
"ransac.count_unique_corrs");
144 results.transformation.clear();
148 if (howManyDifCorrs <
params.ransac_minSetSize)
151 results.transformation.clear();
156 #ifdef AVOID_MULTIPLE_CORRESPONDENCES 163 std::vector<std::vector<int>> listDuplicatedLandmarksThis(maxThis + 1);
165 for (k = 0; k < nCorrs - 1; k++)
167 std::vector<int> duplis;
168 for (
unsigned j = k; j < nCorrs - 1; j++)
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);
175 listDuplicatedLandmarksThis[in_correspondences[k].this_idx] = duplis;
178 std::vector<std::vector<int>> listDuplicatedLandmarksOther(maxOther + 1);
179 for (k = 0; k < nCorrs - 1; k++)
181 std::vector<int> duplis;
182 for (
unsigned j = k; j < nCorrs - 1; j++)
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);
191 listDuplicatedLandmarksOther[in_correspondences[k].other_idx] = duplis;
195 std::deque<TMatchingPairList> alreadyAddedSubSets;
200 const double ransac_consistency_test_chi2_quantile = 0.99;
201 const double chi2_thres_dim1 =
207 size_t largest_consensus_yet = 0;
208 double largestSubSet_RMSE = std::numeric_limits<double>::max();
211 const bool use_dynamic_iter_number =
results.ransac_iters == 0;
212 if (use_dynamic_iter_number)
215 params.probability_find_good_model > 0 &&
216 params.probability_find_good_model < 1);
222 std::vector<bool> alreadySelectedThis, alreadySelectedOther;
224 if (!
params.ransac_algorithmForLandmarks)
226 alreadySelectedThis.assign(maxThis + 1,
false);
227 alreadySelectedOther.assign(maxOther + 1,
false);
233 std::vector<size_t> corrsIdxs(nCorrs), corrsIdxsPermutation;
234 for (
size_t i = 0; i < nCorrs; i++) corrsIdxs[i] = i;
237 for (iter_idx = 0; iter_idx <
results.ransac_iters;
241 CTimeLoggerEntry tle(timlog,
"ransac.iter");
245 timlog.enter(
"ransac.permute");
250 timlog.leave(
"ransac.permute");
256 if (
params.ransac_algorithmForLandmarks)
259 timlog.enter(
"ransac.reset_selection_marks");
261 alreadySelectedThis.assign(maxThis + 1,
false);
262 alreadySelectedOther.assign(maxOther + 1,
false);
264 timlog.leave(
"ransac.reset_selection_marks");
277 timlog.enter(
"ransac.inner_loops");
279 for (
unsigned int j = 0;
280 j < nCorrs && subSet.size() <
params.ransac_maxSetSize; j++)
282 const size_t idx = corrsIdxsPermutation[j];
287 if (alreadySelectedThis[corr_j.
this_idx] ||
292 if (
params.user_individual_compat_callback)
297 if (!
params.user_individual_compat_callback(pm))
301 if (subSet.size() < 2)
307 subSet.push_back(corr_j);
308 markAsPicked(corr_j, alreadySelectedThis, alreadySelectedOther);
310 if (subSet.size() == 2)
318 const double corrs_dist1 =
320 subSet[0].this_x, subSet[0].this_y,
321 subSet[1].this_x, subSet[1].this_y);
323 const double corrs_dist2 =
325 subSet[0].other_x, subSet[0].other_y,
326 subSet[1].other_x, subSet[1].other_y);
330 const double corrs_dist_chi2 =
335 bool is_acceptable = (corrs_dist_chi2 < chi2_thres_dim1);
348 (referenceEstimation.
cov(2, 2) <
356 subSet.erase(subSet.begin() + (subSet.size() - 1));
362 corr_j, alreadySelectedThis, alreadySelectedOther);
369 timlog.enter(
"ransac.test_consistency");
389 const bool passTest =
390 maha_dist <
params.ransac_mahalanobisDistanceThreshold;
395 subSet.push_back(corr_j);
397 corr_j, alreadySelectedThis, alreadySelectedOther);
402 timlog.leave(
"ransac.test_consistency");
408 timlog.leave(
"ransac.inner_loops");
411 const bool has_to_eval_RMSE =
412 (subSet.size() >=
params.ransac_minSetSize);
416 double this_subset_RMSE = 0;
417 if (has_to_eval_RMSE)
420 CTimeLoggerEntry tle(timlog,
"ransac.comp_rmse");
428 for (
size_t k = 0; k < subSet.size(); k++)
432 subSet[k].other_x, subSet[k].other_y, gx, gy);
435 mrpt::math::distanceSqrBetweenPoints<double>(
436 subSet[k].this_x, subSet[k].this_y, gx, gy);
438 this_subset_RMSE /= std::max(static_cast<size_t>(1), subSet.size());
442 this_subset_RMSE = std::numeric_limits<double>::max();
448 if (subSet.size() >=
params.ransac_minSetSize)
457 if (!
params.ransac_fuseByCorrsMatch)
461 for (
size_t i = 0; i <
results.transformation.size(); i++)
464 results.transformation.get(i).mean.distanceTo(
465 referenceEstimation.
mean);
467 results.transformation.get(i).mean.phi() -
468 referenceEstimation.
mean.
phi()));
469 if (diffXY <
params.ransac_fuseMaxDiffXY &&
470 diffPhi <
params.ransac_fuseMaxDiffPhi)
488 for (
size_t i = 0; i < alreadyAddedSubSets.size(); i++)
490 if (subSet == alreadyAddedSubSets[i])
498 if (indexFound != -1)
501 if (
params.ransac_algorithmForLandmarks)
502 results.transformation.get(indexFound).log_w = log(
503 1 + exp(
results.transformation.get(indexFound).log_w));
505 results.transformation.get(indexFound).log_w =
507 exp(
results.transformation.get(indexFound).log_w));
512 alreadyAddedSubSets.push_back(subSet);
515 if (
params.ransac_algorithmForLandmarks)
516 newSOGMode.
log_w = 0;
518 newSOGMode.
log_w = log(static_cast<double>(subSet.size()));
520 newSOGMode.
mean = referenceEstimation.
mean;
521 newSOGMode.
cov = referenceEstimation.
cov;
524 results.transformation.push_back(newSOGMode);
528 const size_t ninliers = subSet.size();
529 if (largest_consensus_yet < ninliers)
531 largest_consensus_yet = ninliers;
534 if (use_dynamic_iter_number)
539 const double fracinliers =
541 static_cast<double>(howManyDifCorrs);
543 1 - pow(fracinliers, static_cast<double>(
546 pNoOutliers = std::max(
547 std::numeric_limits<double>::epsilon(),
550 1.0 - std::numeric_limits<double>::epsilon(),
554 log(1 -
params.probability_find_good_model) /
557 results.ransac_iters = std::max(
561 cout <<
"[tfest::RANSAC] Iter #" << iter_idx
562 <<
":est. # iters=" <<
results.ransac_iters
563 <<
" pNoOutliers=" << pNoOutliers
564 <<
" #inliers: " << ninliers << endl;
569 if (subSet.size() >=
params.ransac_minSetSize &&
570 this_subset_RMSE < largestSubSet_RMSE)
573 cout <<
"[tfest::RANSAC] Iter #" << iter_idx
574 <<
" Better subset: " << subSet.size()
575 <<
" inliers, RMSE=" << this_subset_RMSE << endl;
577 results.largestSubSet = subSet;
578 largestSubSet_RMSE = this_subset_RMSE;
582 if (subSet.size() >=
params.ransac_minSetSize &&
583 this_subset_RMSE < MAX_RMSE_TO_END)
589 timlog.leave(
"ransac.iter");
594 cout <<
"[tfest::RANSAC] Finished after " << iter_idx
598 results.transformation.normalizeWeights();
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");
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 ...
CPose2D mean
The mean value.
const float normalizationStd
The struct for each mode:
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...
Parameters for se2_l2_robust().
double DEG2RAD(const double x)
Degrees to radians.
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)
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...
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.
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
mrpt::math::CMatrixDouble33 cov
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.
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...
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...
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double log_w
The log-weight.
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
For each individual-compatibility (IC) test, the indices of the candidate match between elements in b...
Output placeholder for se2_l2_robust()