Go to the documentation of this file.
42 return dataset.
size();
48 const std::vector<size_t>& useIndices,
49 vector<mrpt::math::TPlane>& fitModels)
51 ASSERT_(useIndices.size() == 3);
54 for (
int i = 0; i < 3; i++) allData.
getPoint(useIndices[i], pt[i]);
59 fitModels[0] =
TPlane(pt[0], pt[1], pt[2]);
72 const vector<mrpt::math::TPlane>& testModels,
73 const double distanceThreshold,
unsigned int& out_bestModelIndex,
74 std::vector<size_t>& out_inlierIndices)
76 ASSERT_(testModels.size() == 1);
77 out_bestModelIndex = 0;
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++)
88 if (d < distanceThreshold) out_inlierIndices.push_back(i);
96 const std::vector<size_t>& useIndices)
111 const size_t N_plane = 300;
112 const size_t N_noise = 100;
114 const double PLANE_EQ[4] = {1, -1, 1, -2};
117 data.reserve(N_plane + N_noise);
119 for (
size_t i = 0; i < N_plane; i++)
121 const double xx = rng.drawUniform(-3, 3);
122 const double yy = rng.drawUniform(-3, 3);
124 -(PLANE_EQ[3] + PLANE_EQ[0] * xx + PLANE_EQ[1] * yy) / PLANE_EQ[2];
125 data.insertPointFast(xx, yy, zz);
128 for (
size_t i = 0; i < N_noise; i++)
130 data.insertPointFast(
131 rng.drawUniform(-4, 4), rng.drawUniform(-4, 4),
132 rng.drawUniform(-4, 4));
138 std::vector<size_t> best_inliers;
139 const double DIST_THRESHOLD = 0.05;
150 best_inliers, best_model);
152 cout <<
"Computation time: " << tictac.
Tac() * 1000.0 <<
" ms\n";
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;
167 points->setColor(0, 0, 1);
168 points->setPointSize(3);
169 points->enableColorFromZ();
170 points->loadFromPointsMap(&
data);
172 scene->insert(points);
181 glPlane->setPose(glPlanePose);
183 scene->insert(glPlane);
185 win.get3DSceneAndLock() = scene;
186 win.unlockAccess3DScene();
202 catch (
const std::exception& e)
209 printf(
"Untyped exception!!");
std::shared_ptr< mrpt::opengl ::CPointCloud > Ptr
static struct FontData data
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
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.
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
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
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)
double Tac() noexcept
Stops the stopwatch.
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.
size_t ransacDatasetSize(const CMatrixDynamic< T > &dataset)
Define overloaded functions for user types as required.
static Ptr Create(Args &&... args)
mrpt::gui::CDisplayWindow3D::Ptr win
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
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.
A namespace of pseudo-random numbers generators of diferent distributions.
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
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.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020 | |