MRPT  2.0.4
test.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 
12 #include <mrpt/math/ransac.h>
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/random.h>
19 #include <mrpt/system/CTicTac.h>
20 #include <iostream>
21 
22 using namespace mrpt;
23 using namespace mrpt::gui;
24 using namespace mrpt::math;
25 using namespace mrpt::random;
26 using namespace mrpt::poses;
27 using namespace mrpt::system;
28 using namespace std;
29 
30 // Define as needed for your application:
32  float, // Numeric data type: float since pointclouds use float
33  mrpt::maps::CPointsMap, // Dataset type (can be an abstract base type)
34  mrpt::math::TPlane // Model type to be estimated
35  >;
36 
37 namespace mrpt::math
38 {
39 // Overload for your custom dataset type:
40 size_t ransacDatasetSize(const mrpt::maps::CPointsMap& dataset)
41 {
42  return dataset.size();
43 }
44 } // namespace mrpt::math
45 
47  const mrpt::maps::CPointsMap& allData,
48  const std::vector<size_t>& useIndices,
49  vector<mrpt::math::TPlane>& fitModels)
50 {
51  ASSERT_(useIndices.size() == 3);
52 
53  TPoint3D pt[3];
54  for (int i = 0; i < 3; i++) allData.getPoint(useIndices[i], pt[i]);
55 
56  try
57  {
58  fitModels.resize(1);
59  fitModels[0] = TPlane(pt[0], pt[1], pt[2]);
60  }
61  catch (exception&)
62  {
63  // If the three points are degenerated, an exception may be thrown in
64  // the plane ctor
65  fitModels.clear();
66  return;
67  }
68 }
69 
71  const mrpt::maps::CPointsMap& allData,
72  const vector<mrpt::math::TPlane>& testModels,
73  const double distanceThreshold, unsigned int& out_bestModelIndex,
74  std::vector<size_t>& out_inlierIndices)
75 {
76  ASSERT_(testModels.size() == 1);
77  out_bestModelIndex = 0;
78  const mrpt::math::TPlane& plane = testModels[0];
79 
80  const size_t N = allData.size();
81  out_inlierIndices.clear();
82  out_inlierIndices.reserve(100);
83  for (size_t i = 0; i < N; i++)
84  {
86  allData.getPoint(i, pt);
87  const double d = plane.distance(pt);
88  if (d < distanceThreshold) out_inlierIndices.push_back(i);
89  }
90 }
91 
92 /** Return "true" if the selected points are a degenerate (invalid) case.
93  */
95  const mrpt::maps::CPointsMap& allData,
96  const std::vector<size_t>& useIndices)
97 {
98  return false;
99 }
100 
101 // ------------------------------------------------------
102 // TestRANSAC
103 // ------------------------------------------------------
104 void TestRANSAC()
105 {
106  auto& rng = getRandomGenerator();
107  rng.randomize();
108 
109  // Generate random points:
110  // ------------------------------------
111  const size_t N_plane = 300;
112  const size_t N_noise = 100;
113 
114  const double PLANE_EQ[4] = {1, -1, 1, -2};
115 
117  data.reserve(N_plane + N_noise);
118 
119  for (size_t i = 0; i < N_plane; i++)
120  {
121  const double xx = rng.drawUniform(-3, 3);
122  const double yy = rng.drawUniform(-3, 3);
123  const double zz =
124  -(PLANE_EQ[3] + PLANE_EQ[0] * xx + PLANE_EQ[1] * yy) / PLANE_EQ[2];
125  data.insertPointFast(xx, yy, zz);
126  }
127 
128  for (size_t i = 0; i < N_noise; i++)
129  {
130  data.insertPointFast(
131  rng.drawUniform(-4, 4), rng.drawUniform(-4, 4),
132  rng.drawUniform(-4, 4));
133  }
134 
135  // Run RANSAC
136  // ------------------------------------
137  mrpt::math::TPlane best_model;
138  std::vector<size_t> best_inliers;
139  const double DIST_THRESHOLD = 0.05;
140 
141  CTicTac tictac;
142  MyRansac myransac;
143 
145 
146  myransac.execute(
148  &ransac3Dplane_degenerate, DIST_THRESHOLD,
149  3, // Minimum set of points
150  best_inliers, best_model);
151 
152  cout << "Computation time: " << tictac.Tac() * 1000.0 << " ms\n";
153 
154  cout << "RANSAC finished: Best model: " << best_model.coefs[0] << " "
155  << best_model.coefs[1] << " " << best_model.coefs[2] << " "
156  << best_model.coefs[3] << endl;
157 
158  // Show GUI
159  // --------------------------
160  mrpt::gui::CDisplayWindow3D win("Set of points", 500, 500);
162 
163  scene->insert(opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1));
164  scene->insert(opengl::stock_objects::CornerXYZ());
165 
167  points->setColor(0, 0, 1);
168  points->setPointSize(3);
169  points->enableColorFromZ();
170  points->loadFromPointsMap(&data);
171 
172  scene->insert(points);
173 
175  opengl::CTexturedPlane::Create(-4, 4, -4, 4);
176 
177  glPlane->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00, 0x80)); // RGBA
178 
179  TPose3D glPlanePose;
180  best_model.getAsPose3D(glPlanePose);
181  glPlane->setPose(glPlanePose);
182 
183  scene->insert(glPlane);
184 
185  win.get3DSceneAndLock() = scene;
186  win.unlockAccess3DScene();
187  win.forceRepaint();
188 
189  win.waitForKey();
190 }
191 
192 // ------------------------------------------------------
193 // MAIN
194 // ------------------------------------------------------
195 int main()
196 {
197  try
198  {
199  TestRANSAC();
200  return 0;
201  }
202  catch (const std::exception& e)
203  {
204  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
205  return -1;
206  }
207  catch (...)
208  {
209  printf("Untyped exception!!");
210  return -1;
211  }
212 }
mrpt::opengl::CPointCloud::Ptr
std::shared_ptr< mrpt::opengl ::CPointCloud > Ptr
Definition: CPointCloud.h:49
mrpt::opengl::internal::data
static struct FontData data
Definition: gltext.cpp:144
mrpt::maps::CPointsMap::size
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:459
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::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::math::TPlane::coefs
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
Definition: TPlane.h:26
mrpt::opengl::CPointCloud::Create
static Ptr Create(Args &&... args)
Definition: CPointCloud.h:49
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::opengl::CTexturedPlane::Ptr
std::shared_ptr< mrpt::opengl ::CTexturedPlane > Ptr
Definition: CTexturedPlane.h:22
mrpt::maps::CPointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
Definition: CPointsMap.h:67
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
stock_objects.h
CPointCloud.h
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::opengl::CTexturedPlane::Create
static Ptr Create(Args &&... args)
Definition: CTexturedPlane.h:22
CDisplayWindow3D.h
random.h
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
TestRANSAC
void TestRANSAC()
Definition: vision_stereo_rectify/test.cpp:70
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::math::ransacDatasetSize
size_t ransacDatasetSize(const CMatrixDynamic< T > &dataset)
Define overloaded functions for user types as required.
Definition: ransac.h:26
mrpt::opengl::CGridPlaneXY::Create
static Ptr Create(Args &&... args)
Definition: CGridPlaneXY.h:31
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:25
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
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::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
mrpt::math::TPlane::getAsPose3D
void getAsPose3D(mrpt::math::TPose3D &outPose) const
Definition: TPlane.cpp:73
mrpt::opengl::COpenGLScene::Create
static Ptr Create(Args &&... args)
Definition: COpenGLScene.h:58
CPose3D.h
mrpt::math::TPlane
3D Plane, represented by its equation
Definition: TPlane.h:22
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
mrpt::system::LVL_DEBUG
@ LVL_DEBUG
Definition: system/COutputLogger.h:30
mrpt::opengl::stock_objects::CornerXYZ
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
Definition: StockObjects.cpp:136
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
CTicTac.h
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::RANSAC_Template
A generic RANSAC implementation.
Definition: ransac.h:47
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::maps::CPointsMap::getPoint
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:198
mrpt::exception_to_str
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
CSimplePointsMap.h
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
CGridPlaneXY.h
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
CTexturedPlane.h
mrpt::system
Definition: backtrace.h:14
ransac.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020