libpointmatcher integration

corelib/src/CMakeLists.txt

@@ -243,6 +243,17 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)
 
+IF(libpointmatcher_FOUND)
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${libpointmatcher_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${libpointmatcher_LIBRARIES}
+	)
+ENDIF(libpointmatcher_FOUND)
+
 IF(ZED_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}

corelib/src/RegistrationIcp.cpp

@@ -37,6 +37,176 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/io/vtk_io.h>
+#include <pcl/conversions.h>
+
+#ifdef RTABMAP_POINTMATCHER
+#include "pointmatcher/PointMatcher.h"
+typedef PointMatcher<float> PM;
+typedef PM::DataPoints DP;
+
+DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
+{
+	typedef DP::Label Label;
+	typedef DP::Labels Labels;
+	typedef DP::View View;
+
+	if (pclCloud->empty())
+		return DP();
+
+	// fill labels
+	// conversions of descriptor fields from pcl
+	// see http://www.ros.org/wiki/pcl/Overview
+	Labels featLabels;
+	Labels descLabels;
+	std::vector<bool> isFeature;
+	featLabels.push_back(Label("x", 1));
+	isFeature.push_back(true);
+	featLabels.push_back(Label("y", 1));
+	isFeature.push_back(true);
+	featLabels.push_back(Label("z", 1));
+	isFeature.push_back(true);
+	featLabels.push_back(Label("pad", 1));
+
+	// create cloud
+	DP cloud(featLabels, descLabels, pclCloud->size());
+	cloud.getFeatureViewByName("pad").setConstant(1);
+
+	// fill cloud
+	View viewX(cloud.getFeatureViewByName("x"));
+	View viewY(cloud.getFeatureViewByName("y"));
+	View viewZ(cloud.getFeatureViewByName("z"));
+	for(unsigned int i=0; i<pclCloud->size(); ++i)
+	{
+		viewX(0, i) = pclCloud->at(i).x;
+		viewY(0, i) = pclCloud->at(i).y;
+		viewZ(0, i) = pclCloud->at(i).z;
+	}
+
+	return cloud;
+}
+
+DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
+{
+	typedef DP::Label Label;
+	typedef DP::Labels Labels;
+	typedef DP::View View;
+
+	if (pclCloud->empty())
+		return DP();
+
+	// fill labels
+	// conversions of descriptor fields from pcl
+	// see http://www.ros.org/wiki/pcl/Overview
+	Labels featLabels;
+	Labels descLabels;
+	std::vector<bool> isFeature;
+	featLabels.push_back(Label("x", 1));
+	isFeature.push_back(true);
+	featLabels.push_back(Label("y", 1));
+	isFeature.push_back(true);
+	featLabels.push_back(Label("z", 1));
+	isFeature.push_back(true);
+
+	descLabels.push_back(Label("normals", 3));
+	isFeature.push_back(false);
+	isFeature.push_back(false);
+	isFeature.push_back(false);
+
+	featLabels.push_back(Label("pad", 1));
+
+	// create cloud
+	DP cloud(featLabels, descLabels, pclCloud->size());
+	cloud.getFeatureViewByName("pad").setConstant(1);
+
+	// fill cloud
+	View viewX(cloud.getFeatureViewByName("x"));
+	View viewY(cloud.getFeatureViewByName("y"));
+	View viewZ(cloud.getFeatureViewByName("z"));
+	View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
+	View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
+	View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
+	for(unsigned int i=0; i<pclCloud->size(); ++i)
+	{
+		viewX(0, i) = pclCloud->at(i).x;
+		viewY(0, i) = pclCloud->at(i).y;
+		viewZ(0, i) = pclCloud->at(i).z;
+		viewNormalX(0, i) = pclCloud->at(i).normal_x;
+		viewNormalY(0, i) = pclCloud->at(i).normal_y;
+		viewNormalZ(0, i) = pclCloud->at(i).normal_z;
+	}
+
+	return cloud;
+}
+
+void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
+{
+	typedef DP::ConstView ConstView;
+
+	if (cloud.features.cols() == 0)
+		return;
+
+	pclCloud.resize(cloud.features.cols());
+	pclCloud.is_dense = true;
+
+		// fill cloud
+	ConstView viewX(cloud.getFeatureViewByName("x"));
+	ConstView viewY(cloud.getFeatureViewByName("y"));
+	ConstView viewZ(cloud.getFeatureViewByName("z"));
+	for(unsigned int i=0; i<pclCloud.size(); ++i)
+	{
+		pclCloud.at(i).x = viewX(0, i);
+		pclCloud.at(i).y = viewY(0, i);
+		pclCloud.at(i).z = viewZ(0, i);
+	}
+}
+
+void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
+{
+	typedef DP::ConstView ConstView;
+
+	if (cloud.features.cols() == 0)
+		return;
+
+	pclCloud.resize(cloud.features.cols());
+	pclCloud.is_dense = true;
+
+		// fill cloud
+	ConstView viewX(cloud.getFeatureViewByName("x"));
+	ConstView viewY(cloud.getFeatureViewByName("y"));
+	ConstView viewZ(cloud.getFeatureViewByName("z"));
+	ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
+	ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
+	ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
+	for(unsigned int i=0; i<pclCloud.size(); ++i)
+	{
+		pclCloud.at(i).x = viewX(0, i);
+		pclCloud.at(i).y = viewY(0, i);
+		pclCloud.at(i).z = viewZ(0, i);
+		pclCloud.at(i).normal_x = viewNormalX(0, i);
+		pclCloud.at(i).normal_y = viewNormalY(0, i);
+		pclCloud.at(i).normal_z = viewNormalZ(0, i);
+	}
+}
+
+template<typename T>
+typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
+{
+	typedef typename PointMatcher<T>::TransformationParameters M;
+	assert(matrix.rows() == matrix.cols());
+	assert((matrix.rows() == 3) || (matrix.rows() == 4));
+	assert((dimp1 == 3) || (dimp1 == 4));
+
+	if (matrix.rows() == dimp1)
+		return matrix;
+
+	M out(M::Identity(dimp1,dimp1));
+	out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2);
+	out.topRightCorner(2,1) = matrix.topRightCorner(2,1);
+	return out;
+}
+
+#endif
 
 namespace rtabmap {
 
@@ -51,11 +221,24 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_epsilon(Parameters::defaultIcpEpsilon()),
 	_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
-	_pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors())
+	_pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()),
+	_libpointmatcher(Parameters::defaultIcpPM()),
+	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
+	_libpointmatcherICP(0)
 {
 	this->parseParameters(parameters);
 }
 
+RegistrationIcp::~RegistrationIcp()
+{
+#ifdef RTABMAP_POINTMATCHER
+	if(_libpointmatcherICP)
+	{
+		delete (PM::ICP*)_libpointmatcherICP;
+	}
+#endif
+}
+
 void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 {
 	Registration::parseParameters(parameters);
@@ -71,6 +254,81 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors);
 
+	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
+	Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
+#ifndef RTABMAP_POINTMATCHER
+	if(_libpointmatcher)
+	{
+		UWARN("Parameter %s is set to true but RTAB-MAp has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
+		_libpointmatcher = false;
+	}
+#else
+	if(_libpointmatcher)
+	{
+		UINFO("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str());
+		if(_libpointmatcherICP!=0)
+		{
+			delete (PM::ICP*)_libpointmatcherICP;
+			_libpointmatcherICP = 0;
+		}
+
+		_libpointmatcherICP = new PM::ICP();
+
+		PM::ICP * icp = (PM::ICP*)_libpointmatcherICP;
+
+		bool useDefaults = true;
+		if(!_libpointmatcherConfig.empty())
+		{
+			// load YAML config
+			std::ifstream ifs(_libpointmatcherConfig.c_str());
+			if (ifs.good())
+			{
+				icp->loadFromYaml(ifs);
+				useDefaults = false;
+			}
+			else
+			{
+				UERROR("Cannot open libpointmatcher config file \"%s\", using default values instead.", _libpointmatcherConfig.c_str());
+			}
+		}
+		if(useDefaults)
+		{
+			// Create the default ICP algorithm
+			// See the implementation of setDefault() to create a custom ICP algorithm
+			icp->setDefault();
+
+			icp->readingDataPointsFilters.clear();
+			icp->readingDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
+
+			icp->referenceDataPointsFilters.clear();
+			icp->referenceDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
+
+			PM::Parameters params;
+			params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
+			icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
+			params.clear();
+
+			params["ratio"] = uNumber2Str(0.65); // For kinect cloud, 0.65 is better than 0.85
+			icp->outlierFilters.clear();
+			icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
+			params.clear();
+
+			icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create(_pointToPlane?"PointToPlaneErrorMinimizer":"PointToPointErrorMinimizer"));
+
+			icp->transformationCheckers.clear();
+			params["maxIterationCount"] = uNumber2Str(_maxIterations);
+			icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("CounterTransformationChecker", params));
+			params.clear();
+
+			params["minDiffRotErr"] =   uNumber2Str(_epsilon*_epsilon*100.0f);
+			params["minDiffTransErr"] = uNumber2Str(_epsilon*_epsilon);
+			params["smoothLength"] =    uNumber2Str(4);
+			icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
+			params.clear();
+		}
+	}
+#endif
+
 	UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
 	UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
 	UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
@@ -90,12 +348,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Voxel size=%f", _voxelSize);
 	UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
 	UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
-	UDEBUG("Max corrrespondence distance=%f", _maxCorrespondenceDistance);
+	UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
 	UDEBUG("Max Iterations=%d", _maxIterations);
 	UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
 	UDEBUG("Max translation=%f", _maxTranslation);
 	UDEBUG("Max rotation=%f", _maxRotation);
 	UDEBUG("Downsampling step=%d", _downsamplingStep);
+	UDEBUG("libpointmatcher=%d", _libpointmatcher?1:0);
 
 	UTimer timer;
 	std::string msg;
@@ -149,15 +408,50 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 				UDEBUG("Conversion time = %f s", timer.ticks());
 				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
-				icpT = util3d::icpPointToPlane(
-						fromCloudNormals,
-						toCloudNormals,
-					   _maxCorrespondenceDistance,
-					   _maxIterations,
-					   hasConverged,
-					   *fromCloudNormalsRegistered,
-					   _epsilon,
-					   this->force3DoF());
+#ifdef RTABMAP_POINTMATCHER
+				if(_libpointmatcher)
+				{
+					// Load point clouds
+					DP data = pclToDP(fromCloudNormals);
+					DP ref = pclToDP(toCloudNormals);
+
+					// Compute the transformation to express data in ref
+					PM::TransformationParameters T;
+					try
+					{
+						UASSERT(_libpointmatcherICP != 0);
+						PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+						T = icp(data, ref);
+						icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+
+						float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+						UDEBUG("match ratio: %f", matchRatio);
+
+						if(!icpT.isNull())
+						{
+							fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+							hasConverged = true;
+						}
+					}
+					catch(const std::exception & e)
+					{
+						UWARN("libpointmatcher has failed: %s", e.what());
+					}
+				}
+				else
+				{
+#endif
+					icpT = util3d::icpPointToPlane(
+							fromCloudNormals,
+							toCloudNormals,
+						   _maxCorrespondenceDistance,
+						   _maxIterations,
+						   hasConverged,
+						   *fromCloudNormalsRegistered,
+						   _epsilon,
+						   this->force3DoF());
+				}
+
 				if(!icpT.isNull() && hasConverged)
 				{
 					util3d::computeVarianceAndCorrespondences(
@@ -219,15 +513,52 @@ Transform RegistrationIcp::computeTransformationImpl(
 					if(toCloudNormals->size() && fromCloudNormals->size())
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
-						icpT = util3d::icpPointToPlane(
-								fromCloudNormals,
-								toCloudNormals,
-							   _maxCorrespondenceDistance,
-							   _maxIterations,
-							   hasConverged,
-							   *fromCloudNormalsRegistered,
-							   _epsilon,
-							   this->force3DoF());
+
+#ifdef RTABMAP_POINTMATCHER
+						if(_libpointmatcher)
+						{
+							// Load point clouds
+							DP data = pclToDP(fromCloudNormals);
+							DP ref = pclToDP(toCloudNormals);
+
+							// Compute the transformation to express data in ref
+							PM::TransformationParameters T;
+							try
+							{
+								UASSERT(_libpointmatcherICP != 0);
+								PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+								T = icp(data, ref);
+								icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+
+								float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+								UDEBUG("match ratio: %f", matchRatio);
+
+								if(!icpT.isNull())
+								{
+									fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+									hasConverged = true;
+								}
+							}
+							catch(const std::exception & e)
+							{
+								UWARN("libpointmatcher has failed: %s", e.what());
+							}
+						}
+						else
+#endif
+						{
+							icpT = util3d::icpPointToPlane(
+									fromCloudNormals,
+									toCloudNormals,
+								   _maxCorrespondenceDistance,
+								   _maxIterations,
+								   hasConverged,
+								   *fromCloudNormalsRegistered,
+								   _epsilon,
+								   this->force3DoF());
+						}
+
+
 						if(!icpT.isNull() && hasConverged)
 						{
 							util3d::computeVarianceAndCorrespondences(
@@ -248,15 +579,49 @@ Transform RegistrationIcp::computeTransformationImpl(
 						toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
 					}
 
-					icpT = util3d::icp(
-							fromCloudFiltered,
-							toCloudFiltered,
-						   _maxCorrespondenceDistance,
-						   _maxIterations,
-						   hasConverged,
-						   *fromCloudRegistered,
-						   _epsilon,
-						   this->force3DoF()); // icp2D
+#ifdef RTABMAP_POINTMATCHER
+					if(_libpointmatcher)
+					{
+						// Load point clouds
+						DP data = pclToDP(fromCloudFiltered);
+						DP ref = pclToDP(toCloudFiltered);
+
+						// Compute the transformation to express data in ref
+						PM::TransformationParameters T;
+						try
+						{
+							UASSERT(_libpointmatcherICP != 0);
+							PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+							T = icp(data, ref);
+							icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+
+							float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+							UDEBUG("match ratio: %f", matchRatio);
+
+							if(!icpT.isNull())
+							{
+								fromCloudRegistered = util3d::transformPointCloud(fromCloudFiltered, icpT);
+								hasConverged = true;
+							}
+						}
+						catch(const std::exception & e)
+						{
+							UWARN("libpointmatcher has failed: %s", e.what());
+						}
+					}
+					else
+#endif
+					{
+						icpT = util3d::icp(
+								fromCloudFiltered,
+								toCloudFiltered,
+							   _maxCorrespondenceDistance,
+							   _maxIterations,
+							   hasConverged,
+							   *fromCloudRegistered,
+							   _epsilon,
+							   this->force3DoF()); // icp2D
+					}
 
 					if(!icpT.isNull() && hasConverged)
 					{