iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
tests/test-PointCloud2Registration.C
/*! @file
@author Shane Grant
@copyright GNU Public License (GPL v3)
@section License
@verbatim
// ////////////////////////////////////////////////////////////////////////
// The iLab Neuromorphic Robotics Toolkit (NRT) //
// Copyright 2010-2012 by the University of Southern California (USC) //
// and the iLab at USC. //
// //
// iLab - University of Southern California //
// Hedco Neurociences Building, Room HNB-10 //
// Los Angeles, Ca 90089-2520 - USA //
// //
// See http://ilab.usc.edu for information about this project. //
// ////////////////////////////////////////////////////////////////////////
// This file is part of The iLab Neuromorphic Robotics Toolkit. //
// //
// The iLab Neuromorphic Robotics Toolkit 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. //
// //
// The iLab Neuromorphic Robotics Toolkit 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 The iLab Neuromorphic Robotics Toolkit. If not, see //
// <http://www.gnu.org/licenses/>. //
// ////////////////////////////////////////////////////////////////////////
@endverbatim */
#include <nrt/config.h>
#ifdef NRT_HAVE_CLOUD
#include <random>
using namespace nrt;
void createRandomCloud( PointCloud2 & cloud, int size )
{
std::mt19937 rng(12345u);
std::uniform_real_distribution<float> dist( -3.0f, 3.0f );
cloud.resize( size );
for( auto i = cloud.geometry_begin(); i != cloud.geometry_end(); ++i )
*i = {dist(rng), dist(rng), dist(rng)};
}
int main( int argc, char ** argv )
{
PointCloud2 cloud1;
createRandomCloud( cloud1, 10 );
PointCloud2 cloud2 = cloud1;
for( auto i = cloud1.geometry_begin(); i != cloud1.geometry_end(); ++i )
{
i->x() += 0.7;
}
// Create the parameters for our registration methods
ConvergenceCriteriaBase::SharedPtr convergeCritera( new StandardConvergenceCriteria(10) );
CorrespondenceEstimationBase::SharedPtr correspondenceEstimator( new CorrespondenceEstimationNearestNeighbor() );
// Construct the registration objects
Registration myRegistrationSVD( correspondenceEstimator, {correspondencePruner},
estimationSVD,
convergeCritera );
Registration myRegistrationLM( correspondenceEstimator, {correspondencePruner},
estimationLM,
convergeCritera );
// Perform registration - given the sample data created above we should
// expect a registration that gives no rotation and an X displacement of -0.7
auto lmResult = myRegistrationLM.align( cloud1, cloud2 );
NRT_INFO( "LM Results (expect -0.7 x translation): " << lmResult.matrix() );
auto svdResult = myRegistrationSVD.align( cloud1, cloud2 );
NRT_INFO( "SVD Results (expect -0.7 x translation): " << svdResult.matrix() );
// Basic test that was in PCL example
PointCloud2 cloud3(5);
cloud3[0] = {0.352222, -0.151883, -0.106395};
cloud3[1] = {-0.397406, -0.473106, 0.292602};
cloud3[2] = {-0.731898, 0.667105, 0.441304};
cloud3[3] = {-0.734766, 0.854581, -0.0361733};
cloud3[4] = {-0.4607, -0.277468, -0.916762};
PointCloud2 cloud4 = cloud3;
for( auto i = cloud3.geometry_begin(); i != cloud3.geometry_end(); ++i )
i->x() += 0.7;
svdResult = myRegistrationSVD.align( cloud3, cloud4 );
NRT_INFO( "SVD PCL example Results (expect -0.7 x translation): " << svdResult.matrix() );
// Testing again with RANSAC based rejection
auto myRegistrationSVDRANSAC = Registration( correspondenceEstimator, {ransacPruner},
estimationSVD,
convergeCritera );
auto myRegistrationLMRANSAC = Registration( correspondenceEstimator, {ransacPruner},
estimationLM,
convergeCritera );
// Perform registration - given the sample data created above we should
// expect a registration that gives no rotation and an X displacement of -0.7
lmResult = myRegistrationLMRANSAC.align( cloud1, cloud2 );
NRT_INFO( "LM RANSAC Results (expect -0.7 x translation): " << lmResult.matrix() );
svdResult = myRegistrationSVDRANSAC.align( cloud1, cloud2 );
NRT_INFO( "SVD RANSAC Results (expect -0.7 x translation): " << svdResult.matrix() );
svdResult = myRegistrationSVDRANSAC.align( cloud3, cloud4 );
NRT_INFO( "SVD RANSAC PCL example Results (expect -0.7 x translation): " << svdResult.matrix() );
// Testing with rotation based transforms
PointCloud2 cloud5 = cloud1;
PointCloud2::AffineTransform transform = PointCloud2::AffineTransform::Identity();
transform.rotate( Eigen::AngleAxisf( 0.261799388, Eigen::Vector3f::UnitZ() ) ); // 15 degrees about Z
transform.rotate( Eigen::AngleAxisf( 0.261799388, Eigen::Vector3f::UnitX() ) ); // 15 degrees about X
transformPointCloudInPlace( cloud5, transform );
svdResult = myRegistrationSVD.align( cloud1, cloud5 );
NRT_INFO( "SVD rotation Results: " << svdResult.rotation() );
NRT_INFO( "does that look like this? " << transform.rotation() );
lmResult = myRegistrationLM.align( cloud1, cloud5 );
NRT_INFO( "LM rotation Results: " << svdResult.rotation() );
NRT_INFO( "does that look like this? " << transform.rotation() );
// rotation and translation
transform.translate( Eigen::Vector3f::UnitZ()*2 );
cloud5 = cloud1;
transformPointCloudInPlace( cloud5, transform );
svdResult = myRegistrationSVD.align( cloud1, cloud5 );
NRT_INFO( "SVD rotation translation Results: " << svdResult.matrix() );
NRT_INFO( "does that look like this? " << transform.matrix() );
NRT_INFO( "does this look like zero:");
auto temp = transformPointCloud( cloud1, svdResult );
for( size_t i = 0; i < cloud1.size(); ++i )
NRT_INFO( (temp.at(i).geometry() - cloud5.at(i).geometry() ) );
lmResult = myRegistrationLM.align( cloud1, cloud5 );
NRT_INFO( "LM rotation translation Results: " << svdResult.matrix() );
NRT_INFO( "does that look like this? " << transform.matrix() );
// with ransac
svdResult = myRegistrationSVDRANSAC.align( cloud1, cloud5 );
NRT_INFO( "SVD RANSAC rotation translation Results: " << svdResult.matrix() );
NRT_INFO( "does that look like this? " << transform.matrix() );
lmResult = myRegistrationLMRANSAC.align( cloud1, cloud5 );
NRT_INFO( "LM RANSAC rotation translation Results: " << svdResult.matrix() );
NRT_INFO( "does that look like this? " << transform.matrix() );
// with normals
Normals normalWorker(PointCloudSearchMethod::knn, 5 );
auto normalCloud = normalWorker.computeFeatures( cloud1 );
cloud5 = normalCloud;
auto myRegistrationP2PRANSAC = Registration( correspondenceEstimator, {ransacPruner},
estimationP2P,
convergeCritera );
auto p2pResult = myRegistrationP2PRANSAC.align( normalCloud, cloud5 );
NRT_INFO( "PointToPlane rotation translation Results: " << p2pResult.matrix() );
NRT_INFO( "does that look like this? " << transform.matrix() );
// with normals part deux
auto myRegistrationP2PLLSRANSAC = Registration( correspondenceEstimator, {ransacPruner},
estimationP2PLLS,
convergeCritera );
auto p2pllsResult = myRegistrationP2PLLSRANSAC.align( normalCloud, cloud5 );
NRT_INFO( "PointToPlane LLS rotation translation Results: " << p2pllsResult.matrix() );
NRT_INFO( "does that look like this? " << transform.matrix() );
}
#else
int main( int argc, char ** argv )
{
return 1;
}
#endif // NRT_HAVE_CLOUD