/* +---------------------------------------------------------------------------+
| The Mobile Robot Programming Toolkit (MRPT) C++ library |
| |
| http://www.mrpt.org/ |
| |
| Copyright (C) 2005-2011 University of Malaga |
| |
| This software was written by the Machine Perception and Intelligent |
| Robotics Lab, University of Malaga (Spain). |
| Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
| |
| This file is part of the MRPT project. |
| |
| MRPT is free software: you can redistribute it and/or modify |
| it under the terms of the GNU General Public License as published by |
| the Free Software Foundation, either version 3 of the License, or |
| (at your option) any later version. |
| |
| MRPT is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| |
| You should have received a copy of the GNU General Public License |
| along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
| |
+---------------------------------------------------------------------------+ */
#include <mrpt/base.h>
#include <mrpt/slam.h>
#include <mrpt/gui.h>
using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace std;
void ransac3Dplane_fit(
const CMatrixDouble &allData,
const 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,
vector_size_t & out_inlierIndices )
{
ASSERT_( testModels.size()==1 )
out_bestModelIndex = 0;
const CMatrixDouble &M = testModels[0];
ASSERT_( size(M,1)==1 && size(M,2)==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 = size(allData,2);
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 mrpt::vector_size_t &useIndices )
{
return false;
}
// ------------------------------------------------------
// TestRANSAC
// ------------------------------------------------------
void TestRANSAC()
{
randomGenerator.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 = randomGenerator.drawUniform(-3,3);
const double yy = randomGenerator.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) = randomGenerator.drawUniform(-4,4);
data(1,i+N_plane) = randomGenerator.drawUniform(-4,4);
data(2,i+N_plane) = randomGenerator.drawUniform(-4,4);
}
// Run RANSAC
// ------------------------------------
CMatrixDouble best_model;
vector_size_t best_inliers;
const double DIST_THRESHOLD = 0.2;
CTicTac tictac;
const size_t TIMES=100;
for (size_t iters=0;iters<TIMES;iters++)
math::RANSAC::execute(
data,
ransac3Dplane_fit,
ransac3Dplane_distance,
ransac3Dplane_degenerate,
DIST_THRESHOLD,
3, // Minimum set of points
best_inliers,
best_model,
iters==0 // Verbose
);
cout << "Computation time: " << tictac.Tac()*1000.0/TIMES << " ms" << endl;
ASSERT_(size(best_model,1)==1 && size(best_model,2)==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::COpenGLScenePtr scene = opengl::COpenGLScene::Create();
scene->insert( opengl::CGridPlaneXY::Create(-20,20,-20,20,0,1) );
scene->insert( opengl::stock_objects::CornerXYZ() );
opengl::CPointCloudPtr points = opengl::CPointCloud::Create();
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::CTexturedPlanePtr glPlane = opengl::CTexturedPlane::Create(-4,4,-4,4);
CPose3D 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;
}
}
