MRPT  2.0.2
ransac_applications.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 "math-precomp.h" // Precompiled headers
11 
12 //#include <mrpt/math/eigen_extensions.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::math;
17 using namespace std;
18 
19 /*---------------------------------------------------------------
20  Aux. functions needed by ransac_detect_3D_planes
21  ---------------------------------------------------------------*/
22 namespace mrpt
23 {
24 namespace math
25 {
26 template <typename T>
28  const CMatrixDynamic<T>& allData, const std::vector<size_t>& useIndices,
29  vector<CMatrixDynamic<T>>& fitModels)
30 {
31  ASSERT_(useIndices.size() == 3);
32 
33  TPoint3D p1(
34  allData(0, useIndices[0]), allData(1, useIndices[0]),
35  allData(2, useIndices[0]));
36  TPoint3D p2(
37  allData(0, useIndices[1]), allData(1, useIndices[1]),
38  allData(2, useIndices[1]));
39  TPoint3D p3(
40  allData(0, useIndices[2]), allData(1, useIndices[2]),
41  allData(2, useIndices[2]));
42 
43  try
44  {
45  TPlane plane(p1, p2, p3);
46  fitModels.resize(1);
47  CMatrixDynamic<T>& M = fitModels[0];
48 
49  M.setSize(1, 4);
50  for (size_t i = 0; i < 4; i++) M(0, i) = T(plane.coefs[i]);
51  }
52  catch (exception&)
53  {
54  fitModels.clear();
55  return;
56  }
57 }
58 
59 template <typename T>
61  const CMatrixDynamic<T>& allData,
62  const vector<CMatrixDynamic<T>>& testModels, const T distanceThreshold,
63  unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices)
64 {
65  ASSERT_(testModels.size() == 1);
66  out_bestModelIndex = 0;
67  const CMatrixDynamic<T>& M = testModels[0];
68 
69  ASSERT_(M.rows() == 1 && M.cols() == 4);
70 
71  TPlane plane;
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);
76 
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++)
81  {
82  const double d = plane.distance(
83  TPoint3D(allData(0, i), allData(1, i), allData(2, i)));
84  if (d < distanceThreshold) out_inlierIndices.push_back(i);
85  }
86 }
87 
88 /** Return "true" if the selected points are a degenerate (invalid) case.
89  */
90 template <typename T>
92  [[maybe_unused]] const CMatrixDynamic<T>& allData,
93  [[maybe_unused]] const std::vector<size_t>& useIndices)
94 {
95  return false;
96 }
97 } // namespace math
98 } // namespace mrpt
99 
100 /*---------------------------------------------------------------
101  ransac_detect_3D_planes
102  ---------------------------------------------------------------*/
103 template <typename NUMTYPE>
106  const CVectorDynamic<NUMTYPE>& z,
107  vector<pair<size_t, TPlane>>& out_detected_planes, const double threshold,
108  const size_t min_inliers_for_valid_plane)
109 {
110  MRPT_START
111 
112  ASSERT_(x.size() == y.size() && x.size() == z.size());
113 
114  out_detected_planes.clear();
115 
116  if (x.empty()) return;
117 
118  // The running lists of remaining points after each plane, as a matrix:
119  CMatrixDynamic<NUMTYPE> remainingPoints(3, x.size());
120  remainingPoints.setRow(0, x);
121  remainingPoints.setRow(1, y);
122  remainingPoints.setRow(2, z);
123 
124  // ---------------------------------------------
125  // For each plane:
126  // ---------------------------------------------
127  for (;;)
128  {
129  std::vector<size_t> this_best_inliers;
130  CMatrixDynamic<NUMTYPE> this_best_model;
131 
134  ransac.execute(
135  remainingPoints, mrpt::math::ransac3Dplane_fit<NUMTYPE>,
136  mrpt::math::ransac3Dplane_distance<NUMTYPE>,
137  mrpt::math::ransac3Dplane_degenerate<NUMTYPE>, threshold,
138  3, // Minimum set of points
139  this_best_inliers, this_best_model,
140  0.999 // Prob. of good result
141  );
142 
143  // Is this plane good enough?
144  if (this_best_inliers.size() >= min_inliers_for_valid_plane)
145  {
146  // Add this plane to the output list:
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))));
153 
154  out_detected_planes.rbegin()->second.unitarize();
155 
156  // Discard the selected points so they are not used again for
157  // finding subsequent planes:
158  remainingPoints.removeColumns(this_best_inliers);
159  }
160  else
161  {
162  break; // Do not search for more planes.
163  }
164  }
165 
166  MRPT_END
167 }
168 
169 // Template explicit instantiations:
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)
176 
179 
180 /*---------------------------------------------------------------
181  Aux. functions needed by ransac_detect_2D_lines
182  ---------------------------------------------------------------*/
183 namespace mrpt
184 {
185 namespace math
186 {
187 template <typename T>
189  const CMatrixDynamic<T>& allData, const std::vector<size_t>& useIndices,
190  vector<CMatrixDynamic<T>>& fitModels)
191 {
192  ASSERT_(useIndices.size() == 2);
193 
194  TPoint2D p1(allData(0, useIndices[0]), allData(1, useIndices[0]));
195  TPoint2D p2(allData(0, useIndices[1]), allData(1, useIndices[1]));
196 
197  try
198  {
199  TLine2D line(p1, p2);
200  fitModels.resize(1);
201  CMatrixDynamic<T>& M = fitModels[0];
202 
203  M.setSize(1, 3);
204  for (size_t i = 0; i < 3; i++) M(0, i) = static_cast<T>(line.coefs[i]);
205  }
206  catch (exception&)
207  {
208  fitModels.clear();
209  return;
210  }
211 }
212 
213 template <typename T>
215  const CMatrixDynamic<T>& allData,
216  const vector<CMatrixDynamic<T>>& testModels, const T distanceThreshold,
217  unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices)
218 {
219  out_inlierIndices.clear();
220  out_bestModelIndex = 0;
221 
222  if (testModels.empty()) return; // No model, no inliers.
223 
224  ASSERTMSG_(
225  testModels.size() == 1,
226  format(
227  "Expected testModels.size()=1, but it's = %u",
228  static_cast<unsigned int>(testModels.size())));
229  const CMatrixDynamic<T>& M = testModels[0];
230 
231  ASSERT_(M.rows() == 1 && M.cols() == 3);
232 
233  TLine2D line;
234  line.coefs[0] = M(0, 0);
235  line.coefs[1] = M(0, 1);
236  line.coefs[2] = M(0, 2);
237 
238  const size_t N = allData.cols();
239  out_inlierIndices.reserve(100);
240  for (size_t i = 0; i < N; i++)
241  {
242  const double d = line.distance(TPoint2D(allData(0, i), allData(1, i)));
243  if (d < distanceThreshold) out_inlierIndices.push_back(i);
244  }
245 }
246 
247 /** Return "true" if the selected points are a degenerate (invalid) case.
248  */
249 template <typename T>
251  [[maybe_unused]] const CMatrixDynamic<T>& allData,
252  [[maybe_unused]] const std::vector<size_t>& useIndices)
253 {
254  return false;
255 }
256 } // namespace math
257 } // namespace mrpt
258 
259 /*---------------------------------------------------------------
260  ransac_detect_2D_lines
261  ---------------------------------------------------------------*/
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)
267 {
268  MRPT_START
269  ASSERT_(x.size() == y.size());
270  out_detected_lines.clear();
271 
272  if (x.empty()) return;
273 
274  // The running lists of remaining points after each plane, as a matrix:
275  CMatrixDynamic<NUMTYPE> remainingPoints(2, x.size());
276  remainingPoints.setRow(0, x);
277  remainingPoints.setRow(1, y);
278 
279  // ---------------------------------------------
280  // For each line:
281  // ---------------------------------------------
282  while (remainingPoints.cols() >= 2)
283  {
284  std::vector<size_t> this_best_inliers;
285  CMatrixDynamic<NUMTYPE> this_best_model;
286 
289  ransac.execute(
290  remainingPoints, ransac2Dline_fit<NUMTYPE>,
291  ransac2Dline_distance<NUMTYPE>, ransac2Dline_degenerate<NUMTYPE>,
292  threshold,
293  2, // Minimum set of points
294  this_best_inliers, this_best_model,
295  0.99999 // Prob. of good result
296  );
297 
298  // Is this plane good enough?
299  if (this_best_inliers.size() >= min_inliers_for_valid_line)
300  {
301  // Add this plane to the output list:
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))));
307 
308  out_detected_lines.rbegin()->second.unitarize();
309 
310  // Discard the selected points so they are not used again for
311  // finding subsequent planes:
312  remainingPoints.removeColumns(this_best_inliers);
313  }
314  else
315  {
316  break; // Do not search for more planes.
317  }
318  }
319 
320  MRPT_END
321 }
322 
323 // Template explicit instantiations:
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)
329 
bool ransac2Dline_degenerate([[maybe_unused]] const CMatrixDynamic< T > &allData, [[maybe_unused]] const std::vector< size_t > &useIndices)
Return "true" if the selected points are a degenerate (invalid) case.
#define MRPT_START
Definition: exceptions.h:241
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
Template for column vectors of dynamic size, compatible with Eigen.
void ransac2Dline_distance(const CMatrixDynamic< T > &allData, const vector< CMatrixDynamic< T >> &testModels, const T distanceThreshold, unsigned int &out_bestModelIndex, std::vector< size_t > &out_inlierIndices)
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool ransac3Dplane_degenerate([[maybe_unused]] const CMatrixDynamic< T > &allData, [[maybe_unused]] const std::vector< size_t > &useIndices)
Return "true" if the selected points are a degenerate (invalid) case.
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
void setRow(const Index row, const VECTOR &v)
STL namespace.
void ransac3Dplane_fit(const CMatrixDynamic< T > &allData, const std::vector< size_t > &useIndices, vector< CMatrixDynamic< T >> &fitModels)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void ransac_detect_3D_planes(const CVectorDynamic< NUMTYPE > &x, const CVectorDynamic< NUMTYPE > &y, const CVectorDynamic< NUMTYPE > &z, std::vector< std::pair< size_t, TPlane >> &out_detected_planes, const double threshold, const size_t min_inliers_for_valid_plane=10)
Fit a number of 3-D planes to a given point cloud, automatically determining the number of existing p...
This base provides a set of functions for maths stuff.
void ransac2Dline_fit(const CMatrixDynamic< T > &allData, const std::vector< size_t > &useIndices, vector< CMatrixDynamic< T >> &fitModels)
#define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_)
3D Plane, represented by its equation
Definition: TPlane.h:22
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
void ransac_detect_2D_lines(const CVectorDynamic< NUMTYPE > &x, const CVectorDynamic< NUMTYPE > &y, std::vector< std::pair< size_t, TLine2D >> &out_detected_lines, const double threshold, const size_t min_inliers_for_valid_line=5)
Fit a number of 2-D lines to a given point cloud, automatically determining the number of existing li...
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:23
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
void ransac3Dplane_distance(const CMatrixDynamic< T > &allData, const vector< CMatrixDynamic< T >> &testModels, const T distanceThreshold, unsigned int &out_bestModelIndex, std::vector< size_t > &out_inlierIndices)
#define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_)
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:28
bool execute(const CMatrixDynamic< NUMTYPE > &data, const TRansacFitFunctor &fit_func, const TRansacDistanceFunctor &dist_func, const TRansacDegenerateFunctor &degen_func, const double distanceThreshold, const unsigned int minimumSizeSamplesToFit, std::vector< size_t > &out_best_inliers, CMatrixDynamic< NUMTYPE > &out_best_model, const double prob_good_sample=0.999, const size_t maxIter=2000) const
An implementation of the RANSAC algorithm for robust fitting of models to data.
Definition: ransac.cpp:24
This template class provides the basic functionality for a general 2D any-size, resizable container o...
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
Definition: TPlane.h:26
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19



Page generated by Doxygen 1.8.14 for MRPT 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020