Debian Codegraph @

libs/tfest/src/se2_l2_ransac.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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>
13 #include <mrpt/math/distributions.h>
14 #include <mrpt/math/geometry.h>
15 #include <mrpt/poses/CPoint2DPDFGaussian.h>
16 #include <mrpt/poses/CPosePDFGaussian.h>
17 #include <mrpt/random.h>
18 #include <mrpt/system/CTimeLogger.h>
19 #include <mrpt/tfest/se2.h>
20
21 #include <iostream>
22
23 using namespace mrpt;
24 using namespace mrpt::tfest;
25 using namespace mrpt::random;
26 using namespace mrpt::poses;
27 using namespace mrpt::math;
28 using namespace std;
29
30 //#define AVOID_MULTIPLE_CORRESPONDENCES
31
32 // mark this pair as "selected" so it won't be picked again:
33 void markAsPicked(
34 const TMatchingPair& c,
35 std::vector<bool>& alreadySelectedThis,
36 std::vector<bool>& alreadySelectedOther
37 #ifdef AVOID_MULTIPLE_CORRESPONDENCES
38 ,
39 const std::vector<std::vector<int>>& listDuplicatedLandmarksThis
40 #endif
41 )
42 {
43 ASSERTDEB_(c.globalIdx < alreadySelectedThis.size());
44 ASSERTDEB_(c.localIdx < alreadySelectedOther.size());
45
46 #ifndef AVOID_MULTIPLE_CORRESPONDENCES
47 alreadySelectedThis[c.globalIdx] = true;
48 alreadySelectedOther[c.localIdx] = true;
49 #else
50 for (std::vector<int>::iterator it1 = listDuplicatedLandmarksThis[c.globalIdx].begin();
51 it1 != listDuplicatedLandmarksThis[c.globalIdx].end(); it1++)
52 alreadySelectedThis[*it1] = true;
53 for (std::vector<int>::iterator it2 = listDuplicatedLandmarksOther[c.localIdx].begin();
54 it2 != listDuplicatedLandmarksOther[c.localIdx].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 ---------------------------------------------------------------*/
79 bool tfest::se2_l2_robust(
80 const mrpt::tfest::TMatchingPairList& in_correspondences,
81 const double normalizationStd,
82 const TSE2RobustParams& params,
83 TSE2RobustResult& results)
84 {
85 //#define DO_PROFILING
86
87 #ifdef DO_PROFILING
88 CTimeLogger timlog;
89 #endif
90
91 const size_t nCorrs = in_correspondences.size();
92
93 // Default: 2 * normalizationStd ("noise level")
94 const double MAX_RMSE_TO_END =
95 (params.max_rmse_to_end <= 0 ? 2 * normalizationStd : params.max_rmse_to_end);
96
97 MRPT_START
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.globalIdx);
116 maxOther = max(maxOther, in_correspondence.localIdx);
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.globalIdx] && !hasCorrOther[in_correspondence.localIdx])
133 {
134 hasCorrThis[in_correspondence.globalIdx] = true;
135 hasCorrOther[in_correspondence.localIdx] = 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].global.x == in_correspondences[j].global.x &&
171 in_correspondences[k].global.y == in_correspondences[j].global.y &&
172 in_correspondences[k].global.z == in_correspondences[j].global.z)
173 duplis.push_back(in_correspondences[j].globalIdx);
174 }
175 listDuplicatedLandmarksThis[in_correspondences[k].globalIdx] = 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].local.x == in_correspondences[j].local.x &&
185 in_correspondences[k].local.y == in_correspondences[j].local.y &&
186 in_correspondences[k].local.z == in_correspondences[j].local.z)
187 duplis.push_back(in_correspondences[j].localIdx);
188 }
189 listDuplicatedLandmarksOther[in_correspondences[k].localIdx] = duplis;
190 }
191 #endif
192
193 std::deque<TMatchingPairList> alreadyAddedSubSets;
194
195 CPosePDFGaussian referenceEstimation;
196 CPoint2DPDFGaussian pt_this;
197
198 const double ransac_consistency_test_chi2_quantile = 0.99;
199 const double chi2_thres_dim1 = mrpt::math::chi2inv(ransac_consistency_test_chi2_quantile, 1);
200
201 // -------------------------
202 // The RANSAC loop
203 // -------------------------
204 size_t largest_consensus_yet = 0; // Used for dynamic # of steps
205 double largestSubSet_RMSE = std::numeric_limits<double>::max();
206
207 results.ransac_iters = params.ransac_nSimulations;
208 const bool use_dynamic_iter_number = results.ransac_iters == 0;
209 if (use_dynamic_iter_number)
210 {
211 ASSERT_(params.probability_find_good_model > 0 && params.probability_find_good_model < 1);
212 // Set an initial # of iterations:
213 results.ransac_iters = 10; // It doesn't matter actually, since will be
214 // changed in the first loop
215 }
216
217 std::vector<bool> alreadySelectedThis, alreadySelectedOther;
218
219 if (!params.ransac_algorithmForLandmarks)
220 {
221 alreadySelectedThis.assign(maxThis + 1, false);
222 alreadySelectedOther.assign(maxOther + 1, false);
223 }
224 // else -> It will be done anyway inside the for() below
225
226 // First: Build a permutation of the correspondences to pick from it
227 // sequentially:
228 std::vector<size_t> corrsIdxs(nCorrs), corrsIdxsPermutation;
229 for (size_t i = 0; i < nCorrs; i++) corrsIdxs[i] = i;
230
231 size_t iter_idx;
232 for (iter_idx = 0; iter_idx < results.ransac_iters;
233 iter_idx++) // results.ransac_iters can be dynamic
234 {
235 #ifdef DO_PROFILING
236 CTimeLoggerEntry tle(timlog, "ransac.iter");
237 #endif
238
239 #ifdef DO_PROFILING
240 timlog.enter("ransac.permute");
241 #endif
242 getRandomGenerator().permuteVector(corrsIdxs, corrsIdxsPermutation);
243
244 #ifdef DO_PROFILING
245 timlog.leave("ransac.permute");
246 #endif
247
248 TMatchingPairList subSet;
249
250 // Select a subset of correspondences at random:
251 if (params.ransac_algorithmForLandmarks)
252 {
253 #ifdef DO_PROFILING
254 timlog.enter("ransac.reset_selection_marks");
255 #endif
256 alreadySelectedThis.assign(maxThis + 1, false);
257 alreadySelectedOther.assign(maxOther + 1, false);
258 #ifdef DO_PROFILING
259 timlog.leave("ransac.reset_selection_marks");
260 #endif
261 }
262 else
263 {
264 // For points: Do not repeat the corrs, and take the number of corrs
265 // as weights
266 }
267
268 // Try to build a subsetof "ransac_maxSetSize" (maximum) elements that achieve
269 // consensus:
270 // ------------------------------------------------------------------------------------------
271 #ifdef DO_PROFILING
272 timlog.enter("ransac.inner_loops");
273 #endif
274 for (unsigned int j = 0; j < nCorrs && subSet.size() < params.ransac_maxSetSize; j++)
275 {
276 const size_t idx = corrsIdxsPermutation[j];
277 const auto& corr_j = in_correspondences[idx];
278
279 // Don't pick the same features twice!
280 if (alreadySelectedThis[corr_j.globalIdx] || alreadySelectedOther[corr_j.localIdx]) continue;
281
282 // Additional user-provided filter:
283 if (params.user_individual_compat_callback)
284 {
285 mrpt::tfest::TPotentialMatch pm;
286 pm.idx_this = corr_j.globalIdx;
287 pm.idx_other = corr_j.localIdx;
288 if (!params.user_individual_compat_callback(pm)) continue; // Skip this one!
289 }
290
291 if (subSet.size() < 2)
292 {
293 // ------------------------------------------------------------------------------------------------------
294 // If we are within the first two correspondences, just add them
295 // to the subset:
296 // ------------------------------------------------------------------------------------------------------
297 subSet.push_back(corr_j);
298 markAsPicked(corr_j, alreadySelectedThis, alreadySelectedOther);
299
300 if (subSet.size() == 2)
301 {
302 // Consistency Test: From
303
304 // Check the feasibility of this pair "idx1"-"idx2":
305 // The distance between the pair of points in MAP1 must be
306 // very close
307 // to that of their correspondences in MAP2:
308 const double corrs_dist1 = mrpt::math::distanceBetweenPoints(
309 subSet[0].global.x, subSet[0].global.y, subSet[1].global.x, subSet[1].global.y);
310
311 const double corrs_dist2 = mrpt::math::distanceBetweenPoints(
312 subSet[0].local.x, subSet[0].local.y, subSet[1].local.x, subSet[1].local.y);
313
314 // Is is a consistent possibility?
315 // We use a chi2 test (see paper for the derivation)
316 const double corrs_dist_chi2 =
317 square(square(corrs_dist1) - square(corrs_dist2)) /
318 (8.0 * square(normalizationStd) * (square(corrs_dist1) + square(corrs_dist2)));
319
320 bool is_acceptable = (corrs_dist_chi2 < chi2_thres_dim1);
321
322 if (is_acceptable)
323 {
324 // Perform estimation:
325 tfest::se2_l2(subSet, referenceEstimation);
326 // Normalized covariance: scale!
327 referenceEstimation.cov *= square(normalizationStd);
328
329 // Additional filter:
330 // If the correspondences as such the transformation
331 // has a high ambiguity, we discard it!
332 is_acceptable = (referenceEstimation.cov(2, 2) < square(DEG2RAD(5.0f)));
333 }
334
335 if (!is_acceptable)
336 {
337 // Remove this correspondence & try again with a
338 // different pair:
339 subSet.erase(subSet.begin() + (subSet.size() - 1));
340 }
341 else
342 {
343 // Only mark as picked if we're really keeping it:
344 markAsPicked(corr_j, alreadySelectedThis, alreadySelectedOther);
345 }
346 }
347 }
348 else
349 {
350 #ifdef DO_PROFILING
351 timlog.enter("ransac.test_consistency");
352 #endif
353
354 // ------------------------------------------------------------------------------------------------------
355 // The normal case:
356 // - test for "consensus" with the current group:
357 // - If it is compatible (ransac_maxErrorXY,
358 // ransac_maxErrorPHI), grow the "consensus set"
359 // - If not, do not add it.
360 // ------------------------------------------------------------------------------------------------------
361
362 // Test for the mahalanobis distance between:
363 // "referenceEstimation (+) point_other" AND "point_this"
364 referenceEstimation.composePoint(
365 mrpt::math::TPoint2D(corr_j.local.x, corr_j.local.y), pt_this);
366
367 const double maha_dist =
368 pt_this.mahalanobisDistanceToPoint(corr_j.global.x, corr_j.global.y);
369
370 const bool passTest = maha_dist < params.ransac_mahalanobisDistanceThreshold;
371
372 if (passTest)
373 {
374 // OK, consensus passed:
375 subSet.push_back(corr_j);
376 markAsPicked(corr_j, alreadySelectedThis, alreadySelectedOther);
377 }
378 // else -> Test failed
379
380 #ifdef DO_PROFILING
381 timlog.leave("ransac.test_consistency");
382 #endif
383 } // end else "normal case"
384
385 } // end for j
386 #ifdef DO_PROFILING
387 timlog.leave("ransac.inner_loops");
388 #endif
389
390 const bool has_to_eval_RMSE = (subSet.size() >= params.ransac_minSetSize);
391
392 // Compute the RMSE of this matching and the corresponding
393 // transformation (only if we'll use this value below)
394 double this_subset_RMSE = 0;
395 if (has_to_eval_RMSE)
396 {
397 #ifdef DO_PROFILING
398 CTimeLoggerEntry tle(timlog, "ransac.comp_rmse");
399 #endif
400
401 // Recompute referenceEstimation from all the corrs:
402 tfest::se2_l2(subSet, referenceEstimation);
403 // Normalized covariance: scale!
404 referenceEstimation.cov *= square(normalizationStd);
405
406 for (size_t k = 0; k < subSet.size(); k++)
407 {
408 double gx, gy;
409 referenceEstimation.mean.composePoint(subSet[k].local.x, subSet[k].local.y, gx, gy);
410
411 this_subset_RMSE += mrpt::math::distanceSqrBetweenPoints<double>(
412 subSet[k].global.x, subSet[k].global.y, gx, gy);
413 }
414 this_subset_RMSE /= std::max(static_cast<size_t>(1), subSet.size());
415 }
416 else
417 {
418 this_subset_RMSE = std::numeric_limits<double>::max();
419 }
420
421 // Save the estimation result as a "particle", only if the subSet
422 // contains
423 // "ransac_minSetSize" elements at least:
424 if (subSet.size() >= params.ransac_minSetSize)
425 {
426 // If this subset was previously added to the SOG, just increment
427 // its weight
428 // and do not add a new mode:
429 int indexFound = -1;
430
431 // JLBC Added DEC-2007: An alternative (optional) method to fuse
432 // Gaussian modes:
433 if (!params.ransac_fuseByCorrsMatch)
434 {
435 // Find matching by approximate match in the X,Y,PHI means
436 // -------------------------------------------------------------------
437 for (size_t i = 0; i < results.transformation.size(); i++)
438 {
439 double diffXY = results.transformation.get(i).mean.distanceTo(referenceEstimation.mean);
440 double diffPhi = fabs(math::wrapToPi(
441 results.transformation.get(i).mean.phi() - referenceEstimation.mean.phi()));
442 if (diffXY < params.ransac_fuseMaxDiffXY && diffPhi < params.ransac_fuseMaxDiffPhi)
443 {
444 // printf("Match by distance found: distXY:%f distPhi=%f
445 // deg\n",diffXY,RAD2DEG(diffPhi));
446 indexFound = i;
447 break;
448 }
449 }
450 }
451 else
452 {
453 // Find matching mode by exact match in the list of
454 // correspondences:
455 // -------------------------------------------------------------------
456 // Sort "subSet" in order to compare them easily!
457 // std::sort( subSet.begin(), subSet.end() );
458
459 // Try to find matching corrs:
460 for (size_t i = 0; i < alreadyAddedSubSets.size(); i++)
461 {
462 if (subSet == alreadyAddedSubSets[i])
463 {
464 indexFound = i;
465 break;
466 }
467 }
468 }
469
470 if (indexFound != -1)
471 {
472 // This is an already added mode:
473 if (params.ransac_algorithmForLandmarks)
474 results.transformation.get(indexFound).log_w =
475 log(1 + exp(results.transformation.get(indexFound).log_w));
476 else
477 results.transformation.get(indexFound).log_w =
478 log(subSet.size() + exp(results.transformation.get(indexFound).log_w));
479 }
480 else
481 {
482 // Add a new mode to the SOG:
483 alreadyAddedSubSets.push_back(subSet);
484
485 CPosePDFSOG::TGaussianMode newSOGMode;
486 if (params.ransac_algorithmForLandmarks)
487 newSOGMode.log_w = 0; // log(1);
488 else
489 newSOGMode.log_w = log(static_cast<double>(subSet.size()));
490
491 newSOGMode.mean = referenceEstimation.mean;
492 newSOGMode.cov = referenceEstimation.cov;
493
494 // Add a new mode to the SOG!
495 results.transformation.push_back(newSOGMode);
496 }
497 } // end if subSet.size()>=ransac_minSetSize
498
499 const size_t ninliers = subSet.size();
500 if (largest_consensus_yet < ninliers)
501 {
502 largest_consensus_yet = ninliers;
503
504 // Dynamic # of steps:
505 if (use_dynamic_iter_number)
506 {
507 // Update estimate of nCorrs, the number of trials to ensure we
508 // pick,
509 // with probability p, a data set with no outliers.
510 const double fracinliers =
511 ninliers / static_cast<double>(howManyDifCorrs); // corrsIdxs.size());
512 double pNoOutliers =
513 1 - pow(fracinliers, static_cast<double>(2.0 /*minimumSizeSamplesToFit*/));
514
515 pNoOutliers = std::max(
516 std::numeric_limits<double>::epsilon(),
517 pNoOutliers); // Avoid division by -Inf
518 pNoOutliers = std::min(
519 1.0 - std::numeric_limits<double>::epsilon(),
520 pNoOutliers); // Avoid division by 0.
521 // Number of
522 results.ransac_iters =
523 mrpt::round(log(1 - params.probability_find_good_model) / log(pNoOutliers));
524
525 results.ransac_iters = std::max(results.ransac_iters, params.ransac_min_nSimulations);
526
527 if (params.verbose)
528 cout << "[tfest::RANSAC] Iter #" << iter_idx << ":est. # iters=" << results.ransac_iters
529 << " pNoOutliers=" << pNoOutliers << " #inliers: " << ninliers << endl;
530 }
531 }
532
533 // Save the largest subset:
534 if (subSet.size() >= params.ransac_minSetSize && this_subset_RMSE < largestSubSet_RMSE)
535 {
536 if (params.verbose)
537 cout << "[tfest::RANSAC] Iter #" << iter_idx << " Better subset: " << subSet.size()
538 << " inliers, RMSE=" << this_subset_RMSE << endl;
539
540 results.largestSubSet = subSet;
541 largestSubSet_RMSE = this_subset_RMSE;
542 }
543
544 // Is the found subset good enough?
545 if (subSet.size() >= params.ransac_minSetSize && this_subset_RMSE < MAX_RMSE_TO_END)
546 {
547 break; // end RANSAC iterations.
548 }
549
550 #ifdef DO_PROFILING
551 timlog.leave("ransac.iter");
552 #endif
553 } // end for each iteration
554
555 if (params.verbose) cout << "[tfest::RANSAC] Finished after " << iter_idx << " iterations.\n";
556
557 // Set the weights of the particles to sum the unity:
558 results.transformation.normalizeWeights();
559
560 // Done!
561
562 MRPT_END_WITH_CLEAN_UP(printf("nCorrs=%u\n", static_cast<unsigned int>(nCorrs));
563 printf("Saving '_debug_in_correspondences.txt'...");
564 in_correspondences.dumpToFile("_debug_in_correspondences.txt");
565 printf("Ok\n"); printf("Saving '_debug_results.transformation.txt'...");
566 results.transformation.saveToTextFile("_debug_results.transformation.txt");
567 printf("Ok\n"););
568
569 return true;
570 }