Debian Codegraph @

libs/tfest/src/se3_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/utils.h> // linspace()
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/CPose3DQuat.h>
16 #include <mrpt/poses/Lie/SO.h>
17 #include <mrpt/random.h>
18 #include <mrpt/tfest/se3.h>
19
20 #include <iostream>
21 #include <numeric>
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 /*---------------------------------------------------------------
31 se3_l2_robust
32 ---------------------------------------------------------------*/
33 bool tfest::se3_l2_robust(
34 const mrpt::tfest::TMatchingPairList& in_correspondences,
35 const TSE3RobustParams& params,
36 TSE3RobustResult& results)
37 {
38 MRPT_START
39
40 const size_t nCorrs = in_correspondences.size();
41
42 // -------------------------------------------
43 // RANSAC parameters
44 // -------------------------------------------
45 // Minimum error achieved so far
46 double min_err = std::numeric_limits<double>::max();
47 size_t max_size = 0; // Maximum size of the consensus set so far
48 double scale; // Output scale
49
50 // Minimum number of points to fit the model
51 const size_t n = params.ransac_minSetSize;
52 // Minimum number of points to be considered a good set
53 const size_t d = mrpt::round(nCorrs * params.ransac_maxSetSizePct);
54 // Maximum number of iterations
55 const size_t maxIters = params.ransac_nmaxSimulations;
56
57 ASSERTMSG_(
58 d >= n,
59 "Minimum number of points to be considered a good set is < Minimum "
60 "number of points to fit the model");
61
62 // -------------------------------------------
63 // MAIN loop
64 // -------------------------------------------
65 for (size_t iterations = 0; iterations < maxIters; iterations++)
66 {
67 if (params.verbose)
68 std::cout << "[tfest::se3_l2_robust] Iteration " << (iterations + 1) << "/" << maxIters
69 << "\n";
70
71 // Generate maybe inliers
72 const auto rub = mrpt::math::linspace<uint32_t>(0, nCorrs - 1, nCorrs);
73 const auto mbSet = getRandomGenerator().permuteVector(rub);
74
75 std::vector<uint32_t> cSet; // consensus set
76
77 // Compute first inliers output
78 TMatchingPairList mbInliers;
79 mbInliers.reserve(n);
80 for (size_t i = 0; mbInliers.size() < n && i < nCorrs; i++)
81 {
82 const size_t idx = mbSet[i];
83
84 // User-provided filter:
85 if (params.user_individual_compat_callback)
86 {
87 mrpt::tfest::TPotentialMatch pm;
88 pm.idx_this = in_correspondences[idx].globalIdx;
89 pm.idx_other = in_correspondences[idx].localIdx;
90 if (!params.user_individual_compat_callback(pm)) continue; // Skip this one!
91 }
92
93 mbInliers.push_back(in_correspondences[idx]);
94 cSet.push_back(idx);
95 }
96
97 // Check minimum number:
98 if (cSet.size() < n)
99 {
100 if (params.verbose)
101 std::cerr << "[tfest::se3_l2_robust] Iter " << iterations
102 << ": It was not possible to find the min no of "
103 "(compatible) matching pairs.\n";
104 continue; // Try again
105 }
106
107 CPose3DQuat mbOutQuat;
108 bool res = mrpt::tfest::se3_l2(mbInliers, mbOutQuat, scale, params.forceScaleToUnity);
109 if (!res)
110 {
111 std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned "
112 "false for tentative subset during RANSAC "
113 "iteration!\n";
114 continue;
115 }
116
117 // Maybe inliers Output
118 const CPose3D mbOut = CPose3D(mbOutQuat);
119 CVectorDouble mbOut_vec(7);
120 mbOut_vec[0] = mbOut.x();
121 mbOut_vec[1] = mbOut.y();
122 mbOut_vec[2] = mbOut.z();
123
124 mbOut_vec[3] = mbOut.yaw();
125 mbOut_vec[4] = mbOut.pitch();
126 mbOut_vec[5] = mbOut.roll();
127
128 mbOut_vec[6] = scale;
129
130 // Inner loop: for each point NOT in the maybe inliers
131 for (size_t k = n; k < nCorrs; k++)
132 {
133 const size_t idx = mbSet[k];
134
135 // User-provided filter:
136 if (params.user_individual_compat_callback)
137 {
138 mrpt::tfest::TPotentialMatch pm;
139 pm.idx_this = in_correspondences[idx].globalIdx;
140 pm.idx_other = in_correspondences[idx].localIdx;
141 if (!params.user_individual_compat_callback(pm)) continue; // Skip this one!
142 }
143
144 // Consensus set: Maybe inliers + new point
145 CPose3DQuat csOutQuat;
146 mbInliers.push_back(in_correspondences[idx]); // Insert
147 res = mrpt::tfest::se3_l2(mbInliers, csOutQuat, scale, params.forceScaleToUnity);
148 mbInliers.erase(mbInliers.end() - 1); // Erase
149
150 if (!res)
151 {
152 std::cerr << "[tfest::se3_l2_robust] tfest::se3_l2() returned "
153 "false for tentative subset during RANSAC "
154 "iteration!\n";
155 continue;
156 }
157
158 // Is this point a supporter of the initial inlier group?
159 const CPose3D csOut = CPose3D(csOutQuat);
160
161 const double linDist = mbOut.distanceTo(csOut);
162 const double angDist =
163 mrpt::poses::Lie::SO<3>::log((csOut - mbOut).getRotationMatrix()).norm();
164 const double scaleDist = std::abs(mbOut_vec[6] - scale);
165
166 if (linDist < params.ransac_threshold_lin && angDist < params.ransac_threshold_ang &&
167 scaleDist < params.ransac_threshold_scale)
168 {
169 // Inlier detected -> add to the inlier list
170 cSet.push_back(idx);
171 }
172 else
173 {
174 // cout << " It " << iterations << " - RANSAC Outlier Detected:
175 // " << k << endl;
176 }
177 } // end 'inner' for
178
179 // Test cSet size
180 if (cSet.size() >= d)
181 {
182 // Good set of points found
183 TMatchingPairList cSetInliers;
184 cSetInliers.resize(cSet.size());
185 for (size_t m = 0; m < cSet.size(); m++) cSetInliers[m] = in_correspondences[cSet[m]];
186
187 // Compute output: Consensus Set + Initial Inliers Guess
188 CPose3DQuat cIOutQuat;
189 res = mrpt::tfest::se3_l2(
190 cSetInliers, cIOutQuat, scale,
191 params.forceScaleToUnity); // Compute output
192 ASSERTMSG_(
193 res,
194 "tfest::se3_l2() returned false for tentative subset during "
195 "RANSAC iteration!");
196
197 // Compute error for consensus_set
198 const CPose3D cIOut = CPose3D(cIOutQuat);
199 const double err = std::sqrt(
200 square(mbOut_vec[0] - cIOut.x()) + square(mbOut_vec[1] - cIOut.y()) +
201 square(mbOut_vec[2] - cIOut.z()) + square(mbOut_vec[3] - cIOut.yaw()) +
202 square(mbOut_vec[4] - cIOut.pitch()) + square(mbOut_vec[5] - cIOut.roll()) +
203 square(mbOut_vec[6] - scale));
204
205 // Is the best set of points so far?
206 if (err < min_err && cSet.size() >= max_size)
207 {
208 min_err = err;
209 max_size = cSet.size();
210 results.transformation = cIOutQuat;
211 results.scale = scale;
212 results.inliers_idx = cSet;
213 }
214 // printf(" - Consensus set size: %u - Error: %.6f\n", (unsigned
215 // int)cSet.size(), err );
216 } // end if cSet.size() > d
217 else
218 {
219 // printf(" - Consensus set size: %u - Not big enough!\n", (unsigned
220 // int)cSet.size() );
221 }
222 } // end 'iterations' for
223
224 if (max_size == 0)
225 {
226 if (params.verbose) std::cerr << "[se3_l2_robust] No solution found, maximum size is == 0!\n";
227 return false;
228 }
229
230 return true;
231 MRPT_END
232 }