MRPT  1.9.9
ransac_applications.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
13 
14 using namespace mrpt;
15 using namespace mrpt::math;
16 using namespace std;
17 
18 /*---------------------------------------------------------------
19  Aux. functions needed by ransac_detect_3D_planes
20  ---------------------------------------------------------------*/
21 namespace mrpt
22 {
23 namespace math
24 {
25 template <typename T>
27  const CMatrixTemplateNumeric<T>& allData,
28  const std::vector<size_t>& useIndices,
29  vector<CMatrixTemplateNumeric<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  CMatrixTemplateNumeric<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 CMatrixTemplateNumeric<T>& allData,
62  const vector<CMatrixTemplateNumeric<T>>& testModels,
63  const T distanceThreshold, unsigned int& out_bestModelIndex,
64  std::vector<size_t>& out_inlierIndices)
65 {
66  ASSERT_(testModels.size() == 1);
67  out_bestModelIndex = 0;
68  const CMatrixTemplateNumeric<T>& M = testModels[0];
69 
70  ASSERT_(M.rows() == 1 && M.cols() == 4);
71 
72  TPlane plane;
73  plane.coefs[0] = M(0, 0);
74  plane.coefs[1] = M(0, 1);
75  plane.coefs[2] = M(0, 2);
76  plane.coefs[3] = M(0, 3);
77 
78  const size_t N = allData.cols();
79  out_inlierIndices.clear();
80  out_inlierIndices.reserve(100);
81  for (size_t i = 0; i < N; i++)
82  {
83  const double d = plane.distance(
84  TPoint3D(
85  allData.get_unsafe(0, i), allData.get_unsafe(1, i),
86  allData.get_unsafe(2, i)));
87  if (d < distanceThreshold) out_inlierIndices.push_back(i);
88  }
89 }
90 
91 /** Return "true" if the selected points are a degenerate (invalid) case.
92  */
93 template <typename T>
95  const CMatrixTemplateNumeric<T>& allData,
96  const std::vector<size_t>& useIndices)
97 {
98  MRPT_UNUSED_PARAM(allData);
99  MRPT_UNUSED_PARAM(useIndices);
100  return false;
101 }
102 } // end namespace
103 } // end namespace
104 
105 /*---------------------------------------------------------------
106  ransac_detect_3D_planes
107  ---------------------------------------------------------------*/
108 template <typename NUMTYPE>
110  const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& x,
111  const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& y,
112  const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& z,
113  vector<pair<size_t, TPlane>>& out_detected_planes, const double threshold,
114  const size_t min_inliers_for_valid_plane)
115 {
116  MRPT_START
117 
118  ASSERT_(x.size() == y.size() && x.size() == z.size());
119 
120  out_detected_planes.clear();
121 
122  if (x.empty()) return;
123 
124  // The running lists of remaining points after each plane, as a matrix:
125  CMatrixTemplateNumeric<NUMTYPE> remainingPoints(3, x.size());
126  remainingPoints.insertRow(0, x);
127  remainingPoints.insertRow(1, y);
128  remainingPoints.insertRow(2, z);
129 
130  // ---------------------------------------------
131  // For each plane:
132  // ---------------------------------------------
133  for (;;)
134  {
135  std::vector<size_t> this_best_inliers;
136  CMatrixTemplateNumeric<NUMTYPE> this_best_model;
137 
140  ransac.execute(
141  remainingPoints, mrpt::math::ransac3Dplane_fit<NUMTYPE>,
142  mrpt::math::ransac3Dplane_distance<NUMTYPE>,
143  mrpt::math::ransac3Dplane_degenerate<NUMTYPE>, threshold,
144  3, // Minimum set of points
145  this_best_inliers, this_best_model,
146  0.999 // Prob. of good result
147  );
148 
149  // Is this plane good enough?
150  if (this_best_inliers.size() >= min_inliers_for_valid_plane)
151  {
152  // Add this plane to the output list:
153  out_detected_planes.push_back(
154  std::make_pair(
155  this_best_inliers.size(),
156  TPlane(
157  double(this_best_model(0, 0)),
158  double(this_best_model(0, 1)),
159  double(this_best_model(0, 2)),
160  double(this_best_model(0, 3)))));
161 
162  out_detected_planes.rbegin()->second.unitarize();
163 
164  // Discard the selected points so they are not used again for
165  // finding subsequent planes:
166  remainingPoints.removeColumns(this_best_inliers);
167  }
168  else
169  {
170  break; // Do not search for more planes.
171  }
172  }
173 
174  MRPT_END
175 }
176 
177 // Template explicit instantiations:
178 #define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_) \
179  template void mrpt::math::ransac_detect_3D_planes<_TYPE_>( \
180  const Eigen::Matrix<_TYPE_, Eigen::Dynamic, 1>& x, \
181  const Eigen::Matrix<_TYPE_, Eigen::Dynamic, 1>& y, \
182  const Eigen::Matrix<_TYPE_, Eigen::Dynamic, 1>& z, \
183  vector<pair<size_t, TPlane>>& out_detected_planes, \
184  const double threshold, const size_t min_inliers_for_valid_plane);
185 
188 #ifdef HAVE_LONG_DOUBLE
190 #endif
191 
192  /*---------------------------------------------------------------
193  Aux. functions needed by ransac_detect_2D_lines
194  ---------------------------------------------------------------*/
195  namespace mrpt
196 {
197  namespace math
198  {
199  template <typename T>
200  void ransac2Dline_fit(
201  const CMatrixTemplateNumeric<T>& allData,
202  const std::vector<size_t>& useIndices,
203  vector<CMatrixTemplateNumeric<T>>& fitModels)
204  {
205  ASSERT_(useIndices.size() == 2);
206 
207  TPoint2D p1(allData(0, useIndices[0]), allData(1, useIndices[0]));
208  TPoint2D p2(allData(0, useIndices[1]), allData(1, useIndices[1]));
209 
210  try
211  {
212  TLine2D line(p1, p2);
213  fitModels.resize(1);
214  CMatrixTemplateNumeric<T>& M = fitModels[0];
215 
216  M.setSize(1, 3);
217  for (size_t i = 0; i < 3; i++) M(0, i) = line.coefs[i];
218  }
219  catch (exception&)
220  {
221  fitModels.clear();
222  return;
223  }
224  }
225 
226  template <typename T>
227  void ransac2Dline_distance(
228  const CMatrixTemplateNumeric<T>& allData,
229  const vector<CMatrixTemplateNumeric<T>>& testModels,
230  const T distanceThreshold, unsigned int& out_bestModelIndex,
231  std::vector<size_t>& out_inlierIndices)
232  {
233  out_inlierIndices.clear();
234  out_bestModelIndex = 0;
235 
236  if (testModels.empty()) return; // No model, no inliers.
237 
238  ASSERTMSG_(
239  testModels.size() == 1,
240  format(
241  "Expected testModels.size()=1, but it's = %u",
242  static_cast<unsigned int>(testModels.size())));
243  const CMatrixTemplateNumeric<T>& M = testModels[0];
244 
245  ASSERT_(M.rows() == 1 && M.cols() == 3);
246 
247  TLine2D line;
248  line.coefs[0] = M(0, 0);
249  line.coefs[1] = M(0, 1);
250  line.coefs[2] = M(0, 2);
251 
252  const size_t N = allData.cols();
253  out_inlierIndices.reserve(100);
254  for (size_t i = 0; i < N; i++)
255  {
256  const double d = line.distance(
257  TPoint2D(allData.get_unsafe(0, i), allData.get_unsafe(1, i)));
258  if (d < distanceThreshold) out_inlierIndices.push_back(i);
259  }
260  }
261 
262  /** Return "true" if the selected points are a degenerate (invalid) case.
263  */
264  template <typename T>
265  bool ransac2Dline_degenerate(
266  const CMatrixTemplateNumeric<T>& allData,
267  const std::vector<size_t>& useIndices)
268  {
269  MRPT_UNUSED_PARAM(allData);
270  MRPT_UNUSED_PARAM(useIndices);
271  return false;
272  }
273  } // end namespace
274 } // end namespace
275 
276 /*---------------------------------------------------------------
277  ransac_detect_2D_lines
278  ---------------------------------------------------------------*/
279 template <typename NUMTYPE>
281  const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& x,
282  const Eigen::Matrix<NUMTYPE, Eigen::Dynamic, 1>& y,
283  std::vector<std::pair<size_t, TLine2D>>& out_detected_lines,
284  const double threshold, const size_t min_inliers_for_valid_line)
285 {
286  MRPT_START
287  ASSERT_(x.size() == y.size());
288  out_detected_lines.clear();
289 
290  if (x.empty()) return;
291 
292  // The running lists of remaining points after each plane, as a matrix:
293  CMatrixTemplateNumeric<NUMTYPE> remainingPoints(2, x.size());
294  remainingPoints.insertRow(0, x);
295  remainingPoints.insertRow(1, y);
296 
297  // ---------------------------------------------
298  // For each line:
299  // ---------------------------------------------
300  while (remainingPoints.cols() >= 2)
301  {
302  std::vector<size_t> this_best_inliers;
303  CMatrixTemplateNumeric<NUMTYPE> this_best_model;
304 
307  ransac.execute(
308  remainingPoints, ransac2Dline_fit<NUMTYPE>,
309  ransac2Dline_distance<NUMTYPE>, ransac2Dline_degenerate<NUMTYPE>,
310  threshold,
311  2, // Minimum set of points
312  this_best_inliers, this_best_model,
313  0.99999 // Prob. of good result
314  );
315 
316  // Is this plane good enough?
317  if (this_best_inliers.size() >= min_inliers_for_valid_line)
318  {
319  // Add this plane to the output list:
320  out_detected_lines.push_back(
321  std::make_pair(
322  this_best_inliers.size(),
323  TLine2D(
324  double(this_best_model(0, 0)),
325  double(this_best_model(0, 1)),
326  double(this_best_model(0, 2)))));
327 
328  out_detected_lines.rbegin()->second.unitarize();
329 
330  // Discard the selected points so they are not used again for
331  // finding subsequent planes:
332  remainingPoints.removeColumns(this_best_inliers);
333  }
334  else
335  {
336  break; // Do not search for more planes.
337  }
338  }
339 
340  MRPT_END
341 }
342 
343 // Template explicit instantiations:
344 #define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_) \
345  template void mrpt::math::ransac_detect_2D_lines<_TYPE_>( \
346  const Eigen::Matrix<_TYPE_, Eigen::Dynamic, 1>& x, \
347  const Eigen::Matrix<_TYPE_, Eigen::Dynamic, 1>& y, \
348  std::vector<std::pair<size_t, TLine2D>>& out_detected_lines, \
349  const double threshold, const size_t min_inliers_for_valid_line);
350 
353 #ifdef HAVE_LONG_DOUBLE
355 #endif
void ransac_detect_3D_planes(const Eigen::Matrix< NUMTYPE, Eigen::Dynamic, 1 > &x, const Eigen::Matrix< NUMTYPE, Eigen::Dynamic, 1 > &y, const Eigen::Matrix< NUMTYPE, Eigen::Dynamic, 1 > &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...
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
void ransac3Dplane_fit(const CMatrixTemplateNumeric< T > &allData, const std::vector< size_t > &useIndices, vector< CMatrixTemplateNumeric< T >> &fitModels)
STL namespace.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
#define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_)
3D Plane, represented by its equation
double coefs[3]
Line coefficients, stored as an array: .
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void ransac_detect_2D_lines(const Eigen::Matrix< NUMTYPE, Eigen::Dynamic, 1 > &x, const Eigen::Matrix< NUMTYPE, Eigen::Dynamic, 1 > &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...
bool execute(const CMatrixTemplateNumeric< 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, CMatrixTemplateNumeric< 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
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
double coefs[4]
Plane coefficients, stored as an array: .
#define MRPT_END
Definition: exceptions.h:266
GLenum GLint GLint y
Definition: glext.h:3538
void ransac3Dplane_distance(const CMatrixTemplateNumeric< T > &allData, const vector< CMatrixTemplateNumeric< T >> &testModels, const T distanceThreshold, unsigned int &out_bestModelIndex, std::vector< size_t > &out_inlierIndices)
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
#define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_)
bool ransac3Dplane_degenerate(const CMatrixTemplateNumeric< T > &allData, const std::vector< size_t > &useIndices)
Return "true" if the selected points are a degenerate (invalid) case.
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:28
Lightweight 2D point.
#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 .



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020