31 ASSERT_(useIndices.size() == 3);
34 allData(0, useIndices[0]), allData(1, useIndices[0]),
35 allData(2, useIndices[0]));
37 allData(0, useIndices[1]), allData(1, useIndices[1]),
38 allData(2, useIndices[1]));
40 allData(0, useIndices[2]), allData(1, useIndices[2]),
41 allData(2, useIndices[2]));
50 for (
size_t i = 0; i < 4; i++) M(0, i) = T(plane.
coefs[i]);
63 unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices)
65 ASSERT_(testModels.size() == 1);
66 out_bestModelIndex = 0;
72 plane.
coefs[0] = M(0, 0);
73 plane.
coefs[1] = M(0, 1);
74 plane.
coefs[2] = M(0, 2);
75 plane.
coefs[3] = M(0, 3);
77 const size_t N = allData.
cols();
78 out_inlierIndices.clear();
79 out_inlierIndices.reserve(100);
80 for (
size_t i = 0; i < N; i++)
83 TPoint3D(allData(0, i), allData(1, i), allData(2, i)));
84 if (d < distanceThreshold) out_inlierIndices.push_back(i);
93 [[maybe_unused]]
const std::vector<size_t>& useIndices)
103 template <
typename NUMTYPE>
107 vector<pair<size_t, TPlane>>& out_detected_planes,
const double threshold,
108 const size_t min_inliers_for_valid_plane)
114 out_detected_planes.clear();
116 if (x.
empty())
return;
120 remainingPoints.setRow(0, x);
121 remainingPoints.setRow(1, y);
122 remainingPoints.setRow(2, z);
129 std::vector<size_t> this_best_inliers;
135 remainingPoints, mrpt::math::ransac3Dplane_fit<NUMTYPE>,
136 mrpt::math::ransac3Dplane_distance<NUMTYPE>,
137 mrpt::math::ransac3Dplane_degenerate<NUMTYPE>, threshold,
139 this_best_inliers, this_best_model,
144 if (this_best_inliers.size() >= min_inliers_for_valid_plane)
147 out_detected_planes.emplace_back(
148 this_best_inliers.size(),
TPlane(
149 double(this_best_model(0, 0)),
150 double(this_best_model(0, 1)),
151 double(this_best_model(0, 2)),
152 double(this_best_model(0, 3))));
154 out_detected_planes.rbegin()->second.unitarize();
158 remainingPoints.removeColumns(this_best_inliers);
170 #define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_) \
171 template void mrpt::math::ransac_detect_3D_planes<_TYPE_>( \
172 const CVectorDynamic<_TYPE_>& x, const CVectorDynamic<_TYPE_>& y, \
173 const CVectorDynamic<_TYPE_>& z, \
174 vector<pair<size_t, TPlane>>& out_detected_planes, \
175 const double threshold, const size_t min_inliers_for_valid_plane)
187 template <
typename T>
192 ASSERT_(useIndices.size() == 2);
194 TPoint2D p1(allData(0, useIndices[0]), allData(1, useIndices[0]));
195 TPoint2D p2(allData(0, useIndices[1]), allData(1, useIndices[1]));
204 for (
size_t i = 0; i < 3; i++) M(0, i) = static_cast<T>(line.
coefs[i]);
213 template <
typename T>
217 unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices)
219 out_inlierIndices.clear();
220 out_bestModelIndex = 0;
222 if (testModels.empty())
return;
225 testModels.size() == 1,
227 "Expected testModels.size()=1, but it's = %u",
228 static_cast<unsigned int>(testModels.size())));
234 line.
coefs[0] = M(0, 0);
235 line.
coefs[1] = M(0, 1);
236 line.
coefs[2] = M(0, 2);
238 const size_t N = allData.
cols();
239 out_inlierIndices.reserve(100);
240 for (
size_t i = 0; i < N; i++)
243 if (d < distanceThreshold) out_inlierIndices.push_back(i);
249 template <
typename T>
252 [[maybe_unused]]
const std::vector<size_t>& useIndices)
262 template <
typename NUMTYPE>
265 std::vector<std::pair<size_t, TLine2D>>& out_detected_lines,
266 const double threshold,
const size_t min_inliers_for_valid_line)
270 out_detected_lines.clear();
272 if (x.
empty())
return;
276 remainingPoints.
setRow(0, x);
277 remainingPoints.
setRow(1, y);
282 while (remainingPoints.
cols() >= 2)
284 std::vector<size_t> this_best_inliers;
290 remainingPoints, ransac2Dline_fit<NUMTYPE>,
291 ransac2Dline_distance<NUMTYPE>, ransac2Dline_degenerate<NUMTYPE>,
294 this_best_inliers, this_best_model,
299 if (this_best_inliers.size() >= min_inliers_for_valid_line)
302 out_detected_lines.emplace_back(
303 this_best_inliers.size(),
TLine2D(
304 double(this_best_model(0, 0)),
305 double(this_best_model(0, 1)),
306 double(this_best_model(0, 2))));
308 out_detected_lines.rbegin()->second.unitarize();
324 #define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_) \
325 template void mrpt::math::ransac_detect_2D_lines<_TYPE_>( \
326 const CVectorDynamic<_TYPE_>& x, const CVectorDynamic<_TYPE_>& y, \
327 std::vector<std::pair<size_t, TLine2D>>& out_detected_lines, \
328 const double threshold, const size_t min_inliers_for_valid_line)