Main MRPT website > C++ reference for MRPT 1.5.6
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 
12 
14 
15 using namespace mrpt;
16 using namespace mrpt::utils;
17 using namespace mrpt::math;
18 using namespace std;
19 
20 
21 /*---------------------------------------------------------------
22  Aux. functions needed by ransac_detect_3D_planes
23  ---------------------------------------------------------------*/
24 namespace mrpt
25 {
26  namespace math
27  {
28  template <typename T>
30  const CMatrixTemplateNumeric<T> &allData,
31  const vector_size_t &useIndices,
32  vector< CMatrixTemplateNumeric<T> > &fitModels )
33  {
34  ASSERT_(useIndices.size()==3);
35 
36  TPoint3D p1( allData(0,useIndices[0]),allData(1,useIndices[0]),allData(2,useIndices[0]) );
37  TPoint3D p2( allData(0,useIndices[1]),allData(1,useIndices[1]),allData(2,useIndices[1]) );
38  TPoint3D p3( allData(0,useIndices[2]),allData(1,useIndices[2]),allData(2,useIndices[2]) );
39 
40  try
41  {
42  TPlane plane( p1,p2,p3 );
43  fitModels.resize(1);
44  CMatrixTemplateNumeric<T> &M = fitModels[0];
45 
46  M.setSize(1,4);
47  for (size_t i=0;i<4;i++)
48  M(0,i)=T(plane.coefs[i]);
49  }
50  catch(exception &)
51  {
52  fitModels.clear();
53  return;
54  }
55  }
56 
57 
58  template <typename T>
60  const CMatrixTemplateNumeric<T> &allData,
61  const vector< CMatrixTemplateNumeric<T> > & testModels,
62  const T distanceThreshold,
63  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( TPoint3D( allData.get_unsafe(0,i),allData.get_unsafe(1,i),allData.get_unsafe(2,i) ) );
84  if (d<distanceThreshold)
85  out_inlierIndices.push_back(i);
86  }
87  }
88 
89  /** Return "true" if the selected points are a degenerate (invalid) case.
90  */
91  template <typename T>
93  const CMatrixTemplateNumeric<T> &allData,
94  const mrpt::vector_size_t &useIndices )
95  {
96  MRPT_UNUSED_PARAM(allData);
97  MRPT_UNUSED_PARAM(useIndices);
98  return false;
99  }
100  } // end namespace
101 } // end namespace
102 
103 
104 /*---------------------------------------------------------------
105  ransac_detect_3D_planes
106  ---------------------------------------------------------------*/
107 template <typename NUMTYPE>
109  const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &x,
110  const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &y,
111  const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &z,
112  vector<pair<size_t,TPlane> > &out_detected_planes,
113  const double threshold,
114  const size_t min_inliers_for_valid_plane
115  )
116 {
117  MRPT_START
118 
119  ASSERT_(x.size()==y.size() && x.size()==z.size())
120 
121  out_detected_planes.clear();
122 
123  if (x.empty())
124  return;
125 
126  // The running lists of remaining points after each plane, as a matrix:
127  CMatrixTemplateNumeric<NUMTYPE> remainingPoints( 3, x.size() );
128  remainingPoints.insertRow(0,x);
129  remainingPoints.insertRow(1,y);
130  remainingPoints.insertRow(2,z);
131 
132 
133  // ---------------------------------------------
134  // For each plane:
135  // ---------------------------------------------
136  for (;;)
137  {
138  mrpt::vector_size_t this_best_inliers;
139  CMatrixTemplateNumeric<NUMTYPE> this_best_model;
140 
142  ransac.setVerbosityLevel(mrpt::utils::LVL_INFO);
143  ransac.execute(
144  remainingPoints,
148  threshold,
149  3, // Minimum set of points
150  this_best_inliers,
151  this_best_model,
152  0.999 // Prob. of good result
153  );
154 
155  // Is this plane good enough?
156  if (this_best_inliers.size()>=min_inliers_for_valid_plane)
157  {
158  // Add this plane to the output list:
159  out_detected_planes.push_back(
160  std::make_pair(
161  this_best_inliers.size(),
162  TPlane( double(this_best_model(0,0)), double(this_best_model(0,1)), double(this_best_model(0,2)), double(this_best_model(0,3)) )
163  ) );
164 
165  out_detected_planes.rbegin()->second.unitarize();
166 
167  // Discard the selected points so they are not used again for finding subsequent planes:
168  remainingPoints.removeColumns(this_best_inliers);
169  }
170  else
171  {
172  break; // Do not search for more planes.
173  }
174  }
175 
176  MRPT_END
177 }
178 
179 
180 
181 // Template explicit instantiations:
182 #define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_) \
183  template void BASE_IMPEXP mrpt::math::ransac_detect_3D_planes<_TYPE_>( \
184  const Eigen::Matrix<_TYPE_,Eigen::Dynamic,1> &x, \
185  const Eigen::Matrix<_TYPE_,Eigen::Dynamic,1> &y, \
186  const Eigen::Matrix<_TYPE_,Eigen::Dynamic,1> &z, \
187  vector<pair<size_t,TPlane> > &out_detected_planes, \
188  const double threshold, \
189  const size_t min_inliers_for_valid_plane);
190 
193 #ifdef HAVE_LONG_DOUBLE
195 #endif
196 
197 
198 
199 /*---------------------------------------------------------------
200  Aux. functions needed by ransac_detect_2D_lines
201  ---------------------------------------------------------------*/
202 namespace mrpt
203 {
204  namespace math
205  {
206  template <typename T>
207  void ransac2Dline_fit(
208  const CMatrixTemplateNumeric<T> &allData,
209  const vector_size_t &useIndices,
210  vector< CMatrixTemplateNumeric<T> > &fitModels )
211  {
212  ASSERT_(useIndices.size()==2);
213 
214  TPoint2D p1( allData(0,useIndices[0]),allData(1,useIndices[0]) );
215  TPoint2D p2( allData(0,useIndices[1]),allData(1,useIndices[1]) );
216 
217  try
218  {
219  TLine2D line(p1,p2);
220  fitModels.resize(1);
221  CMatrixTemplateNumeric<T> &M = fitModels[0];
222 
223  M.setSize(1,3);
224  for (size_t i=0;i<3;i++)
225  M(0,i)=line.coefs[i];
226  }
227  catch(exception &)
228  {
229  fitModels.clear();
230  return;
231  }
232  }
233 
234 
235  template <typename T>
236  void ransac2Dline_distance(
237  const CMatrixTemplateNumeric<T> &allData,
238  const vector< CMatrixTemplateNumeric<T> > & testModels,
239  const T distanceThreshold,
240  unsigned int & out_bestModelIndex,
241  vector_size_t & out_inlierIndices )
242  {
243  out_inlierIndices.clear();
244  out_bestModelIndex = 0;
245 
246  if (testModels.empty()) return; // No model, no inliers.
247 
248  ASSERTMSG_( testModels.size()==1, format("Expected testModels.size()=1, but it's = %u",static_cast<unsigned int>(testModels.size()) ) )
249  const CMatrixTemplateNumeric<T> &M = testModels[0];
250 
251  ASSERT_( size(M,1)==1 && size(M,2)==3 )
252 
253  TLine2D line;
254  line.coefs[0] = M(0,0);
255  line.coefs[1] = M(0,1);
256  line.coefs[2] = M(0,2);
257 
258  const size_t N = size(allData,2);
259  out_inlierIndices.reserve(100);
260  for (size_t i=0;i<N;i++)
261  {
262  const double d = line.distance( TPoint2D( allData.get_unsafe(0,i),allData.get_unsafe(1,i) ) );
263  if (d<distanceThreshold)
264  out_inlierIndices.push_back(i);
265  }
266  }
267 
268  /** Return "true" if the selected points are a degenerate (invalid) case.
269  */
270  template <typename T>
271  bool ransac2Dline_degenerate(
272  const CMatrixTemplateNumeric<T> &allData,
273  const mrpt::vector_size_t &useIndices )
274  {
275  MRPT_UNUSED_PARAM(allData);
276  MRPT_UNUSED_PARAM(useIndices);
277  return false;
278  }
279  } // end namespace
280 } // end namespace
281 
282 /*---------------------------------------------------------------
283  ransac_detect_2D_lines
284  ---------------------------------------------------------------*/
285 template <typename NUMTYPE>
287  const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &x,
288  const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &y,
289  std::vector<std::pair<size_t,TLine2D> > &out_detected_lines,
290  const double threshold,
291  const size_t min_inliers_for_valid_line
292  )
293 {
294  MRPT_START
295 
296  ASSERT_(x.size()==y.size())
297 
298  out_detected_lines.clear();
299 
300  if (x.empty())
301  return;
302 
303  // The running lists of remaining points after each plane, as a matrix:
304  CMatrixTemplateNumeric<NUMTYPE> remainingPoints( 2, x.size() );
305  remainingPoints.insertRow(0,x);
306  remainingPoints.insertRow(1,y);
307 
308  // ---------------------------------------------
309  // For each line:
310  // ---------------------------------------------
311  while (size(remainingPoints,2)>=2)
312  {
313  mrpt::vector_size_t this_best_inliers;
314  CMatrixTemplateNumeric<NUMTYPE> this_best_model;
315 
317  ransac.setVerbosityLevel(mrpt::utils::LVL_INFO);
318  ransac.execute(
319  remainingPoints,
320  ransac2Dline_fit,
321  ransac2Dline_distance,
322  ransac2Dline_degenerate,
323  threshold,
324  2, // Minimum set of points
325  this_best_inliers,
326  this_best_model,
327  0.99999 // Prob. of good result
328  );
329 
330  // Is this plane good enough?
331  if (this_best_inliers.size()>=min_inliers_for_valid_line)
332  {
333  // Add this plane to the output list:
334  out_detected_lines.push_back(
335  std::make_pair(
336  this_best_inliers.size(),
337  TLine2D(double(this_best_model(0,0)), double(this_best_model(0,1)), double(this_best_model(0,2)) )
338  ) );
339 
340  out_detected_lines.rbegin()->second.unitarize();
341 
342  // Discard the selected points so they are not used again for finding subsequent planes:
343  remainingPoints.removeColumns(this_best_inliers);
344  }
345  else
346  {
347  break; // Do not search for more planes.
348  }
349  }
350 
351  MRPT_END
352 }
353 
354 // Template explicit instantiations:
355 #define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_) \
356  template void BASE_IMPEXP mrpt::math::ransac_detect_2D_lines<_TYPE_>( \
357  const Eigen::Matrix<_TYPE_,Eigen::Dynamic,1> &x, \
358  const Eigen::Matrix<_TYPE_,Eigen::Dynamic,1> &y, \
359  std::vector<std::pair<size_t,TLine2D> > &out_detected_lines, \
360  const double threshold, \
361  const size_t min_inliers_for_valid_line ); \
362 
365 #ifdef HAVE_LONG_DOUBLE
367 #endif
368 
369 
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void BASE_IMPEXP 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...
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.
#define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_)
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
3D Plane, represented by its equation
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
double coefs[3]
Line coefficients, stored as an array: .
double distance(const TPoint3D &point) const
Distance to 3D point.
GLsizeiptr size
Definition: glew.h:1586
#define MRPT_START
GLdouble GLdouble z
Definition: glew.h:1464
double coefs[4]
Plane coefficients, stored as an array: .
#define ASSERT_(f)
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
void ransac3Dplane_distance(const CMatrixDouble &planeCorresp, const vector< CMatrixDouble > &testModels, const double distanceThreshold, unsigned int &out_bestModelIndex, mrpt::vector_size_t &out_inlierIndices)
bool execute(const CMatrixTemplateNumeric< NUMTYPE > &data, TRansacFitFunctor fit_func, TRansacDistanceFunctor dist_func, 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:26
Lightweight 3D point.
void BASE_IMPEXP 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...
#define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_)
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:30
Lightweight 2D point.
#define ASSERTMSG_(f, __ERROR_MSG)
void ransac3Dplane_fit(const CMatrixTemplateNumeric< T > &allData, const vector_size_t &useIndices, vector< CMatrixTemplateNumeric< T > > &fitModels)
2D line without bounds, represented by its equation .
std::vector< size_t > vector_size_t



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018