Example: math_ransac_plane3d_example
This example shows how to use MRPT’s RANSAC C++ template to detect planes from a 3D point cloud given as a matrix (each row is a point).
See also the RANSAC tutorial page.
![math_ransac_plane3d_example screenshot](_images/math_ransac_plane3d_example_screenshot.gif)
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include <mrpt/gui/CDisplayWindow3D.h> #include <mrpt/math/ransac.h> #include <mrpt/opengl/CGridPlaneXY.h> #include <mrpt/opengl/CPointCloud.h> #include <mrpt/opengl/CTexturedPlane.h> #include <mrpt/opengl/stock_objects.h> #include <mrpt/poses/CPose3D.h> #include <mrpt/random.h> #include <mrpt/system/CTicTac.h> #include <iostream> using namespace mrpt; using namespace mrpt::gui; using namespace mrpt::math; using namespace mrpt::random; using namespace mrpt::poses; using namespace mrpt::system; using namespace std; void ransac3Dplane_fit( const CMatrixDouble& allData, const std::vector<size_t>& useIndices, vector<CMatrixDouble>& fitModels) { ASSERT_(useIndices.size() == 3); TPoint3D p1( allData(0, useIndices[0]), allData(1, useIndices[0]), allData(2, useIndices[0])); TPoint3D p2( allData(0, useIndices[1]), allData(1, useIndices[1]), allData(2, useIndices[1])); TPoint3D p3( allData(0, useIndices[2]), allData(1, useIndices[2]), allData(2, useIndices[2])); try { TPlane plane(p1, p2, p3); fitModels.resize(1); CMatrixDouble& M = fitModels[0]; M.setSize(1, 4); for (size_t i = 0; i < 4; i++) M(0, i) = plane.coefs[i]; } catch (exception&) { fitModels.clear(); return; } } void ransac3Dplane_distance( const CMatrixDouble& allData, const vector<CMatrixDouble>& testModels, const double distanceThreshold, unsigned int& out_bestModelIndex, std::vector<size_t>& out_inlierIndices) { ASSERT_(testModels.size() == 1); out_bestModelIndex = 0; const CMatrixDouble& M = testModels[0]; ASSERT_(M.rows() == 1 && M.cols() == 4); TPlane plane; plane.coefs[0] = M(0, 0); plane.coefs[1] = M(0, 1); plane.coefs[2] = M(0, 2); plane.coefs[3] = M(0, 3); const size_t N = allData.cols(); out_inlierIndices.clear(); out_inlierIndices.reserve(100); for (size_t i = 0; i < N; i++) { const double d = plane.distance( TPoint3D(allData(0, i), allData(1, i), allData(2, i))); if (d < distanceThreshold) out_inlierIndices.push_back(i); } } bool ransac3Dplane_degenerate( const CMatrixDouble& allData, const std::vector<size_t>& useIndices) { return false; } // ------------------------------------------------------ // TestRANSAC // ------------------------------------------------------ void TestRANSAC() { getRandomGenerator().randomize(); // Generate random points: // ------------------------------------ const size_t N_plane = 300; const size_t N_noise = 100; const double PLANE_EQ[4] = {1, -1, 1, -2}; CMatrixDouble data(3, N_plane + N_noise); for (size_t i = 0; i < N_plane; i++) { const double xx = getRandomGenerator().drawUniform(-3, 3); const double yy = getRandomGenerator().drawUniform(-3, 3); const double zz = -(PLANE_EQ[3] + PLANE_EQ[0] * xx + PLANE_EQ[1] * yy) / PLANE_EQ[2]; data(0, i) = xx; data(1, i) = yy; data(2, i) = zz; } for (size_t i = 0; i < N_noise; i++) { data(0, i + N_plane) = getRandomGenerator().drawUniform(-4, 4); data(1, i + N_plane) = getRandomGenerator().drawUniform(-4, 4); data(2, i + N_plane) = getRandomGenerator().drawUniform(-4, 4); } // Run RANSAC // ------------------------------------ CMatrixDouble best_model; std::vector<size_t> best_inliers; const double DIST_THRESHOLD = 0.05; CTicTac tictac; const size_t TIMES = 100; math::RANSAC myransac; for (size_t iters = 0; iters < TIMES; iters++) { myransac.setVerbosityLevel( iters == 0 ? mrpt::system::LVL_DEBUG : mrpt::system::LVL_INFO); myransac.execute( data, ransac3Dplane_fit, ransac3Dplane_distance, ransac3Dplane_degenerate, DIST_THRESHOLD, 3, // Minimum set of points best_inliers, best_model); } cout << "Computation time: " << tictac.Tac() * 1000.0 / TIMES << " ms" << endl; ASSERT_(best_model.rows() == 1 && best_model.cols() == 4); cout << "RANSAC finished: Best model: " << best_model << endl; // cout << "Best inliers: " << best_inliers << endl; TPlane plane( best_model(0, 0), best_model(0, 1), best_model(0, 2), best_model(0, 3)); // Show GUI // -------------------------- mrpt::gui::CDisplayWindow3D win("Set of points", 500, 500); opengl::COpenGLScene::Ptr scene = opengl::COpenGLScene::Create(); scene->insert(opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1)); scene->insert(opengl::stock_objects::CornerXYZ()); opengl::CPointCloud::Ptr points = opengl::CPointCloud::Create(); points->setColor(0, 0, 1); points->setPointSize(3); points->enableColorFromZ(); { std::vector<double> xs, ys, zs; data.extractRow(0, xs); data.extractRow(1, ys); data.extractRow(2, zs); points->setAllPoints(xs, ys, zs); } scene->insert(points); opengl::CTexturedPlane::Ptr glPlane = opengl::CTexturedPlane::Create(-4, 4, -4, 4); glPlane->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00, 0x80)); // RGBA TPose3D glPlanePose; plane.getAsPose3D(glPlanePose); glPlane->setPose(glPlanePose); scene->insert(glPlane); win.get3DSceneAndLock() = scene; win.unlockAccess3DScene(); win.forceRepaint(); win.waitForKey(); } // ------------------------------------------------------ // MAIN // ------------------------------------------------------ int main() { try { TestRANSAC(); return 0; } catch (const std::exception& e) { std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl; return -1; } }