Added Mem/LaserScanVoxelSize and OdomF2M/ScanSubtractAngle parameters. util3d::computeNormalsComplexity() now returns PCA's eigen vectors and values optionally. RegistrationIcp: detecting complexity of environment when PointToPLane is used, if too low, PointToPoint is done and movements are limited to main direction of the normals.

corelib/src/Memory.cpp

@@ -88,6 +88,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
+	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
 	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
@@ -440,6 +441,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
 	Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
+	Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
 	Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
 	Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
@@ -3760,6 +3762,35 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
 		UDEBUG("time downsampling scan = %fs", t);
 	}
+	if(!laserScan.empty() && _laserScanVoxelSize > 0.0f && !isIntermediateNode)
+	{
+		float pointsBeforeFiltering = laserScan.cols;
+		if(laserScan.channels() == 4 || laserScan.channels() == 7)
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
+			cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
+			laserScan = util3d::laserScanFromPointCloud(*cloud);
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+			cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
+			if(laserScan.channels() == 2 || laserScan.channels() == 5)
+			{
+				laserScan = util3d::laserScan2dFromPointCloud(*cloud);
+			}
+			else
+			{
+				laserScan = util3d::laserScanFromPointCloud(*cloud);
+			}
+		}
+		float ratio = float(laserScan.cols) / pointsBeforeFiltering;
+		maxLaserScanMaxPts = int(float(maxLaserScanMaxPts) * ratio);
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemScan_voxel_filtering(), t*1000.0f);
+		UDEBUG("time voxel filtering scan = %fs", t);
+	}
 	if(!laserScan.empty() &&
 		(_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) &&
 		laserScan.channels() > 1 && laserScan.channels() < 5 &&

corelib/src/OdometryF2M.cpp

@@ -65,6 +65,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
+	scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
 	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	map_(new Signature(-1)),
@@ -80,6 +81,10 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
+	if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_))
+	{
+		scanSubtractAngle_ *= M_PI/180.0f;
+	}
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
 	UASSERT(bundleMaxFrames_ >= 0);
@@ -605,7 +610,7 @@ Transform OdometryF2M::computeTransform(
 									mapCloudNormals,
 									pcl::IndicesPtr(new std::vector<int>),
 									scanSubtractRadius_,
-									0.0f);
+									scanSubtractAngle_);
 							newPoints = frameCloudNormalsIndices->size();
 						}
 						else

corelib/src/RegistrationIcp.cpp

@@ -433,6 +433,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 			double variance = 1.0;
 			bool transformComputed = false;
 			bool tooLowComplexityForPlaneToPlane = false;
+			cv::Mat complexityVectors;
 
 			if( _pointToPlane &&
 				_voxelSize == 0.0f &&
@@ -442,14 +443,16 @@ Transform RegistrationIcp::computeTransformationImpl(
 			{
 				//special case if we have already normals computed and there is no filtering
 
-				double fromComplexity = util3d::computeNormalsComplexity(fromScan);
-				double toComplexity = util3d::computeNormalsComplexity(toScan);
+				cv::Mat complexityVectorsFrom, complexityVectorsTo;
+				double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
+				double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
 				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
 				info.icpStructuralComplexity = complexity;
 				if(complexity < _pointToPlaneMinComplexity)
 				{
 					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());
+					complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
+					UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
 				}
 				else
 				{
@@ -610,14 +613,16 @@ Transform RegistrationIcp::computeTransformationImpl(
 						normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
 					}
 
-					double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5);
-					double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5);
+					cv::Mat complexityVectorsFrom, complexityVectorsTo;
+					double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5, &complexityVectorsFrom);
+					double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5, &complexityVectorsTo);
 					float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
 					info.icpStructuralComplexity = complexity;
 					if(complexity < _pointToPlaneMinComplexity)
 					{
 						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());
+						complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
+						UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
 					}
 					else
 					{
@@ -721,7 +726,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 						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());
 					}
 
-					if(_voxelSize > 0.0f)
+					if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane)
 					{
 						// update output scans
 						if(fromScan.channels() == 2 || fromScan.channels() == 5)
@@ -743,7 +748,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 
 #ifdef RTABMAP_POINTMATCHER
-					if(_libpointmatcher && !_pointToPlane) // don't use libpointmatcher if it is configured for point to plane
+					if(_libpointmatcher)
 					{
 						// Load point clouds
 						DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);
@@ -756,7 +761,30 @@ Transform RegistrationIcp::computeTransformationImpl(
 							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);
+							if(_pointToPlane)
+							{
+								// temporary set PointToPointErrorMinimizer
+								PM::ICP & icpTmp = icp;
+								icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
+
+								for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();)
+								{
+									if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0)
+									{
+										iter = icpTmp.outlierFilters.erase(iter);
+									}
+									else
+									{
+										++iter;
+									}
+								}
+
+								T = icpTmp(data, ref);
+							}
+							else
+							{
+								T = icp(data, ref);
+							}
 							UDEBUG("libpointmatcher icp...done!");
 							icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
 
@@ -790,6 +818,39 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 					if(!icpT.isNull() && hasConverged)
 					{
+						if(tooLowComplexityForPlaneToPlane)
+						{
+							Transform guessInv = guess.inverse();
+							Transform t = guessInv * icpT.inverse() * guess;
+							Eigen::Vector3f v(t.x(), t.y(), t.z());
+							if(complexityVectors.cols == 2)
+							{
+								// limit translation in direction of the first eigen vector
+								Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
+								float a = v.dot(n);
+								v = n*a;
+							}
+							else if(complexityVectors.rows == 3)
+							{
+								// limit translation in direction of the first and second eigen vectors
+								Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
+								Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
+								float a = v.dot(n1);
+								float b = v.dot(n2);
+								v = n1*a;
+								v += n2*b;
+							}
+							else
+							{
+								UWARN("not supposed to be here!");
+								v = Eigen::Vector3f(0,0,0);
+							}
+							float roll, pitch, yaw;
+							t.getEulerAngles(roll, pitch, yaw);
+							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
+							icpT = guess * t.inverse() * guessInv;
+						}
+
 						util3d::computeVarianceAndCorrespondences(
 								fromCloudRegistered,
 								toCloudFiltered,

corelib/src/util3d.cpp

@@ -1531,6 +1531,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
@@ -1550,6 +1551,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
 
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
@@ -1568,6 +1570,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
@@ -1587,6 +1590,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
 
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	for(int i=0; i<laserScan.cols; ++i)
 	{

corelib/src/util3d_surface.cpp

@@ -2338,7 +2338,10 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 	return normals;
 }
 
-float computeNormalsComplexity(const cv::Mat & scan)
+float computeNormalsComplexity(
+		const cv::Mat & scan,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
 {
 	if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7))
 	{
@@ -2384,6 +2387,15 @@ float computeNormalsComplexity(const cv::Mat & scan)
 		{
 			cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
 
+			if(pcaEigenVectors)
+			{
+				*pcaEigenVectors = pca_analysis.eigenvectors;
+			}
+			if(pcaEigenValues)
+			{
+				*pcaEigenValues = pca_analysis.eigenvalues;
+			}
+
 			// 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);
 		}
@@ -2395,7 +2407,11 @@ float computeNormalsComplexity(const cv::Mat & scan)
 	return 0.0f;
 }
 
-float computeNormalsComplexity(const pcl::PointCloud<pcl::PointNormal> & cloud, bool is2d)
+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
 {
 	 //Construct a buffer used by the pca analysis
 	int sz = static_cast<int>(cloud.size()*2);
@@ -2419,13 +2435,26 @@ float computeNormalsComplexity(const pcl::PointCloud<pcl::PointNormal> & cloud,
 	{
 		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
 
+		if(pcaEigenVectors)
+		{
+			*pcaEigenVectors = pca_analysis.eigenvectors;
+		}
+		if(pcaEigenValues)
+		{
+			*pcaEigenValues = pca_analysis.eigenvalues;
+		}
+
 		// 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)
+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::Normal> & normals,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
 {
 	 //Construct a buffer used by the pca analysis
 	int sz = static_cast<int>(normals.size()*2);
@@ -2449,13 +2478,26 @@ float computeNormalsComplexity(const pcl::PointCloud<pcl::Normal> & normals, boo
 	{
 		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
 
+		if(pcaEigenVectors)
+		{
+			*pcaEigenVectors = pca_analysis.eigenvectors;
+		}
+		if(pcaEigenValues)
+		{
+			*pcaEigenValues = pca_analysis.eigenvalues;
+		}
+
 		// 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)
+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
 {
 	 //Construct a buffer used by the pca analysis
 	int sz = static_cast<int>(cloud.size()*2);
@@ -2479,6 +2521,15 @@ float computeNormalsComplexity(const pcl::PointCloud<pcl::PointXYZRGBNormal> & c
 	{
 		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
 
+		if(pcaEigenVectors)
+		{
+			*pcaEigenVectors = pca_analysis.eigenvectors;
+		}
+		if(pcaEigenValues)
+		{
+			*pcaEigenValues = pca_analysis.eigenvalues;
+		}
+
 		// 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);
 	}