MRPT  2.0.3
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 
EXPLICIT_INST_ransac_detect_3D_planes
#define EXPLICIT_INST_ransac_detect_3D_planes(_TYPE_)
Definition: ransac_applications.cpp:170
mrpt::math::ransac_detect_3D_planes
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...
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::math::TPlane::distance
double distance(const TPoint3D &point) const
Distance to 3D point.
Definition: TPlane.cpp:38
mrpt::math::TPoint3D_< double >
mrpt::system::COutputLogger::setVerbosityLevel
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
Definition: COutputLogger.cpp:149
mrpt::math::CMatrixDynamic::setSize
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
Definition: CMatrixDynamic.h:352
mrpt::math::TPoint2D_< double >
mrpt::math::TPlane::coefs
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
Definition: TPlane.h:26
mrpt::math::CVectorDynamic::empty
bool empty() const
Definition: CVectorDynamic.h:143
math-precomp.h
mrpt::math::ransac3Dplane_distance
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)
Definition: ransac_applications.cpp:60
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::system::LVL_INFO
@ LVL_INFO
Definition: system/COutputLogger.h:31
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::math::MatrixBase::removeColumns
void removeColumns(const std::vector< std::size_t > &idxsToRemove)
Removes columns of the matrix.
Definition: MatrixBase_impl.h:38
mrpt::math::CMatrixDynamic::setRow
void setRow(const Index row, const VECTOR &v)
Definition: CMatrixDynamic.h:489
mrpt::math::RANSAC_Template::execute
bool execute(const DATASET &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, MODEL &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_impl.h:21
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::math::TLine2D::coefs
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:23
mrpt::math::ransac2Dline_degenerate
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.
Definition: ransac_applications.cpp:250
mrpt::math::ransac3Dplane_degenerate
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.
Definition: ransac_applications.cpp:91
mrpt::math::ransac2Dline_distance
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)
Definition: ransac_applications.cpp:214
mrpt::math::CVectorDynamic
Template for column vectors of dynamic size, compatible with Eigen.
Definition: CVectorDynamic.h:31
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::math::CVectorDynamic::size
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CVectorDynamic.h:141
mrpt::math::TPlane
3D Plane, represented by its equation
Definition: TPlane.h:22
mrpt::math::TLine2D::distance
double distance(const TPoint2D &point) const
Distance from a given point.
Definition: TLine2D.cpp:33
ransac_applications.h
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::math::TPoint2D
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::TLine2D
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19
mrpt::math::RANSAC_Template
A generic RANSAC implementation.
Definition: ransac.h:47
EXPLICIT_INSTANT_ransac_detect_2D_lines
#define EXPLICIT_INSTANT_ransac_detect_2D_lines(_TYPE_)
Definition: ransac_applications.cpp:324
mrpt::math::CMatrixDynamic::cols
size_type cols() const
Number of columns in the matrix.
Definition: CMatrixDynamic.h:340
mrpt::math::ransac3Dplane_fit
void ransac3Dplane_fit(const CMatrixDynamic< T > &allData, const std::vector< size_t > &useIndices, vector< CMatrixDynamic< T >> &fitModels)
Definition: ransac_applications.cpp:27
mrpt::math::ransac2Dline_fit
void ransac2Dline_fit(const CMatrixDynamic< T > &allData, const std::vector< size_t > &useIndices, vector< CMatrixDynamic< T >> &fitModels)
Definition: ransac_applications.cpp:188
mrpt::math::CMatrixDynamic
This template class provides the basic functionality for a general 2D any-size, resizable container o...
Definition: CMatrixDynamic.h:39
mrpt::math::CMatrixDynamic::rows
size_type rows() const
Number of rows in the matrix.
Definition: CMatrixDynamic.h:337
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::math::ransac_detect_2D_lines
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...
Definition: ransac_applications.cpp:263



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020