Lidar low complexity update. util3d::computeNormalsComplexity(): added optional transform (used in RegistrationIcp to get normal vectors in right coordinate frame). RegistrationIcp: Fixed PointToPoint ICP not used with PM when recomputing transform from low complexity. Added more info in warning messages when low complexity happens. OdometryF2M: avoid adding key frame when scan has low complexity. Same for the first frame to init the local scan map.

corelib/include/rtabmap/core/OdometryInfo.h

@@ -81,6 +81,7 @@ public:
 		output.transform = transform;
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
+		output.guessVelocity = guessVelocity;
 		output.distanceTravelled = distanceTravelled;
 		output.memoryUsage = memoryUsage;
 		output.type = type;

corelib/include/rtabmap/core/util3d_surface.h

@@ -339,20 +339,24 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 
 float RTABMAP_EXP computeNormalsComplexity(
 		const LaserScan & scan,
+		const Transform & t = Transform::getIdentity(),
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::Normal> & normals,
+		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
 		cv::Mat * pcaEigenValues = 0);

corelib/src/RegistrationIcp.cpp

@@ -600,15 +600,34 @@ Transform RegistrationIcp::computeTransformationImpl(
 				//special case if we have already normals computed and there is no filtering
 
 				cv::Mat complexityVectorsFrom, complexityVectorsTo;
-				double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
+				cv::Mat complexityValuesFrom, complexityValuesTo;
-				double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
+				double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom);
+				double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
 				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
 				info.icpStructuralComplexity = complexity;
 				if(complexity < _pointToPlaneMinComplexity)
 				{
 					tooLowComplexityForPlaneToPlane = true;
 					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, orientation is still optimized but translation will be limited to direction of normals.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
+
+					UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
+							(complexityVectors.rows == 3 && complexityVectors.cols == 3));
+					UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
+						  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
+						  "direction of normals (%s: %s).",
+						  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
+						  fromComplexity<toComplexity?"From":"To",
+						  complexityVectors.rows==2?
+								  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
+								  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
+
+					if(ULogger::level() == ULogger::kDebug)
+					{
+						std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
+						std::cout << "complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
+						std::cout << "complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
+						std::cout << "complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
+					}
 				}
 				else
 				{
@@ -773,15 +792,33 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 
 					cv::Mat complexityVectorsFrom, complexityVectorsTo;
-					double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.is2d(), &complexityVectorsFrom);
+					cv::Mat complexityValuesFrom, complexityValuesTo;
-					double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.is2d(), &complexityVectorsTo);
+					double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, Transform::getIdentity(), fromScan.is2d(), &complexityVectorsFrom, &complexityValuesFrom);
+					double toComplexity = util3d::computeNormalsComplexity(*normalsTo, Transform::getIdentity(), toScan.is2d(), &complexityVectorsTo, &complexityValuesTo);
 					float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
 					info.icpStructuralComplexity = complexity;
 					if(complexity < _pointToPlaneMinComplexity)
 					{
 						tooLowComplexityForPlaneToPlane = true;
 						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, orientation is still optimized but translation will be limited to direction of normals.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
+						UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
+								(complexityVectors.rows == 3 && complexityVectors.cols == 3));
+						UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
+							  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
+							  "direction of normals (%s: %s).",
+							  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
+							  fromComplexity<toComplexity?"From":"To",
+							  complexityVectors.rows==2?
+									  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
+									  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
+
+						if(ULogger::level() == ULogger::kDebug)
+						{
+							std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
+							std::cout << "complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
+							std::cout << "complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
+							std::cout << "complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
+						}
 					}
 					else
 					{
@@ -976,7 +1013,19 @@ Transform RegistrationIcp::computeTransformationImpl(
 							if(_pointToPlane)
 							{
 								// temporary set PointToPointErrorMinimizer
-								PM::ICP & icpTmp = icp;
+								PM::ICP icpTmp = icp;
+
+								PM::Parameters params;
+								params["maxDist"] = uNumber2Str(_voxelSize>0?_voxelSize:_maxCorrespondenceDistance/2.0f);
+								UWARN("libpointmatcher icp...temporary maxDist=%s (%s=%f, %s=%f)", params["maxDist"].c_str(),  Parameters::kIcpMaxCorrespondenceDistance().c_str(), _maxCorrespondenceDistance, Parameters::kIcpVoxelSize().c_str(), _voxelSize);
+								params["knn"] = uNumber2Str(_libpointmatcherKnn);
+								params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
+#if POINTMATCHER_VERSION_INT >= 10300
+								icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
+#else
+								icpTmp.matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
+#endif
+
 #if POINTMATCHER_VERSION_INT >= 10300
 								icpTmp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
 #else
@@ -1043,7 +1092,10 @@ Transform RegistrationIcp::computeTransformationImpl(
 								// 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;
+								Eigen::Vector3f vp = n*a;
+								UWARN("Normals low complexity: Limiting translation from (%f,%f) to (%f,%f)",
+										v[0], v[1], vp[0], vp[1]);
+								v= vp;
 							}
 							else if(complexityVectors.rows == 3)
 							{
@@ -1052,8 +1104,11 @@ Transform RegistrationIcp::computeTransformationImpl(
 								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;
+								Eigen::Vector3f vp = n1*a;
-								v += n2*b;
+								vp += n2*b;
+								UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
+										v[0], v[1], v[2], vp[0], vp[1], vp[2]);
+								v = vp;
 							}
 							else
 							{

corelib/src/odometry/OdometryF2M.cpp

@@ -573,7 +573,14 @@ Transform OdometryF2M::computeTransform(
 						  visKeyFrameThr_ == 0 ||
 						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
 						  regInfo.inliers <= visKeyFrameThr_);
-				bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
+				float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
+				bool p2n = Parameters::defaultIcpPointToPlane();
+				Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
+				Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
+				bool addGeometricKeyFrame =
+					regPipeline_->isScanRequired() &&
+					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_) &&
+					(addVisualKeyFrame || !p2n || regInfo.icpStructuralComplexity>=minComplexity);
 
 				addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
 				addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
@@ -1260,8 +1267,31 @@ Transform OdometryF2M::computeTransform(
 			{
 				if (lastFrame_->sensorData().laserScanRaw().size())
 				{
-					frameValid = true;
 					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
+
+					double complexity = 0.0;;
+					if(!frameValid)
+					{
+						float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
+						bool p2n = Parameters::defaultIcpPointToPlane();
+						Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
+						Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
+						if(p2n && minComplexity>0.0f)
+						{
+							complexity = util3d::computeNormalsComplexity(*mapCloudNormals);
+							if(complexity > minComplexity)
+							{
+								frameValid = true;
+							}
+						}
+						else
+						{
+							frameValid = true;
+						}
+					}
+
+					if(frameValid)
+					{
 						if (scanMapMaxRange_ > 0 ){
 							UINFO("Local map will be updated using range instead of time with range threshold set at %f", scanMapMaxRange_);
 						} else {
@@ -1293,6 +1323,11 @@ Transform OdometryF2M::computeTransform(
 						addKeyFrame = true;
 					}
 					else
+					{
+						UWARN("Scan complexity too low (%f) to init first keyframe.", complexity);
+					}
+				}
+				else
 				{
 					UWARN("Missing scan to initialize odometry.");
 				}

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -152,7 +152,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 		}
 
-		UDEBUG("fill poses to gtsam... rootId=%d", rootId);
+		UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d gpsPriorOnly=%d landmarksIgnored=%d)",
+				rootId, priorsIgnored()?1:0, gpsPriorOnly?1:0, landmarksIgnored()?1:0);
 		gtsam::Values initialEstimate;
 		std::map<int, bool> isLandmarkWithRotation;
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)

corelib/src/util3d_surface.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/Memory.h"
@@ -2902,6 +2903,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 
 float computeNormalsComplexity(
 		const LaserScan & scan,
+		const Transform & t,
 		cv::Mat * pcaEigenVectors,
 		cv::Mat * pcaEigenValues)
 {
@@ -2913,6 +2915,13 @@ float computeNormalsComplexity(
 		cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
 		int oi = 0;
 		int nOffset = scan.getNormalsOffset();
+		bool doTransform = false;
+		Transform tn;
+		if(!t.isIdentity() || !scan.localTransform().isIdentity())
+		{
+			tn = (t*scan.localTransform()).rotation();
+			doTransform = true;
+		}
 		for (int i = 0; i < scan.size(); ++i)
 		{
 			const float * ptrScan = scan.data().ptr<float>(0, i);
@@ -2921,19 +2930,29 @@ float computeNormalsComplexity(
 			{
 				if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]))
 				{
+					cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], 0);
+					if(doTransform)
+					{
+						n = util3d::transformPoint(n, tn);
+					}
 					float * ptr = data_normals.ptr<float>(oi++, 0);
-					ptr[0] = ptrScan[nOffset];
+					ptr[0] = n.x;
-					ptr[1] = ptrScan[nOffset+1];
+					ptr[1] = n.y;
 				}
 			}
 			else
 			{
 				if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]) && uIsFinite(ptrScan[nOffset+2]))
 				{
+					cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], ptrScan[nOffset+2]);
+					if(doTransform)
+					{
+						n = util3d::transformPoint(n, tn);
+					}
 					float * ptr = data_normals.ptr<float>(oi++, 0);
-					ptr[0] = ptrScan[nOffset];
+					ptr[0] = n.x;
-					ptr[1] = ptrScan[nOffset+1];
+					ptr[1] = n.y;
-					ptr[2] = ptrScan[nOffset+2];
+					ptr[2] = n.z;
 				}
 			}
 		}
@@ -2963,6 +2982,7 @@ float computeNormalsComplexity(
 
 float computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		const Transform & t,
 		bool is2d,
 		cv::Mat * pcaEigenVectors,
 		cv::Mat * pcaEigenValues)
@@ -2971,17 +2991,29 @@ float computeNormalsComplexity(
 	int sz = static_cast<int>(cloud.size()*2);
 	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
 	int oi = 0;
+	bool doTransform = false;
+	Transform tn;
+	if(!t.isIdentity())
+	{
+		tn = t.rotation();
+		doTransform = true;
+	}
 	for (unsigned int i = 0; i < cloud.size(); ++i)
 	{
 		const pcl::PointNormal & pt = cloud.at(i);
+		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
+		if(doTransform)
+		{
+			n = util3d::transformPoint(n, tn);
+		}
 		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[0] = n.x;
-			ptr[1] = pt.normal_y;
+			ptr[1] = n.y;
 			if(!is2d)
 			{
-				ptr[2] = pt.normal_z;
+				ptr[2] = n.z;
 			}
 		}
 	}
@@ -3006,6 +3038,7 @@ float computeNormalsComplexity(
 
 float computeNormalsComplexity(
 		const pcl::PointCloud<pcl::Normal> & normals,
+		const Transform & t,
 		bool is2d,
 		cv::Mat * pcaEigenVectors,
 		cv::Mat * pcaEigenValues)
@@ -3014,17 +3047,29 @@ float computeNormalsComplexity(
 	int sz = static_cast<int>(normals.size()*2);
 	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
 	int oi = 0;
+	bool doTransform = false;
+	Transform tn;
+	if(!t.isIdentity())
+	{
+		tn = t.rotation();
+		doTransform = true;
+	}
 	for (unsigned int i = 0; i < normals.size(); ++i)
 	{
 		const pcl::Normal & pt = normals.at(i);
+		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
+		if(doTransform)
+		{
+			n = util3d::transformPoint(n, tn);
+		}
 		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[0] = n.x;
-			ptr[1] = pt.normal_y;
+			ptr[1] = n.y;
 			if(!is2d)
 			{
-				ptr[2] = pt.normal_z;
+				ptr[2] = n.z;
 			}
 		}
 	}
@@ -3049,6 +3094,7 @@ float computeNormalsComplexity(
 
 float computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const Transform & t,
 		bool is2d,
 		cv::Mat * pcaEigenVectors,
 		cv::Mat * pcaEigenValues)
@@ -3057,17 +3103,29 @@ float computeNormalsComplexity(
 	int sz = static_cast<int>(cloud.size()*2);
 	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
 	int oi = 0;
+	bool doTransform = false;
+	Transform tn;
+	if(!t.isIdentity())
+	{
+		tn = t.rotation();
+		doTransform = true;
+	}
 	for (unsigned int i = 0; i < cloud.size(); ++i)
 	{
 		const pcl::PointXYZRGBNormal & pt = cloud.at(i);
+		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
+		if(doTransform)
+		{
+			n = util3d::transformPoint(n, tn);
+		}
 		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[0] = n.x;
-			ptr[1] = pt.normal_y;
+			ptr[1] = n.y;
 			if(!is2d)
 			{
-				ptr[2] = pt.normal_z;
+				ptr[2] = n.z;
 			}
 		}
 	}