MRPT  2.0.3
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 
11 #include <mrpt/math/ransac.h>
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/random.h>
18 #include <mrpt/system/CTicTac.h>
19 #include <iostream>
20 
21 using namespace mrpt;
22 using namespace mrpt::gui;
23 using namespace mrpt::math;
24 using namespace mrpt::random;
25 using namespace mrpt::poses;
26 using namespace mrpt::system;
27 using namespace std;
28 
30  const CMatrixDouble& allData, const std::vector<size_t>& useIndices,
31  vector<CMatrixDouble>& fitModels)
32 {
33  ASSERT_(useIndices.size() == 3);
34 
35  TPoint3D p1(
36  allData(0, useIndices[0]), allData(1, useIndices[0]),
37  allData(2, useIndices[0]));
38  TPoint3D p2(
39  allData(0, useIndices[1]), allData(1, useIndices[1]),
40  allData(2, useIndices[1]));
41  TPoint3D p3(
42  allData(0, useIndices[2]), allData(1, useIndices[2]),
43  allData(2, useIndices[2]));
44 
45  try
46  {
47  TPlane plane(p1, p2, p3);
48  fitModels.resize(1);
49  CMatrixDouble& M = fitModels[0];
50 
51  M.setSize(1, 4);
52  for (size_t i = 0; i < 4; i++) M(0, i) = plane.coefs[i];
53  }
54  catch (exception&)
55  {
56  fitModels.clear();
57  return;
58  }
59 }
60 
62  const CMatrixDouble& allData, const vector<CMatrixDouble>& testModels,
63  const double distanceThreshold, unsigned int& out_bestModelIndex,
64  std::vector<size_t>& out_inlierIndices)
65 {
66  ASSERT_(testModels.size() == 1);
67  out_bestModelIndex = 0;
68  const CMatrixDouble& 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(allData(0, i), allData(1, i), allData(2, i)));
85  if (d < distanceThreshold) out_inlierIndices.push_back(i);
86  }
87 }
88 
89 /** Return "true" if the selected points are a degenerate (invalid) case.
90  */
92  const CMatrixDouble& allData, const std::vector<size_t>& useIndices)
93 {
94  return false;
95 }
96 
97 // ------------------------------------------------------
98 // TestRANSAC
99 // ------------------------------------------------------
100 void TestRANSAC()
101 {
103 
104  // Generate random points:
105  // ------------------------------------
106  const size_t N_plane = 300;
107  const size_t N_noise = 100;
108 
109  const double PLANE_EQ[4] = {1, -1, 1, -2};
110 
111  CMatrixDouble data(3, N_plane + N_noise);
112  for (size_t i = 0; i < N_plane; i++)
113  {
114  const double xx = getRandomGenerator().drawUniform(-3, 3);
115  const double yy = getRandomGenerator().drawUniform(-3, 3);
116  const double zz =
117  -(PLANE_EQ[3] + PLANE_EQ[0] * xx + PLANE_EQ[1] * yy) / PLANE_EQ[2];
118  data(0, i) = xx;
119  data(1, i) = yy;
120  data(2, i) = zz;
121  }
122 
123  for (size_t i = 0; i < N_noise; i++)
124  {
125  data(0, i + N_plane) = getRandomGenerator().drawUniform(-4, 4);
126  data(1, i + N_plane) = getRandomGenerator().drawUniform(-4, 4);
127  data(2, i + N_plane) = getRandomGenerator().drawUniform(-4, 4);
128  }
129 
130  // Run RANSAC
131  // ------------------------------------
132  CMatrixDouble best_model;
133  std::vector<size_t> best_inliers;
134  const double DIST_THRESHOLD = 0.05;
135 
136  CTicTac tictac;
137  const size_t TIMES = 100;
138 
139  math::RANSAC myransac;
140  for (size_t iters = 0; iters < TIMES; iters++)
141  {
142  myransac.setVerbosityLevel(
144 
145  myransac.execute(
147  ransac3Dplane_degenerate, DIST_THRESHOLD,
148  3, // Minimum set of points
149  best_inliers, best_model);
150  }
151 
152  cout << "Computation time: " << tictac.Tac() * 1000.0 / TIMES << " ms"
153  << endl;
154 
155  ASSERT_(best_model.rows() == 1 && best_model.cols() == 4);
156 
157  cout << "RANSAC finished: Best model: " << best_model << endl;
158  // cout << "Best inliers: " << best_inliers << endl;
159 
160  TPlane plane(
161  best_model(0, 0), best_model(0, 1), best_model(0, 2), best_model(0, 3));
162 
163  // Show GUI
164  // --------------------------
165  mrpt::gui::CDisplayWindow3D win("Set of points", 500, 500);
167 
168  scene->insert(opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1));
169  scene->insert(opengl::stock_objects::CornerXYZ());
170 
172  points->setColor(0, 0, 1);
173  points->setPointSize(3);
174  points->enableColorFromZ();
175 
176  {
177  std::vector<double> xs, ys, zs;
178 
179  data.extractRow(0, xs);
180  data.extractRow(1, ys);
181  data.extractRow(2, zs);
182  points->setAllPoints(xs, ys, zs);
183  }
184 
185  scene->insert(points);
186 
188  opengl::CTexturedPlane::Create(-4, 4, -4, 4);
189 
190  glPlane->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00, 0x80)); // RGBA
191 
192  TPose3D glPlanePose;
193  plane.getAsPose3D(glPlanePose);
194  glPlane->setPose(glPlanePose);
195 
196  scene->insert(glPlane);
197 
198  win.get3DSceneAndLock() = scene;
199  win.unlockAccess3DScene();
200  win.forceRepaint();
201 
202  win.waitForKey();
203 }
204 
205 // ------------------------------------------------------
206 // MAIN
207 // ------------------------------------------------------
208 int main()
209 {
210  try
211  {
212  TestRANSAC();
213  return 0;
214  }
215  catch (const std::exception& e)
216  {
217  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
218  return -1;
219  }
220  catch (...)
221  {
222  printf("Untyped exception!!");
223  return -1;
224  }
225 }
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::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::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
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::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
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
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::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:92
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
TestRANSAC
void TestRANSAC()
Definition: vision_stereo_rectify/test.cpp:70
mrpt::random::CRandomGenerator::drawUniform
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
Definition: RandomGenerators.h:142
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::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::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::math::CMatrixDynamic::cols
size_type cols() const
Number of columns in the matrix.
Definition: CMatrixDynamic.h:340
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
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
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::math::CMatrixDynamic< double >
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
mrpt::math::CMatrixDynamic::rows
size_type rows() const
Number of rows in the matrix.
Definition: CMatrixDynamic.h:337
CTexturedPlane.h
mrpt::system
Definition: backtrace.h:14
ransac.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020