MRPT  1.9.9
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-2019, 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  const CMatrixDynamic<T>& allData, const std::vector<size_t>& useIndices)
93 {
94  MRPT_UNUSED_PARAM(allData);
95  MRPT_UNUSED_PARAM(useIndices);
96  return false;
97 }
98 } // namespace math
99 } // namespace mrpt
100 
101 /*---------------------------------------------------------------
102  ransac_detect_3D_planes
103  ---------------------------------------------------------------*/
104 template <typename NUMTYPE>
107  const CVectorDynamic<NUMTYPE>& z,
108  vector<pair<size_t, TPlane>>& out_detected_planes, const double threshold,
109  const size_t min_inliers_for_valid_plane)
110 {
111  MRPT_START
112 
113  ASSERT_(x.size() == y.size() && x.size() == z.size());
114 
115  out_detected_planes.clear();
116 
117  if (x.empty()) return;
118 
119  // The running lists of remaining points after each plane, as a matrix:
120  CMatrixDynamic<NUMTYPE> remainingPoints(3, x.size());
121  remainingPoints.setRow(0, x);
122  remainingPoints.setRow(1, y);
123  remainingPoints.setRow(2, z);
124 
125  // ---------------------------------------------
126  // For each plane:
127  // ---------------------------------------------
128  for (;;)
129  {
130  std::vector<size_t> this_best_inliers;
131  CMatrixDynamic<NUMTYPE> this_best_model;
132 
135  ransac.execute(
136  remainingPoints, mrpt::math::ransac3Dplane_fit<NUMTYPE>,
137  mrpt::math::ransac3Dplane_distance<NUMTYPE>,
138  mrpt::math::ransac3Dplane_degenerate<NUMTYPE>, threshold,
139  3, // Minimum set of points
140  this_best_inliers, this_best_model,
141  0.999 // Prob. of good result
142  );
143 
144  // Is this plane good enough?
145  if (this_best_inliers.size() >= min_inliers_for_valid_plane)
146  {
147  // Add this plane to the output list:
148  out_detected_planes.emplace_back(
149  this_best_inliers.size(), TPlane(
150  double(this_best_model(0, 0)),
151  double(this_best_model(0, 1)),
152  double(this_best_model(0, 2)),
153  double(this_best_model(0, 3))));
154 
155  out_detected_planes.rbegin()->second.unitarize();
156 
157  // Discard the selected points so they are not used again for
158  // finding subsequent planes:
159  remainingPoints.removeColumns(this_best_inliers);
160  }
161  else
162  {
163  break; // Do not search for more planes.
164  }
165  }
166 
167  MRPT_END
168 }
169 
170 // Template explicit instantiations:
171 #define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_) \
172  template void mrpt::math::ransac_detect_3D_planes<_TYPE_>( \
173  const CVectorDynamic<_TYPE_>& x, const CVectorDynamic<_TYPE_>& y, \
174  const CVectorDynamic<_TYPE_>& z, \
175  vector<pair<size_t, TPlane>>& out_detected_planes, \
176  const double threshold, const size_t min_inliers_for_valid_plane)
177 
180 
181 /*---------------------------------------------------------------
182  Aux. functions needed by ransac_detect_2D_lines
183  ---------------------------------------------------------------*/
184 namespace mrpt
185 {
186 namespace math
187 {
188 template <typename T>
190  const CMatrixDynamic<T>& allData, const std::vector<size_t>& useIndices,
191  vector<CMatrixDynamic<T>>& fitModels)
192 {
193  ASSERT_(useIndices.size() == 2);
194 
195  TPoint2D p1(allData(0, useIndices[0]), allData(1, useIndices[0]));
196  TPoint2D p2(allData(0, useIndices[1]), allData(1, useIndices[1]));
197 
198  try
199  {
200  TLine2D line(p1, p2);
201  fitModels.resize(1);
202  CMatrixDynamic<T>& M = fitModels[0];
203 
204  M.setSize(1, 3);
205  for (size_t i = 0; i < 3; i++) M(0, i) = line.coefs[i];
206  }
207  catch (exception&)
208  {
209  fitModels.clear();
210  return;
211  }
212 }
213 
214 template <typename T>
216  const CMatrixDynamic<T>& allData,
217  const vector<CMatrixDynamic<T>>& testModels, const T distanceThreshold,
218  unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices)
219 {
220  out_inlierIndices.clear();
221  out_bestModelIndex = 0;
222 
223  if (testModels.empty()) return; // No model, no inliers.
224 
225  ASSERTMSG_(
226  testModels.size() == 1,
227  format(
228  "Expected testModels.size()=1, but it's = %u",
229  static_cast<unsigned int>(testModels.size())));
230  const CMatrixDynamic<T>& M = testModels[0];
231 
232  ASSERT_(M.rows() == 1 && M.cols() == 3);
233 
234  TLine2D line;
235  line.coefs[0] = M(0, 0);
236  line.coefs[1] = M(0, 1);
237  line.coefs[2] = M(0, 2);
238 
239  const size_t N = allData.cols();
240  out_inlierIndices.reserve(100);
241  for (size_t i = 0; i < N; i++)
242  {
243  const double d = line.distance(TPoint2D(allData(0, i), allData(1, i)));
244  if (d < distanceThreshold) out_inlierIndices.push_back(i);
245  }
246 }
247 
248 /** Return "true" if the selected points are a degenerate (invalid) case.
249  */
250 template <typename T>
252  const CMatrixDynamic<T>& allData, const std::vector<size_t>& useIndices)
253 {
254  MRPT_UNUSED_PARAM(allData);
255  MRPT_UNUSED_PARAM(useIndices);
256  return false;
257 }
258 } // namespace math
259 } // namespace mrpt
260 
261 /*---------------------------------------------------------------
262  ransac_detect_2D_lines
263  ---------------------------------------------------------------*/
264 template <typename NUMTYPE>
267  std::vector<std::pair<size_t, TLine2D>>& out_detected_lines,
268  const double threshold, const size_t min_inliers_for_valid_line)
269 {
270  MRPT_START
271  ASSERT_(x.size() == y.size());
272  out_detected_lines.clear();
273 
274  if (x.empty()) return;
275 
276  // The running lists of remaining points after each plane, as a matrix:
277  CMatrixDynamic<NUMTYPE> remainingPoints(2, x.size());
278  remainingPoints.setRow(0, x);
279  remainingPoints.setRow(1, y);
280 
281  // ---------------------------------------------
282  // For each line:
283  // ---------------------------------------------
284  while (remainingPoints.cols() >= 2)
285  {
286  std::vector<size_t> this_best_inliers;
287  CMatrixDynamic<NUMTYPE> this_best_model;
288 
291  ransac.execute(
292  remainingPoints, ransac2Dline_fit<NUMTYPE>,
293  ransac2Dline_distance<NUMTYPE>, ransac2Dline_degenerate<NUMTYPE>,
294  threshold,
295  2, // Minimum set of points
296  this_best_inliers, this_best_model,
297  0.99999 // Prob. of good result
298  );
299 
300  // Is this plane good enough?
301  if (this_best_inliers.size() >= min_inliers_for_valid_line)
302  {
303  // Add this plane to the output list:
304  out_detected_lines.emplace_back(
305  this_best_inliers.size(), TLine2D(
306  double(this_best_model(0, 0)),
307  double(this_best_model(0, 1)),
308  double(this_best_model(0, 2))));
309 
310  out_detected_lines.rbegin()->second.unitarize();
311 
312  // Discard the selected points so they are not used again for
313  // finding subsequent planes:
314  remainingPoints.removeColumns(this_best_inliers);
315  }
316  else
317  {
318  break; // Do not search for more planes.
319  }
320  }
321 
322  MRPT_END
323 }
324 
325 // Template explicit instantiations:
326 #define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_) \
327  template void mrpt::math::ransac_detect_2D_lines<_TYPE_>( \
328  const CVectorDynamic<_TYPE_>& x, const CVectorDynamic<_TYPE_>& y, \
329  std::vector<std::pair<size_t, TLine2D>>& out_detected_lines, \
330  const double threshold, const size_t min_inliers_for_valid_line)
331 
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
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)
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
#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
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
bool ransac2Dline_degenerate(const CMatrixDynamic< T > &allData, const std::vector< size_t > &useIndices)
Return "true" if the selected points are a degenerate (invalid) case.
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)
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
#define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_)
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:28
Lightweight 2D point.
Definition: TPoint2D.h:31
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
bool ransac3Dplane_degenerate(const CMatrixDynamic< T > &allData, const std::vector< size_t > &useIndices)
Return "true" if the selected points are a degenerate (invalid) case.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019