Main MRPT website > C++ reference for 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-2017, 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 "base-precomp.h" // Precompiled headers
11 
13 
14 using namespace mrpt;
15 using namespace mrpt::utils;
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 CMatrixTemplateNumeric<T>& allData, const 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  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_(size(M, 1) == 1 && size(M, 2) == 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 = size(allData, 2);
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 mrpt::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  mrpt::vector_size_t this_best_inliers;
136  CMatrixTemplateNumeric<NUMTYPE> this_best_model;
137 
139  ransac.setVerbosityLevel(mrpt::utils::LVL_INFO);
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 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  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_(size(M, 1) == 1 && size(M, 2) == 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 = size(allData, 2);
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 mrpt::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 
288  ASSERT_(x.size() == y.size())
289 
290  out_detected_lines.clear();
291 
292  if (x.empty()) return;
293 
294  // The running lists of remaining points after each plane, as a matrix:
295  CMatrixTemplateNumeric<NUMTYPE> remainingPoints(2, x.size());
296  remainingPoints.insertRow(0, x);
297  remainingPoints.insertRow(1, y);
298 
299  // ---------------------------------------------
300  // For each line:
301  // ---------------------------------------------
302  while (size(remainingPoints, 2) >= 2)
303  {
304  mrpt::vector_size_t this_best_inliers;
305  CMatrixTemplateNumeric<NUMTYPE> this_best_model;
306 
308  ransac.setVerbosityLevel(mrpt::utils::LVL_INFO);
309  ransac.execute(
310  remainingPoints, ransac2Dline_fit<NUMTYPE>,
311  ransac2Dline_distance<NUMTYPE>, ransac2Dline_degenerate<NUMTYPE>,
312  threshold,
313  2, // Minimum set of points
314  this_best_inliers, this_best_model,
315  0.99999 // Prob. of good result
316  );
317 
318  // Is this plane good enough?
319  if (this_best_inliers.size() >= min_inliers_for_valid_line)
320  {
321  // Add this plane to the output list:
322  out_detected_lines.push_back(
323  std::make_pair(
324  this_best_inliers.size(),
325  TLine2D(
326  double(this_best_model(0, 0)),
327  double(this_best_model(0, 1)),
328  double(this_best_model(0, 2)))));
329 
330  out_detected_lines.rbegin()->second.unitarize();
331 
332  // Discard the selected points so they are not used again for
333  // finding subsequent planes:
334  remainingPoints.removeColumns(this_best_inliers);
335  }
336  else
337  {
338  break; // Do not search for more planes.
339  }
340  }
341 
342  MRPT_END
343 }
344 
345 // Template explicit instantiations:
346 #define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_) \
347  template void mrpt::math::ransac_detect_2D_lines<_TYPE_>( \
348  const Eigen::Matrix<_TYPE_, Eigen::Dynamic, 1>& x, \
349  const Eigen::Matrix<_TYPE_, Eigen::Dynamic, 1>& y, \
350  std::vector<std::pair<size_t, TLine2D>>& out_detected_lines, \
351  const double threshold, const size_t min_inliers_for_valid_line);
352 
355 #ifdef HAVE_LONG_DOUBLE
357 #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...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
GLdouble GLdouble z
Definition: glext.h:3872
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...
STL namespace.
double distance(const TPoint3D &point) const
Distance to 3D point.
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, mrpt::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:25
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
bool ransac3Dplane_degenerate(const CMatrixDouble &planeCorresp, const mrpt::vector_size_t &useIndices)
Return "true" if the selected points are a degenerate (invalid) case.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void ransac3Dplane_fit(const CMatrixTemplateNumeric< T > &allData, const vector_size_t &useIndices, vector< CMatrixTemplateNumeric< T >> &fitModels)
#define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_)
3D Plane, represented by its equation
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:19
double coefs[3]
Line coefficients, stored as an array: .
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double coefs[4]
Plane coefficients, stored as an array: .
#define ASSERT_(f)
std::vector< size_t > vector_size_t
Definition: types_simple.h:26
GLenum GLint GLint y
Definition: glext.h:3538
void ransac3Dplane_distance(const CMatrixDouble &planeCorresp, const vector< CMatrixDouble > &testModels, const double distanceThreshold, unsigned int &out_bestModelIndex, mrpt::vector_size_t &out_inlierIndices)
GLsizeiptr size
Definition: glext.h:3923
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
#define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_)
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:31
Lightweight 2D point.
#define ASSERTMSG_(f, __ERROR_MSG)
2D line without bounds, represented by its equation .
std::vector< size_t > vector_size_t



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019