test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include <mrpt/math/ransac.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/opengl/CTexturedPlane.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.get_unsafe(0, i), allData.get_unsafe(1, i),
                                allData.get_unsafe(2, i)));
                if (d < distanceThreshold) out_inlierIndices.push_back(i);
        }
}
 
/** Return "true" if the selected points are a degenerate (invalid) case.
 */
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.2;
 
        CTicTac tictac;
        const size_t TIMES = 100;
 
        math::RANSAC myransac;
        for (size_t iters = 0; iters < TIMES; iters++)
                myransac.execute(
                        data, ransac3Dplane_fit, ransac3Dplane_distance,
                        ransac3Dplane_degenerate, DIST_THRESHOLD,
                        3,  // Minimum set of points
                        best_inliers, best_model,
                        iters == 0 ? mrpt::system::LVL_DEBUG
                                           : mrpt::system::LVL_INFO  // Verbose
                        );
 
        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 =
                mrpt::make_aligned_shared<opengl::COpenGLScene>();
 
        scene->insert(
                mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
                        -20, 20, -20, 20, 0, 1));
        scene->insert(opengl::stock_objects::CornerXYZ());
 
        opengl::CPointCloud::Ptr points =
                mrpt::make_aligned_shared<opengl::CPointCloud>();
        points->setColor(0, 0, 1);
        points->setPointSize(3);
        points->enableColorFromZ();
 
        {
                std::vector<float> xs, ys, zs;
 
                data.extractRow(0, xs);
                data.extractRow(1, ys);
                data.extractRow(2, zs);
                points->setAllPointsFast(xs, ys, zs);
        }
 
        scene->insert(points);
 
        opengl::CTexturedPlane::Ptr glPlane =
                mrpt::make_aligned_shared<opengl::CTexturedPlane>(-4, 4, -4, 4);
 
        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 (std::exception& e)
        {
                std::cout << "MRPT exception caught: " << e.what() << std::endl;
                return -1;
        }
        catch (...)
        {
                printf("Untyped exception!!");
                return -1;
        }
}