Added parameter "IcpPointToPlaneMaxComplexity". Added util3d::computeNormalsComplexity(). OdomInfo has now RegistrationInfo field to avoid duplicating members.

corelib/include/rtabmap/core/OdometryEvent.h

@@ -41,7 +41,7 @@ class OdometryEvent : public UEvent
 public:
 	OdometryEvent()
 	{
-		_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+		_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 	OdometryEvent(
 		const SensorData & data,
@@ -51,17 +51,17 @@ public:
 			_pose(pose),
 			_info(info)
 	{
-		if(_info.covariance.empty())
+		if(_info.reg.covariance.empty())
 		{
-			_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+			_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 		}
-		UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1);
+		UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
 	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
@@ -69,7 +69,7 @@ public:
 	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
 	const Transform & pose() const {return _pose;}
-	const cv::Mat & covariance() const {return _info.covariance;}
+	const cv::Mat & covariance() const {return _info.reg.covariance;}
 	std::vector<float> velocity() const {
 		if(_info.interval>0.0)
 		{

corelib/include/rtabmap/core/OdometryInfo.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <map>
 #include "rtabmap/core/Transform.h"
+#include "rtabmap/core/RegistrationInfo.h"
 #include <opencv2/features2d/features2d.hpp>
 
 namespace rtabmap {
@@ -39,9 +40,6 @@ class OdometryInfo
 public:
 	OdometryInfo() :
 		lost(true),
-		matches(0),
-		inliers(0),
-		icpInliersRatio(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
@@ -62,10 +60,7 @@ public:
 	{
 		OdometryInfo output;
 		output.lost = lost;
-		output.matches = matches;
+		output.reg = reg.copyWithoutData();
-		output.inliers = inliers;
-		output.icpInliersRatio = icpInliersRatio;
-		output.covariance = covariance.clone();
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
@@ -87,10 +82,7 @@ public:
 	}
 
 	bool lost;
-	int matches;
+	RegistrationInfo reg;
-	int inliers;
-	float icpInliersRatio;
-	cv::Mat covariance;
 	int features;
 	int localMapSize;
 	int localScanMapSize;
@@ -108,12 +100,10 @@ public:
 	Transform transformGroundTruth;
 	float distanceTravelled;
 
-	int type; // 0=F2M, 1=F2F
+	int type;
 
 	// F2M
 	std::multimap<int, cv::KeyPoint> words;
-	std::vector<int> wordMatches;
-	std::vector<int> wordInliers;
 	std::map<int, cv::Point3f> localMap;
 	cv::Mat localScanMap;
 

corelib/include/rtabmap/core/Parameters.h

@@ -513,6 +513,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
     RTABMAP_PARAM(Icp, PointToPlaneK,             int, 20,      "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
     RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 0.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.0,   "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
 
     // libpointmatcher
     RTABMAP_PARAM(Icp, PM,                       bool, false,   "Use libpointmatcher for ICP registration instead of PCL's implementation.");

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -66,6 +66,7 @@ private:
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;
+	float _pointToPlaneMinComplexity;
 	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
 	float _libpointmatcherOutlierRatio;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -39,10 +39,26 @@ public:
 		matches(0),
 		icpInliersRatio(0),
 		icpTranslation(0.0f),
-		icpRotation(0.0f)
+		icpRotation(0.0f),
+		icpStructuralComplexity(0.0f)
+
 	{
 	}
 
+	RegistrationInfo copyWithoutData() const
+	{
+		RegistrationInfo output;
+		output.covariance = covariance.clone();
+		output.rejectedMsg = rejectedMsg;
+		output.inliers = inliers;
+		output.matches = matches;
+		output.icpInliersRatio = icpInliersRatio;
+		output.icpTranslation = icpTranslation;
+		output.icpRotation = icpRotation;
+		output.icpStructuralComplexity = icpStructuralComplexity;
+		return output;
+	}
+
 	cv::Mat covariance;
 	std::string rejectedMsg;
 
@@ -56,6 +72,7 @@ public:
 	float icpInliersRatio;
 	float icpTranslation;
 	float icpRotation;
+	float icpStructuralComplexity;
 };
 
 }

corelib/include/rtabmap/core/Statistics.h

@@ -77,7 +77,10 @@ class RTABMAP_EXP Statistics
 
 	RTABMAP_STATS(NeighborLinkRefining, Accepted,);
 	RTABMAP_STATS(NeighborLinkRefining, Inliers,);
-	RTABMAP_STATS(NeighborLinkRefining, Inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,);
 	RTABMAP_STATS(NeighborLinkRefining, Variance,);
 	RTABMAP_STATS(NeighborLinkRefining, Pts,);
 

corelib/include/rtabmap/core/util3d_surface.h

@@ -262,6 +262,18 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		float normalSmoothingSize = 10.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
+float RTABMAP_EXP computeNormalsComplexity(
+		const cv::Mat & scan);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::Normal> & normals,
+		bool is2d = false);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		bool is2d = false);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		bool is2d = false);
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float searchRadius = 0.0f,

corelib/src/OdometryDVO.cpp

@@ -270,7 +270,7 @@ Transform OdometryDVO::computeTransform(
 	if(info)
 	{
 		info->type = (int)kTypeDVO;
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 	}
 
 	UINFO("Odom update time = %fs", timer.elapsed());

corelib/src/OdometryF2F.cpp

@@ -246,13 +246,17 @@ Transform OdometryF2F::computeTransform(
 
 	if(info)
 	{
-		info->type = 1;
+		info->type = kTypeF2F;
-		info->covariance = regInfo.covariance;
-		info->inliers = regInfo.inliers;
-		info->icpInliersRatio = regInfo.icpInliersRatio;
-		info->matches = regInfo.matches;
 		info->features = newFrame.sensorData().keypoints().size();
 		info->keyFrameAdded = addKeyFrame;
+		if(this->isInfoDataFilled())
+		{
+			info->reg = regInfo;
+		}
+		else
+		{
+			info->reg = regInfo.copyWithoutData();
+		}
 	}
 
 	UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",

corelib/src/OdometryF2M.cpp

@@ -894,10 +894,6 @@ Transform OdometryF2M::computeTransform(
 
 	if(info)
 	{
-		info->covariance = regInfo.covariance;
-		info->inliers = regInfo.inliers;
-		info->matches = regInfo.matches;
-		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->features = nFeatures;
 		info->localKeyFrames = (int)bundlePoses_.size();
 		info->keyFrameAdded = addKeyFrame;
@@ -907,8 +903,11 @@ Transform OdometryF2M::computeTransform(
 
 		if(this->isInfoDataFilled())
 		{
-			info->wordMatches = regInfo.matchesIDs;
+			info->reg = regInfo;
-			info->wordInliers = regInfo.inliersIDs;
+		}
+		else
+		{
+			info->reg = regInfo.copyWithoutData();
 		}
 	}
 

corelib/src/OdometryFovis.cpp

@@ -381,9 +381,9 @@ Transform OdometryFovis::computeTransform(
 		info->type = (int)kTypeFovis;
 		info->keyFrameAdded = fovis_->getChangeReferenceFrames();
 		info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
-		info->matches = fovis_->getMotionEstimator()->getNumMatches();
+		info->reg.matches = fovis_->getMotionEstimator()->getNumMatches();
-		info->inliers = fovis_->getMotionEstimator()->getNumInliers();
+		info->reg.inliers = fovis_->getMotionEstimator()->getNumInliers();
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 
 		if(this->isInfoDataFilled())
 		{

corelib/src/OdometryMono.cpp

@@ -352,7 +352,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 
 					if(this->isInfoDataFilled() && info)
 					{
-						info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
+						info->reg.matchesIDs.insert(info->reg.matchesIDs.end(), matches.begin(), matches.end());
 					}
 					correspondences = (int)matches.size();
 
@@ -397,10 +397,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 
 							if(this->isInfoDataFilled() && info && inliersV.size())
 							{
-								info->wordInliers.resize(inliersV.size());
+								info->reg.inliersIDs.resize(inliersV.size());
 								for(unsigned int i=0; i<inliersV.size(); ++i)
 								{
-									info->wordInliers[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
+									info->reg.inliersIDs[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
 								}
 							}
 
@@ -976,7 +976,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		if(info)
 		{
 			// a very high variance tells that the new pose is not linked with the previous one
-			info->covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
+			info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
 		}
 
 			// generate kpts
@@ -1013,8 +1013,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 	if(this->isInfoDataFilled() && info)
 	{
 		//info->variance = variance;
-		info->inliers = inliers;
+		info->reg.inliers = inliers;
-		info->matches = correspondences;
+		info->reg.matches = correspondences;
 		info->features = nFeatures;
 		info->localMapSize = (int)localMap_.size();
 		info->localMap = localMap_;

corelib/src/OdometryORBSLAM2.cpp

@@ -891,15 +891,15 @@ Transform OdometryORBSLAM2::computeTransform(
 	{
 		info->lost = t.isNull();
 		info->type = (int)kTypeORBSLAM2;
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 		info->localMapSize = totalMapPoints;
 		info->localKeyFrames = totalKfs;
 
 		if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap)
 		{
 			const std::vector<cv::KeyPoint> & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys;
-			info->wordMatches.resize(kpts.size());
+			info->reg.matchesIDs.resize(kpts.size());
-			info->wordInliers.resize(kpts.size());
+			info->reg.inliersIDs.resize(kpts.size());
 			int oi = 0;
 			for (unsigned int i = 0; i < kpts.size(); ++i)
 			{
@@ -915,14 +915,15 @@ Transform OdometryORBSLAM2::computeTransform(
 				info->words.insert(std::make_pair(wordId, kpts[i]));
 				if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
 				{
-					info->wordMatches[oi] = wordId;
+					info->reg.matchesIDs[oi] = wordId;
-					info->wordInliers[oi] = wordId;
+					info->reg.inliersIDs[oi] = wordId;
 					++oi;
 				}
 			}
-			info->wordMatches.resize(oi);
+			info->reg.matchesIDs.resize(oi);
-			info->wordInliers.resize(oi);
+			info->reg.inliersIDs.resize(oi);
-			info->inliers = oi;
+			info->reg.inliers = oi;
+			info->reg.matches = oi;
 
 			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)

corelib/src/OdometryViso2.cpp

@@ -273,11 +273,11 @@ Transform OdometryViso2::computeTransform(
 	{
 		info->type = (int)kTypeViso2;
 		info->keyFrameAdded = !keep_reference_frame_;
-		info->matches = viso2_->getNumberOfMatches();
+		info->reg.matches = viso2_->getNumberOfMatches();
-		info->inliers = viso2_->getNumberOfInliers();
+		info->reg.inliers = viso2_->getNumberOfInliers();
 		if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
 		{
-			info->covariance = covariance;
+			info->reg.covariance = covariance;
 		}
 
 		if(this->isInfoDataFilled())

corelib/src/RegistrationIcp.cpp

@@ -232,6 +232,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
 	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
 	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
+	_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
 	_libpointmatcher(Parameters::defaultIcpPM()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
@@ -265,6 +266,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
+	UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
 
 	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
 	Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
@@ -428,6 +431,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 			float correspondencesRatio = 0.0f;
 			int correspondences = 0;
 			double variance = 1.0;
+			bool transformComputed = false;
+			bool tooLowComplexityForPlaneToPlane = false;
 
 			if( _pointToPlane &&
 				_voxelSize == 0.0f &&
@@ -436,72 +441,88 @@ Transform RegistrationIcp::computeTransformationImpl(
 				!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
 			{
 				//special case if we have already normals computed and there is no filtering
-				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
-				pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
 
-				fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+				double fromComplexity = util3d::computeNormalsComplexity(fromScan);
-				toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
+				double toComplexity = util3d::computeNormalsComplexity(toScan);
-
+				UWARN("%d->%d %f %f", fromSignature.id(), toSignature.id(), fromComplexity, toComplexity);
-
+				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
-				UDEBUG("Conversion time = %f s", timer.ticks());
+				info.icpStructuralComplexity = complexity;
-				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+				if(complexity < _pointToPlaneMinComplexity)
-#ifdef RTABMAP_POINTMATCHER
-				if(_libpointmatcher)
 				{
-					// Load point clouds
+					tooLowComplexityForPlaneToPlane = true;
-					DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
+					UWARN("ICP PointToPlane ignored as structural complexity is too low: %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
-					DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
-
-					// Compute the transformation to express data in ref
-					PM::TransformationParameters T;
-					try
-					{
-						UASSERT(_libpointmatcherICP != 0);
-						PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
-						UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
-						T = icp(data, ref);
-						icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
-						UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
-
-						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(
+					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
-							fromCloudNormals,
+					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
-							toCloudNormals,
-						   _maxCorrespondenceDistance,
-						   _maxIterations,
-						   hasConverged,
-						   *fromCloudNormalsRegistered,
-						   _epsilon,
-						   this->force3DoF());
-				}
 
-				if(!icpT.isNull() && hasConverged)
+					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
-				{
+					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
-					util3d::computeVarianceAndCorrespondences(
+
-							fromCloudNormalsRegistered,
+
-							toCloudNormals,
+					UDEBUG("Conversion time = %f s", timer.ticks());
-							_maxCorrespondenceDistance,
+					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
-							variance,
+#ifdef RTABMAP_POINTMATCHER
-							correspondences);
+					if(_libpointmatcher)
+					{
+						// Load point clouds
+						DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
+						DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
+
+						// Compute the transformation to express data in ref
+						PM::TransformationParameters T;
+						try
+						{
+							UASSERT(_libpointmatcherICP != 0);
+							PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+							UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
+							T = icp(data, ref);
+							icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+							UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
+
+							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(
+								fromCloudNormalsRegistered,
+								toCloudNormals,
+								_maxCorrespondenceDistance,
+								variance,
+								correspondences);
+					}
+					transformComputed = true;
 				}
 			}
-			else
+
+			if(!transformComputed)
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
@@ -534,44 +555,43 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
 				if(_pointToPlane && // ICP Point To Plane
+					!tooLowComplexityForPlaneToPlane && // if previously rejected above
 					!((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D
 				{
-					pcl::PointCloud<pcl::Normal>::Ptr normals;
 					Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
+					pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
+					if(fromScan.channels() == 2 || fromScan.channels() == 5)
+					{
+						if(_voxelSize > 0.0f)
+						{
+							normalsFrom = util3d::computeNormals2D(
+									fromCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
+						else
+						{
+							normalsFrom = util3d::computeFastOrganizedNormals2D(
+									fromCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
+					}
+					else
+					{
+						normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
+					}
+
 					Transform toT = guess * toLocalTransform;
 					Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
-
+					pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
-					if(fromScan.channels() == 2 || fromScan.channels() == 5)
-					{
-						if(_voxelSize > 0.0f)
-						{
-							normals = util3d::computeNormals2D(
-									fromCloudFiltered,
-									_pointToPlaneK,
-									_pointToPlaneRadius,
-									viewpointFrom);
-						}
-						else
-						{
-							normals = util3d::computeFastOrganizedNormals2D(
-									fromCloudFiltered,
-									_pointToPlaneK,
-									_pointToPlaneRadius,
-									viewpointFrom);
-						}
-					}
-					else
-					{
-						normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
-					}
-					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-					pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
-
 					if(toScan.channels() == 2 || toScan.channels() == 5)
 					{
 						if(_voxelSize > 0.0f)
 						{
-							normals = util3d::computeNormals2D(
+							normalsTo = util3d::computeNormals2D(
 									toCloudFiltered,
 									_pointToPlaneK,
 									_pointToPlaneRadius,
@@ -579,7 +599,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 						}
 						else
 						{
-							normals = util3d::computeFastOrganizedNormals2D(
+							normalsTo = util3d::computeFastOrganizedNormals2D(
 									toCloudFiltered,
 									_pointToPlaneK,
 									_pointToPlaneRadius,
@@ -588,99 +608,116 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 					else
 					{
-						normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
+						normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
 					}
-					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-					pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
 
-					std::vector<int> indices;
+					double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5);
-					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
+					double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5);
-					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+					float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
-
+					info.icpStructuralComplexity = complexity;
-					// update output scans
+					if(complexity < _pointToPlaneMinComplexity)
-					if(fromScan.channels() == 2 || fromScan.channels() == 5)
 					{
-						fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						tooLowComplexityForPlaneToPlane = true;
+						UWARN("ICP PointToPlane ignored as structural complexity is too low: %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
 					}
 					else
 					{
-						fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-					}
+						pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
-					if(toScan.channels() == 2 || toScan.channels() == 5)
-					{
-						toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
-					}
-					else
-					{
-						toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
-					}
 
-					UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
+						pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
 
-					if(toCloudNormals->size() && fromCloudNormals->size())
+						std::vector<int> indices;
-					{
+						toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
-						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+						fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+
+						// update output scans
+						if(fromScan.channels() == 2 || fromScan.channels() == 5)
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						else
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						if(toScan.channels() == 2 || toScan.channels() == 5)
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						else
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
+
+						if(toCloudNormals->size() && fromCloudNormals->size())
+						{
+							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
 
 #ifdef RTABMAP_POINTMATCHER
-						if(_libpointmatcher)
+							if(_libpointmatcher)
-						{
-							// Load point clouds
-							DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
-							DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
-
-							// Compute the transformation to express data in ref
-							PM::TransformationParameters T;
-							try
 							{
-								UASSERT(_libpointmatcherICP != 0);
+								// Load point clouds
-								PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+								DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
-								UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
+								DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
-								T = icp(data, ref);
-								UDEBUG("libpointmatcher icp...done!");
-								icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
 
-								float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+								// Compute the transformation to express data in ref
-								UDEBUG("match ratio: %f", matchRatio);
+								PM::TransformationParameters T;
-
+								try
-								if(!icpT.isNull())
 								{
-									fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+									UASSERT(_libpointmatcherICP != 0);
-									hasConverged = true;
+									PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+									UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
+									T = icp(data, ref);
+									UDEBUG("libpointmatcher icp...done!");
+									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());
 								}
 							}
-							catch(const std::exception & e)
+							else
+#endif
 							{
-								UWARN("libpointmatcher has failed: %s", e.what());
+								icpT = util3d::icpPointToPlane(
+										fromCloudNormals,
+										toCloudNormals,
+									   _maxCorrespondenceDistance,
+									   _maxIterations,
+									   hasConverged,
+									   *fromCloudNormalsRegistered,
+									   _epsilon,
+									   this->force3DoF());
+							}
+
+							if(!icpT.isNull() && hasConverged)
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudNormalsRegistered,
+										toCloudNormals,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
 							}
 						}
-						else
+						transformComputed = true;
-#endif
-						{
-							icpT = util3d::icpPointToPlane(
-									fromCloudNormals,
-									toCloudNormals,
-								   _maxCorrespondenceDistance,
-								   _maxIterations,
-								   hasConverged,
-								   *fromCloudNormalsRegistered,
-								   _epsilon,
-								   this->force3DoF());
-						}
-
-						if(!icpT.isNull() && hasConverged)
-						{
-							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
-									toCloudNormals,
-									_maxCorrespondenceDistance,
-									variance,
-									correspondences);
-						}
 					}
 				}
-				else // ICP Point to Point
+
+				if(!transformComputed) // ICP Point to Point
 				{
-					if(_pointToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher))
+					if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher))
 					{
 						UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
 					}
@@ -707,7 +744,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 
 #ifdef RTABMAP_POINTMATCHER
-					if(_libpointmatcher)
+					if(_libpointmatcher && !_pointToPlane) // don't use libpointmatcher if it is configured for point to plane
 					{
 						// Load point clouds
 						DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);

corelib/src/Rtabmap.cpp

@@ -1123,7 +1123,10 @@ bool Rtabmap::process(
 						}
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
-						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
 					}
 				}

corelib/src/RtabmapThread.cpp

@@ -336,7 +336,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
 					if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
 					{
 						OdometryInfo infoCov;
-						infoCov.covariance = e->info().odomCovariance;
+						infoCov.reg.covariance = e->info().odomCovariance;
 						this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 					}
 					else
@@ -347,7 +347,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
 				else
 				{ 
 					OdometryInfo infoCov;
-					infoCov.covariance = e->info().odomCovariance;
+					infoCov.reg.covariance = e->info().odomCovariance;
 					this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 				}
 				
@@ -570,7 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		if(!lastPose_.isIdentity() &&
 						(odomEvent.pose().isIdentity() ||
-						odomEvent.info().covariance.at<double>(0,0)>=9999))
+						odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
 		{
 			if(odomEvent.pose().isIdentity())
 			{
@@ -578,20 +578,20 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			}
 			else
 			{
-				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().covariance.at<double>(0,0));
+				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0));
 			}
 			pushNewState(kStateTriggeringMap);
 			covariance_ = cv::Mat();
 		}
 
-		if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
+		if(uIsFinite(odomEvent.info().reg.covariance.at<double>(0,0)) &&
-			odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
+			odomEvent.info().reg.covariance.at<double>(0,0) != 1.0 &&
-			odomEvent.info().covariance.at<double>(0,0)>0.0)
+			odomEvent.info().reg.covariance.at<double>(0,0)>0.0)
 		{
 			// Use largest covariance error (to be independent of the odometry frame rate)
-			if(covariance_.empty() || odomEvent.info().covariance.at<double>(0,0) > covariance_.at<double>(0,0))
+			if(covariance_.empty() || odomEvent.info().reg.covariance.at<double>(0,0) > covariance_.at<double>(0,0))
 			{
-				covariance_ = odomEvent.info().covariance;
+				covariance_ = odomEvent.info().reg.covariance;
 			}
 		}
 
@@ -614,7 +614,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			covariance_ = cv::Mat::eye(6,6,CV_64FC1);
 		}
 		OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
-		odomInfo.covariance = covariance_;
+		odomInfo.reg.covariance = covariance_;
 		if(ignoreFrame)
 		{
 			// set negative id so rtabmap will detect it as an intermediate node

corelib/src/util3d.cpp

@@ -1808,7 +1808,7 @@ cv::Mat projectCloudToCamera(
 {
 	UASSERT(!cameraTransform.isNull());
 	UASSERT(!laserScan.empty());
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
 
 	float fx = cameraMatrixK.at<double>(0,0);
@@ -1819,46 +1819,25 @@ cv::Mat projectCloudToCamera(
 	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
 	Transform t = cameraTransform.inverse();
 
-	const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
-	const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
-	const cv::Vec4f* vec4Ptr = laserScan.ptr<cv::Vec4f>();
-	const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
-	const float* vec7Ptr = laserScan.ptr<float>();
-
 	int count = 0;
 	for(int i=0; i<laserScan.cols; ++i)
 	{
+		const float* ptr = laserScan.ptr<float>(0, i);
+
 		// Get 3D from laser scan
 		cv::Point3f ptScan;
-		if(laserScan.type() == CV_32FC2)
+		if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
 		{
-			ptScan.x = vec2Ptr[i][0];
+			// 2D scans
-			ptScan.y = vec2Ptr[i][1];
+			ptScan.x = ptr[0];
+			ptScan.y = ptr[1];
 			ptScan.z = 0;
 		}
-		else if(laserScan.type() == CV_32FC3)
+		else // 3D scans
 		{
-			ptScan.x = vec3Ptr[i][0];
+			ptScan.x = ptr[0];
-			ptScan.y = vec3Ptr[i][1];
+			ptScan.y = ptr[1];
-			ptScan.z = vec3Ptr[i][2];
+			ptScan.z = ptr[2];
-		}
-		else if(laserScan.type() == CV_32FC(4))
-		{
-			ptScan.x = vec4Ptr[i][0];
-			ptScan.y = vec4Ptr[i][1];
-			ptScan.z = vec4Ptr[i][2];
-		}
-		else if(laserScan.type() == CV_32FC(6))
-		{
-			ptScan.x = vec6Ptr[i][0];
-			ptScan.y = vec6Ptr[i][1];
-			ptScan.z = vec6Ptr[i][2];
-		}
-		else // 7f
-		{
-			ptScan.x = (vec7Ptr+i*7)[0];
-			ptScan.y = (vec7Ptr+i*7)[1];
-			ptScan.z = (vec7Ptr+i*7)[2];
 		}
 		ptScan = util3d::transformPoint(ptScan, t);
 

corelib/src/util3d_surface.cpp

@@ -2338,6 +2338,153 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 	return normals;
 }
 
+float computeNormalsComplexity(const cv::Mat & scan)
+{
+	if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7))
+	{
+		 //Construct a buffer used by the pca analysis
+		int sz = static_cast<int>(scan.cols*2);
+		bool is2d = scan.channels() == 5;
+		cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+		int oi = 0;
+		for (int i = 0; i < scan.cols; ++i)
+		{
+			const float * ptrScan = scan.ptr<float>(0, i);
+			if(scan.channels() == 5)
+			{
+				if(uIsFinite(ptrScan[2]) && uIsFinite(ptrScan[3]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[2];
+					ptr[1] = ptrScan[3];
+				}
+			}
+			else if(scan.channels() == 6)
+			{
+				if(uIsFinite(ptrScan[3]) && uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[3];
+					ptr[1] = ptrScan[4];
+					ptr[2] = ptrScan[5];
+				}
+			}
+			else
+			{
+				if(uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]) && uIsFinite(ptrScan[6]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[4];
+					ptr[1] = ptrScan[5];
+					ptr[2] = ptrScan[6];
+				}
+			}
+		}
+		if(oi>1)
+		{
+			cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
+
+			// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
+			return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+		}
+	}
+	else if(!scan.empty())
+	{
+		UERROR("Scan doesn't have normals!");
+	}
+	return 0.0f;
+}
+
+float computeNormalsComplexity(const pcl::PointCloud<pcl::PointNormal> & cloud, bool is2d)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(cloud.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointNormal & pt = cloud.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
+
+		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
+		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
+float computeNormalsComplexity(const pcl::PointCloud<pcl::Normal> & normals, bool is2d)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(normals.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	for (unsigned int i = 0; i < normals.size(); ++i)
+	{
+		const pcl::Normal & pt = normals.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
+
+		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
+		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
+float computeNormalsComplexity(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, bool is2d)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(cloud.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointXYZRGBNormal & pt = cloud.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
+
+		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
+		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float searchRadius,

guilib/src/MainWindow.cpp

@@ -560,6 +560,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->updateStat("Odometry/Inliers/", false);
 	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
 	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
+	_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
+	_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
+	_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
 	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
 	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
 	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
@@ -812,7 +815,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
 			if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
 			{
 				OdometryInfo odomInfo;
-				odomInfo.covariance = cameraEvent->info().odomCovariance;
+				odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
 				if (!_processingOdometry && !_processingStatistics)
 				{
 					_processingOdometry = true; // if we receive too many odometry events!
@@ -927,11 +930,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		pose = _lastOdomPose;
 		lost = true;
 	}
-	else if(odom.info().inliers>0 &&
+	else if(odom.info().reg.inliers>0 &&
 			_preferencesDialog->getOdomQualityWarnThr() &&
-			odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
+			odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
 	{
-		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
 		lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
 		_cloudViewer->setBackgroundColor(Qt::darkYellow);
 		_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
@@ -1202,9 +1205,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				if(_preferencesDialog->isOdomOnlyInliersShown())
 				{
 					std::multimap<int, cv::KeyPoint> kpInliers;
-					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
 					{
-						kpInliers.insert(*odom.info().words.find(odom.info().wordInliers[i]));
+						kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
 					}
 					_ui->imageView_odometry->setFeatures(
 							kpInliers,
@@ -1271,13 +1274,13 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			{
 				if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
 				{
-					for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
 					{
-						_ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
+						_ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
 					}
-					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
 					{
-						_ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
+						_ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
 					}
 				}
 			}
@@ -1323,15 +1326,18 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	}
 
 	//Process info
-	_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());

guilib/src/OdometryViewer.cpp

@@ -179,7 +179,7 @@ void OdometryViewer::clear()
 void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 {
 	processingData_ = true;
-	int quality = odom.info().inliers;
+	int quality = odom.info().reg.inliers;
 
 	bool lost = false;
 	bool lostStateChanged = false;
@@ -193,11 +193,11 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 
 		lost = true;
 	}
-	else if(odom.info().inliers>0 &&
+	else if(odom.info().reg.inliers>0 &&
 			qualityWarningThr_ &&
-			odom.info().inliers < qualityWarningThr_)
+			odom.info().reg.inliers < qualityWarningThr_)
 	{
-		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_);
 		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
 		imageView_->setBackgroundColor(Qt::darkYellow);
 		cloudView_->setBackgroundColor(Qt::darkYellow);
@@ -425,13 +425,13 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 			{
 				if(imageView_->isFeaturesShown())
 				{
-					for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
 					{
-						imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
+						imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
 					}
-					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
 					{
-						imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
+						imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
 					}
 				}
 			}

guilib/src/PreferencesDialog.cpp

@@ -863,6 +863,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
 	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str());
 	_ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str());
+	_ui->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str());
 
 	_ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str());
 	_ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-531</y>
+            <y>-215</y>
-            <width>673</width>
+            <width>678</width>
-            <height>2749</height>
+            <height>2736</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>18</number>
+                <number>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -13497,6 +13497,61 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
+                      <item row="4" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>9999.989999999999782</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.100000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_125">
+                        <property name="text">
+                         <string>Uniform sampling voxel size. Set to 0 to disable.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>999999</number>
+                        </property>
+                        <property name="singleStep">
+                         <number>1</number>
+                        </property>
+                        <property name="value">
+                         <number>1</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="8" column="0">
                        <widget class="QCheckBox" name="loopClosure_icpPointToPlane">
                         <property name="text">
@@ -13598,35 +13653,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_125">
-                        <property name="text">
-                         <string>Uniform sampling voxel size. Set to 0 to disable.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="0">
-                       <widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
-                        <property name="minimum">
-                         <number>1</number>
-                        </property>
-                        <property name="maximum">
-                         <number>999999</number>
-                        </property>
-                        <property name="singleStep">
-                         <number>1</number>
-                        </property>
-                        <property name="value">
-                         <number>1</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="3" column="0">
                        <widget class="QDoubleSpinBox" name="loopClosure_icpVoxelSize">
                         <property name="suffix">
@@ -13723,25 +13749,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
-                       <widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.001000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>9999.989999999999782</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.010000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.100000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="8" column="1">
                        <widget class="QLabel" name="label_144">
                         <property name="text">
@@ -13803,8 +13810,27 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="0">
+                      <item row="11" column="1">
-                       <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
+                       <widget class="QLabel" name="label_429">
+                        <property name="text">
+                         <string>Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="11" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsMinComplexity">
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
                         <property name="singleStep">
                          <double>0.010000000000000</double>
                         </property>

tools/KittiDataset/main.cpp

@@ -400,8 +400,8 @@ int main(int argc, char * argv[])
 			if(odomInfo.interval>0.0)
 				speed = odomInfo.transform.x()/odomInfo.interval*3.6;
 			externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
-			externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
+			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
-			externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
+			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
 
 			bool processData = true;
 			if(iteration % mapUpdate != 0)
@@ -413,11 +413,11 @@ int main(int argc, char * argv[])
 			}
 			if(covariance.empty())
 			{
-				covariance = odomInfo.covariance;
+				covariance = odomInfo.reg.covariance;
 			}
 			else
 			{
-				covariance += odomInfo.covariance;
+				covariance += odomInfo.reg.covariance;
 			}
 
 			timer.restart();
@@ -431,7 +431,7 @@ int main(int argc, char * argv[])
 
 			++iteration;
 			printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
-					iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
 			if(processData && rtabmap.getLoopClosureId()>0)
 			{
 				printf(" *");

tools/RgbdDataset/main.cpp

@@ -226,8 +226,8 @@ int main(int argc, char * argv[])
 			Transform pose = odom.process(data, &odomInfo);
 			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
 			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
+			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
-			externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
+			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
 
 			bool processData = true;
 			if(detectionRate>0.0f &&
@@ -251,11 +251,11 @@ int main(int argc, char * argv[])
 			}
 			if(covariance.empty())
 			{
-				covariance = odomInfo.covariance;
+				covariance = odomInfo.reg.covariance;
 			}
 			else
 			{
-				covariance += odomInfo.covariance;
+				covariance += odomInfo.reg.covariance;
 			}
 
 			timer.restart();
@@ -269,7 +269,7 @@ int main(int argc, char * argv[])
 
 			++iteration;
 			printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
-					iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
 			if(processData && rtabmap.getLoopClosureId()>0)
 			{
 				printf(" *");