# 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).

C++ example source code:

```/* +------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)            |
|                          https://www.mrpt.org/                         |
|                                                                        |
| Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
+------------------------------------------------------------------------+ */

#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::Scene::Ptr scene = opengl::Scene::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;
}
}```