test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, 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::COpenGLScene::Ptr scene = opengl::COpenGLScene::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;
    }
    catch (...)
    {
        printf("Untyped exception!!");
        return -1;
    }
}