Example: math_ransac_examples
![math_ransac_examples screenshot](_images/math_ransac_examples_screenshot.png)
![math_ransac_examples screenshot](_images/math_ransac_examples_screenshot.png)
![math_ransac_examples screenshot](_images/math_ransac_examples_screenshot.png)
![math_ransac_examples screenshot](_images/math_ransac_examples_screenshot.png)
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2024, 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/containers/copy_container_typecasting.h> #include <mrpt/gui/CDisplayWindow3D.h> #include <mrpt/gui/CDisplayWindowPlots.h> #include <mrpt/math/ransac_applications.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; mrpt::gui::CDisplayWindow3D::Ptr win; // ------------------------------------------------------ // TestRANSACPlanes // ------------------------------------------------------ void TestRANSACPlanes() { getRandomGenerator().randomize(); // Generate random points: // ------------------------------------ const size_t N_PLANES = 3; const size_t N_plane = 300; const size_t N_noise = 300; const double PLANE_EQ[N_PLANES][4] = { {1, -1, 1, -2}, {1, +1.5, 1, -1}, {0, -1, 1, +2}}; CVectorDouble xs, ys, zs; for (size_t p = 0; p < N_PLANES; p++) { for (size_t i = 0; i < N_plane; i++) { const double xx = getRandomGenerator().drawUniform(-3, 3) + 5 * cos(0.4 * p); const double yy = getRandomGenerator().drawUniform(-3, 3) + 5 * sin(0.4 * p); const double zz = -(PLANE_EQ[p][3] + PLANE_EQ[p][0] * xx + PLANE_EQ[p][1] * yy) / PLANE_EQ[p][2]; xs.push_back(xx); ys.push_back(yy); zs.push_back(zz); } } for (size_t i = 0; i < N_noise; i++) { xs.push_back(getRandomGenerator().drawUniform(-7, 7)); ys.push_back(getRandomGenerator().drawUniform(-7, 7)); zs.push_back(getRandomGenerator().drawUniform(-7, 7)); } // Run RANSAC // ------------------------------------ vector<pair<size_t, TPlane>> detectedPlanes; const double DIST_THRESHOLD = 0.05; CTicTac tictac; ransac_detect_3D_planes(xs, ys, zs, detectedPlanes, DIST_THRESHOLD, 40); // Display output: cout << "RANSAC method: ransac_detect_3D_planes" << endl; cout << " Computation time: " << tictac.Tac() * 1000.0 << " ms" << endl; cout << " " << detectedPlanes.size() << " planes detected." << endl; // Show GUI // -------------------------- win = mrpt::gui::CDisplayWindow3D::Create("RANSAC: 3D planes", 500, 500); opengl::Scene::Ptr scene = opengl::Scene::Create(); scene->insert(opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1)); scene->insert(opengl::stock_objects::CornerXYZ()); for (vector<pair<size_t, TPlane>>::iterator p = detectedPlanes.begin(); p != detectedPlanes.end(); ++p) { auto glPlane = opengl::CTexturedPlane::Create(-10, 10, -10, 10); TPose3D glPlanePose; p->second.getAsPose3D(glPlanePose); glPlane->setPose(glPlanePose); glPlane->setColor( getRandomGenerator().drawUniform<float>(0, 1), getRandomGenerator().drawUniform<float>(0, 1), getRandomGenerator().drawUniform<float>(0, 1), 0.6f); scene->insert(glPlane); } { auto points = opengl::CPointCloud::Create(); points->setColor(0, 0, 1); points->setPointSize(3); points->enableColorFromZ(); // Convert double -> float: vector<float> xsf, ysf, zsf; mrpt::containers::copy_container_typecasting(xs, xsf); mrpt::containers::copy_container_typecasting(ys, ysf); mrpt::containers::copy_container_typecasting(zs, zsf); points->setAllPoints(xsf, ysf, zsf); scene->insert(points); } win->get3DSceneAndLock() = scene; win->unlockAccess3DScene(); win->forceRepaint(); win->waitForKey(); } // ------------------------------------------------------ // TestRANSACLines // ------------------------------------------------------ void TestRANSACLines() { getRandomGenerator().randomize(); // Generate random points in 2D // ------------------------------------ const size_t N_LINES = 4; const size_t N_line = 30; const size_t N_noise = 50; const double LINE_EQ[N_LINES][3] = { {1, -1, -2}, {1, +1.5, -1}, {0, -1, +2}, {0.5, -0.3, +1}}; CVectorDouble xs, ys; for (size_t p = 0; p < N_LINES; p++) { for (size_t i = 0; i < N_line; i++) { const double xx = getRandomGenerator().drawUniform(-10, 10); const double yy = getRandomGenerator().drawGaussian1D(0, 0.05) - (LINE_EQ[p][2] + LINE_EQ[p][0] * xx) / LINE_EQ[p][1]; xs.push_back(xx); ys.push_back(yy); } } for (size_t i = 0; i < N_noise; i++) { xs.push_back(getRandomGenerator().drawUniform(-15, 15)); ys.push_back(getRandomGenerator().drawUniform(-15, 15)); } // Run RANSAC // ------------------------------------ vector<pair<size_t, TLine2D>> detectedLines; const double DIST_THRESHOLD = 0.2; CTicTac tictac; ransac_detect_2D_lines(xs, ys, detectedLines, DIST_THRESHOLD, 20); // Display output: cout << "RANSAC method: ransac_detect_2D_lines" << endl; cout << " Computation time: " << tictac.Tac() * 1000.0 << " ms" << endl; cout << " " << detectedLines.size() << " lines detected." << endl; // Show GUI // -------------------------- mrpt::gui::CDisplayWindowPlots win2("Set of points", 500, 500); win2.plot(xs, ys, ".b4", "points"); unsigned int n = 0; for (vector<pair<size_t, TLine2D>>::iterator p = detectedLines.begin(); p != detectedLines.end(); ++p) { CVectorDouble lx(2), ly(2); lx[0] = -15; lx[1] = 15; for (CVectorDouble::Index q = 0; q < lx.size(); q++) ly[q] = -(p->second.coefs[2] + p->second.coefs[0] * lx[q]) / p->second.coefs[1]; win2.plot(lx, ly, "r-1", format("line_%u", n++)); } win2.axis_fit(); win2.axis_equal(); win2.waitForKey(); } // ------------------------------------------------------ // MAIN // ------------------------------------------------------ int main() { try { TestRANSACPlanes(); cout << endl << "Now running detection of lines..." << endl << endl; TestRANSACLines(); win.reset(); return 0; } catch (const std::exception& e) { std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl; return -1; } }