Go to the documentation of this file.
30 const CMatrixDouble& allData,
const std::vector<size_t>& useIndices,
31 vector<CMatrixDouble>& fitModels)
33 ASSERT_(useIndices.size() == 3);
36 allData(0, useIndices[0]), allData(1, useIndices[0]),
37 allData(2, useIndices[0]));
39 allData(0, useIndices[1]), allData(1, useIndices[1]),
40 allData(2, useIndices[1]));
42 allData(0, useIndices[2]), allData(1, useIndices[2]),
43 allData(2, useIndices[2]));
52 for (
size_t i = 0; i < 4; i++) M(0, i) = plane.coefs[i];
62 const CMatrixDouble& allData,
const vector<CMatrixDouble>& testModels,
63 const double distanceThreshold,
unsigned int& out_bestModelIndex,
64 std::vector<size_t>& out_inlierIndices)
66 ASSERT_(testModels.size() == 1);
67 out_bestModelIndex = 0;
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);
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++)
84 TPoint3D(allData(0, i), allData(1, i), allData(2, i)));
85 if (d < distanceThreshold) out_inlierIndices.push_back(i);
92 const CMatrixDouble& allData,
const std::vector<size_t>& useIndices)
106 const size_t N_plane = 300;
107 const size_t N_noise = 100;
109 const double PLANE_EQ[4] = {1, -1, 1, -2};
112 for (
size_t i = 0; i < N_plane; i++)
117 -(PLANE_EQ[3] + PLANE_EQ[0] * xx + PLANE_EQ[1] * yy) / PLANE_EQ[2];
123 for (
size_t i = 0; i < N_noise; i++)
133 std::vector<size_t> best_inliers;
134 const double DIST_THRESHOLD = 0.05;
137 const size_t TIMES = 100;
140 for (
size_t iters = 0; iters < TIMES; iters++)
149 best_inliers, best_model);
152 cout <<
"Computation time: " << tictac.
Tac() * 1000.0 / TIMES <<
" ms"
157 cout <<
"RANSAC finished: Best model: " << best_model << endl;
161 best_model(0, 0), best_model(0, 1), best_model(0, 2), best_model(0, 3));
172 points->setColor(0, 0, 1);
173 points->setPointSize(3);
174 points->enableColorFromZ();
177 std::vector<double> xs, ys, zs;
179 data.extractRow(0, xs);
180 data.extractRow(1, ys);
181 data.extractRow(2, zs);
182 points->setAllPoints(xs, ys, zs);
185 scene->insert(points);
194 glPlane->setPose(glPlanePose);
196 scene->insert(glPlane);
198 win.get3DSceneAndLock() = scene;
199 win.unlockAccess3DScene();
215 catch (
const std::exception& e)
222 printf(
"Untyped exception!!");
std::shared_ptr< mrpt::opengl ::CPointCloud > Ptr
static struct FontData data
TPoint3D_< double > TPoint3D
Lightweight 3D point.
double distance(const TPoint3D &point) const
Distance to 3D point.
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
A high-performance stopwatch, with typical resolution of nanoseconds.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
static Ptr Create(Args &&... args)
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)
std::shared_ptr< mrpt::opengl ::CTexturedPlane > Ptr
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
static Ptr Create(Args &&... args)
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
double Tac() noexcept
Stops the stopwatch.
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
bool execute(const DATASET &data, const TRansacFitFunctor &fit_func, const TRansacDistanceFunctor &dist_func, const TRansacDegenerateFunctor °en_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.
static Ptr Create(Args &&... args)
mrpt::gui::CDisplayWindow3D::Ptr win
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.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Classes for creating GUI windows for 2D and 3D visualization.
void getAsPose3D(mrpt::math::TPose3D &outPose) const
static Ptr Create(Args &&... args)
3D Plane, represented by its equation
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
This base provides a set of functions for maths stuff.
A generic RANSAC implementation.
size_type cols() const
Number of columns in the matrix.
A namespace of pseudo-random numbers generators of diferent distributions.
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...
void ransac3Dplane_fit(const CMatrixDynamic< T > &allData, const std::vector< size_t > &useIndices, vector< CMatrixDynamic< T >> &fitModels)
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
size_type rows() const
Number of rows in the matrix.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |