Added RegistrationIcp tests

corelib/include/rtabmap/core/Parameters.h

@@ -832,7 +832,8 @@ class RTABMAP_CORE_EXPORT Parameters
     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, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
     RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
-    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneComplexityCentered, bool, false, uFormat("If false (default), the complexity metric uses the uncentered second-moment matrix (1/N) * sum(n_i * n_i^T), whose smallest eigenvalue directly measures how well the surface normals span R^N. If true, uses centered PCA (cv::PCA covariance) for backwards compatibility -- but the centered metric is known to mis-classify perpendicular-surface scenes as degenerate when normals are consistently viewpoint-flipped (only N distinct directions in N-D collapse to rank N-1 after centering). For true degeneracies (parallel surfaces, e.g. corridors) the two metrics agree because the normal mean is zero. The %s threshold of 0.02 works under either setting.", kIcpPointToPlaneMinComplexity().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to keep the PointToPlane transform and limit its correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to recompute the transform with PointToPoint and accept it \"as is\", set to 3 for the legacy behavior of recomputing with PointToPoint then applying the same constraint as strategy 1.", kIcpPointToPlaneMinComplexity().c_str()));
     RTABMAP_PARAM(Icp, OutlierRatio,                float, 0.85,   uFormat("Outlier ratio. For libpointmatcher (%s=1), sets TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib (%s=2), sets \"finalOverlapRatio\". For PCL (%s=0), if 0<value<1, installs a RANSAC correspondence rejector with inlier threshold = value * %s. The value should be between 0 and 1.", kIcpStrategy().c_str(), kIcpStrategy().c_str(), kIcpStrategy().c_str(), kIcpMaxCorrespondenceDistance().c_str()));
     RTABMAP_PARAM_STR(Icp, DebugExportFormat,       "",           "Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are \"pcd\", \"ply\" or \"vtk\". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten.");
 

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -39,8 +39,33 @@ namespace rtabmap {
 class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration
 {
 public:
+	enum IcpStrategy {
+		kIcpUndef = -1,        /**< Undefined / invalid type. */
+		kIcpPCL = 0,           /**< Point Cloud Library. */
+		kIcpPointMatcher = 1,  /**< libpointmatcher. */
+		kIcpCCCoreLib = 2,     /**< CCCoreLib (Cloud Compare). */
+		kIcpEnd = 3            /**< Sentinel: always keep last. Used to iterate through strategies. */
+	};
+
+public:
+	/**
+	 * @brief Returns true if @p strategy is built into this rtabmap binary.
+	 *
+	 * @ref kPCL is always available; @ref kPointMatcher requires the
+	 * RTABMAP_POINTMATCHER build flag; @ref kCCCoreLib requires
+	 * RTABMAP_CCCORELIB.
+	 */
+	static bool available(IcpStrategy strategy);
+
+	/**
+	 * @brief Human-readable name for @p strategy ("PCL", "libpointmatcher",
+	 * "CCCoreLib", or "Unknown").
+	 */
+	static const char * strategyName(IcpStrategy strategy);
+
 	// take ownership of child
 	RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
+	RegistrationIcp(IcpStrategy strategy, const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 	virtual ~RegistrationIcp();
 
 	virtual void parseParameters(const ParametersMap & parameters);
@@ -56,7 +81,7 @@ protected:
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
 
 private:
-	int _strategy;
+	IcpStrategy _strategy;
 	float _maxTranslation;
 	float _maxRotation;
 	float _voxelSize;
@@ -75,6 +100,7 @@ private:
 	float _pointToPlaneRadius;
 	float _pointToPlaneGroundNormalsUp;
 	float _pointToPlaneMinComplexity;
+	bool _pointToPlaneComplexityCentered;
 	int _pointToPlaneLowComplexityStrategy;
 	std::string _libpointmatcherConfig;
 	int _libpointmatcherKnn;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -61,7 +61,8 @@ public:
 		icpStructuralComplexity(0.0f),
 		icpStructuralDistribution(0.0f),
 		icpCorrespondences(0),
-		icpRMS(0)
+		icpRMS(0),
+		icpIterations(-1)
 
 	{
 	}
@@ -87,6 +88,7 @@ public:
 		output.icpStructuralDistribution = icpStructuralDistribution;
 		output.icpCorrespondences = icpCorrespondences;
         output.icpRMS = icpRMS;
+		output.icpIterations = icpIterations;
 		return output;
 	}
 
@@ -115,6 +117,7 @@ public:
 	float icpStructuralDistribution; /**< Distribution of structural features in the overlap. */
 	int   icpCorrespondences;     /**< Number of ICP point correspondences. */
 	float icpRMS;                 /**< Root-mean-square error of ICP correspondences (m). */
+	int   icpIterations;          /**< Number of ICP iterations actually performed (-1 if unavailable from backend). */
 };
 
 }

corelib/include/rtabmap/core/util3d_registration.h

@@ -215,7 +215,8 @@ Transform RTABMAP_CORE_EXPORT icp(
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false,
-		float ransacOutlierRatio = 0.0f);
+		float ransacOutlierRatio = 0.0f,
+		int * iterationsDone = nullptr);
 /**
  * @brief Performs Iterative Closest Point (ICP) alignment between two point clouds and returns the resulting transform.
  * @see util3d::icp()
@@ -233,7 +234,8 @@ Transform RTABMAP_CORE_EXPORT icp(
 		pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false,
-		float ransacOutlierRatio = 0.0f);
+		float ransacOutlierRatio = 0.0f,
+		int * iterationsDone = nullptr);
 
 /**
  * @brief Performs Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
@@ -269,7 +271,8 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false,
-		float ransacOutlierRatio = 0.0f);
+		float ransacOutlierRatio = 0.0f,
+		int * iterationsDone = nullptr);
 /**
  * @briefPerforms Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
  * @see util3d::icpPointToPlane()
@@ -287,7 +290,8 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
 		float epsilon = 0.0f,
 		bool icp2D = false,
-		float ransacOutlierRatio = 0.0f);
+		float ransacOutlierRatio = 0.0f,
+		int * iterationsDone = nullptr);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/include/rtabmap/core/util3d_surface.h

@@ -427,17 +427,25 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const LaserScan & scan,
 		const Transform & t = Transform::getIdentity(),
 		cv::Mat * pcaEigenVectors = 0,
-		cv::Mat * pcaEigenValues = 0);
+		cv::Mat * pcaEigenValues = 0,
+		bool centered = true);
 /**
  * @ingroup ComputeNormalsComplexity
  * @brief Computes the complexity of surface normals in a point cloud of type `pcl::Normal`.
+ *
+ * @param centered When true (default), use covariance PCA (centered at mean).
+ * Use false for the uncentered second-moment matrix `M = (1/N) sum(n_i * n_i^T)`,
+ * which measures span of normal directions and correctly identifies degeneracy
+ * even when there are only N (rather than N+1) distinct viewpoint-flipped
+ * normal directions in N-D space.
  */
 float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const pcl::PointCloud<pcl::Normal> & normals,
 		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
-		cv::Mat * pcaEigenValues = 0);
+		cv::Mat * pcaEigenValues = 0,
+		bool centered = true);
 /**
  * @ingroup ComputeNormalsComplexity
  * @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointNormal`.
@@ -447,7 +455,8 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
-		cv::Mat * pcaEigenValues = 0);
+		cv::Mat * pcaEigenValues = 0,
+		bool centered = true);
 /**
  * @ingroup ComputeNormalsComplexity
  * @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointXYZINormal`.
@@ -457,7 +466,8 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
-		cv::Mat * pcaEigenValues = 0);
+		cv::Mat * pcaEigenValues = 0,
+		bool centered = true);
 /**
  * @ingroup ComputeNormalsComplexity
  * @brief Computes the complexity of surface normals in a point cloud of type `pcl::PointXYZRGBNormal`.
@@ -467,7 +477,8 @@ float RTABMAP_CORE_EXPORT computeNormalsComplexity(
 		const Transform & t = Transform::getIdentity(),
 		bool is2d = false,
 		cv::Mat * pcaEigenVectors = 0,
-		cv::Mat * pcaEigenValues = 0);
+		cv::Mat * pcaEigenValues = 0,
+		bool centered = true);
 
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,

corelib/src/RegistrationIcp.cpp

@@ -54,9 +54,103 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+namespace {
+
+// Strategy-1 projection used by both PointToPlane and PointToPoint branches
+// in computeTransformationImpl(): when the structural complexity is below
+// Icp/PointToPlaneMinComplexity (corridor-like environment), project the ICP
+// translation correction onto the constrained eigenvectors of the normals
+// (the first one always; the second only if its eigenvalue is itself above
+// the complexity threshold).  The unconstrained DoFs are then sourced from
+// the guess.  Returns the constrained transform.
+Transform constrainLowComplexityTransform(
+		const Transform & icpT,
+		const Transform & guess,
+		const cv::Mat & complexityVectors,
+		float secondEigenValue,
+		float pointToPlaneMinComplexity,
+		float complexity)
+{
+	const Transform guessInv = guess.inverse();
+	Transform t = guessInv * icpT.inverse() * guess;
+	Eigen::Vector3f v(t.x(), t.y(), t.z());
+	if(complexityVectors.cols == 2)
+	{
+		// 2D: project onto the first (and only constrained) eigenvector.
+		Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
+		Eigen::Vector3f vp = n * v.dot(n);
+		UWARN("Normals low complexity (%f): Limiting translation from (%f,%f) to (%f,%f)",
+				complexity, v[0], v[1], vp[0], vp[1]);
+		v = vp;
+	}
+	else if(complexityVectors.rows == 3)
+	{
+		// 3D: project onto first and second eigenvectors. Second is only
+		// kept when its eigenvalue is itself above the complexity threshold.
+		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));
+		Eigen::Vector3f vp = n1 * v.dot(n1);
+		if(secondEigenValue >= pointToPlaneMinComplexity)
+		{
+			vp += n2 * v.dot(n2);
+		}
+		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
+	{
+		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);
+	return guess * t.inverse() * guessInv;
+}
+
+}  // namespace
+
+bool RegistrationIcp::available(IcpStrategy strategy)
+{
+	switch(strategy)
+	{
+	case kIcpPCL:           return true;
+	case kIcpPointMatcher:
+#ifdef RTABMAP_POINTMATCHER
+		return true;
+#else
+		return false;
+#endif
+	case kIcpCCCoreLib:
+#ifdef RTABMAP_CCCORELIB
+		return true;
+#else
+		return false;
+#endif
+	default:             return false;
+	}
+}
+
+const char * RegistrationIcp::strategyName(IcpStrategy strategy)
+{
+	switch(strategy)
+	{
+	case kIcpPCL:          return "PCL";
+	case kIcpPointMatcher: return "libpointmatcher";
+	case kIcpCCCoreLib:    return "CCCoreLib";
+	default:               return "Unknown";
+	}
+}
+
 RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
+	RegistrationIcp(static_cast<IcpStrategy>(Parameters::defaultIcpStrategy()), parameters, child)
+{
+}
+
+RegistrationIcp::RegistrationIcp(IcpStrategy strategy, const ParametersMap & parameters, Registration * child) :
 	Registration(parameters, child),
-	_strategy(Parameters::defaultIcpStrategy()),
+	_strategy(strategy),
 	_maxTranslation(Parameters::defaultIcpMaxTranslation()),
 	_maxRotation(Parameters::defaultIcpMaxRotation()),
 	_voxelSize(Parameters::defaultIcpVoxelSize()),
@@ -75,6 +169,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
 	_pointToPlaneGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
+	_pointToPlaneComplexityCentered(Parameters::defaultIcpPointToPlaneComplexityCentered()),
 	_pointToPlaneLowComplexityStrategy(Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
@@ -103,7 +198,11 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 {
 	Registration::parseParameters(parameters);
 
-	Parameters::parse(parameters, Parameters::kIcpStrategy(), _strategy);
+	{
+		int strategyAsInt = static_cast<int>(_strategy);
+		Parameters::parse(parameters, Parameters::kIcpStrategy(), strategyAsInt);
+		_strategy = static_cast<IcpStrategy>(strategyAsInt);
+	}
 	Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
 	Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
 	Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
@@ -123,6 +222,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), _pointToPlaneGroundNormalsUp);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneComplexityCentered(), _pointToPlaneComplexityCentered);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneLowComplexityStrategy(), _pointToPlaneLowComplexityStrategy);
 	UASSERT(_pointToPlaneGroundNormalsUp >= 0.0f && _pointToPlaneGroundNormalsUp <= 1.0f);
 	UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
@@ -342,7 +442,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Enabled filters: from=%s to=%s", _filtersEnabled&1?"true":"false", _filtersEnabled&2?"true":"false");
 	UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
 	UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);
-	UDEBUG("Strategy=%d", _strategy);
+	UDEBUG("IcpStrategy=%d", _strategy);
 
 	UTimer timer;
 	std::string msg;
@@ -525,15 +625,26 @@ Transform RegistrationIcp::computeTransformationImpl(
 			{
 				cv::Mat complexityVectorsFrom, complexityVectorsTo;
 				cv::Mat complexityValuesFrom, complexityValuesTo;
-				double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom);
-				double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
+				double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom, _pointToPlaneComplexityCentered);
+				double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo, _pointToPlaneComplexityCentered);
 				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
 				info.icpStructuralComplexity = complexity;
 				UDEBUG("structural complexity: from=%f to=%f", fromComplexity, toComplexity);
 				if(complexity < _pointToPlaneMinComplexity)
 				{
 					tooLowComplexityForPlaneToPlane = true;
-					pointToPlane = false;
+					// Strategy 1 (project) keeps PointToPlane on: it gives
+					// cleaner residuals on the constrained perpendicular
+					// axes than PointToPoint does (point-to-plane cancels
+					// the random in-plane jitter via the normal dot
+					// product), and the in-plane DoFs are then pinned to
+					// the guess by the projection block in the
+					// PointToPlane branch. Strategy 0 (reject) and 2
+					// (accept PointToPoint as-is) keep the old behavior.
+					if(_pointToPlaneLowComplexityStrategy != 1)
+					{
+						pointToPlane = false;
+					}
 					if(complexity > 0.0f)
 					{
 						complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
@@ -616,6 +727,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 						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);
+						// CounterTransformationChecker (added first by parseParameters()) tracks
+						// the actual iteration count in its first condition variable.
+						if(!icp.transformationCheckers.empty())
+						{
+							const auto & vars = icp.transformationCheckers[0]->getConditionVariables();
+							if(vars.size() > 0) info.icpIterations = static_cast<int>(vars(0));
+						}
 						icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
 						UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
 
@@ -645,11 +763,23 @@ Transform RegistrationIcp::computeTransformationImpl(
 						   *fromCloudNormalsRegistered,
 						   _epsilon,
 						   this->force3DoF(),
-						   _outlierRatio);
+						   _outlierRatio,
+						   &info.icpIterations);
 				}
 
 				if(!icpT.isNull() && hasConverged)
 				{
+					// Strategy 1 with low complexity: project the
+					// PointToPlane correction onto the constrained
+					// eigenvectors (in-plane DoFs come from the guess).
+					if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy == 1)
+					{
+						icpT = constrainLowComplexityTransform(icpT, guess,
+								complexityVectors, secondEigenValue,
+								_pointToPlaneMinComplexity, info.icpStructuralComplexity);
+						fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+					}
+
 					util3d::computeVarianceAndCorrespondences(
 							fromCloudNormalsRegistered,
 							toCloudNormals,
@@ -726,10 +856,20 @@ Transform RegistrationIcp::computeTransformationImpl(
 							}
 
 							T = icpTmp(data, ref);
+							if(!icpTmp.transformationCheckers.empty())
+							{
+								const auto & vars = icpTmp.transformationCheckers[0]->getConditionVariables();
+								if(vars.size() > 0) info.icpIterations = static_cast<int>(vars(0));
+							}
 						}
 						else
 						{
 							T = icp(data, ref);
+							if(!icp.transformationCheckers.empty())
+							{
+								const auto & vars = icp.transformationCheckers[0]->getConditionVariables();
+								if(vars.size() > 0) info.icpIterations = static_cast<int>(vars(0));
+							}
 						}
 						UDEBUG("libpointmatcher icp...done!");
 						icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
@@ -780,68 +920,31 @@ Transform RegistrationIcp::computeTransformationImpl(
 							   *fromCloudRegistered,
 							   _epsilon,
 							   this->force3DoF(), // icp2D
-							   _outlierRatio);
+							   _outlierRatio,
+							   &info.icpIterations);
 					}
 				}
 
 				if(!icpT.isNull() && hasConverged)
 				{
-					if(tooLowComplexityForPlaneToPlane)
+					// PointToPoint branch only reached for strategy 2
+					// ("accept as-is") or 3 (legacy: recompute then project).
+					// Strategy 0 returned early; strategy 1 stays on the
+					// PointToPlane branch.
+					if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy == 3)
 					{
-						if(_pointToPlaneLowComplexityStrategy == 1)
-						{
-							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);
-								Eigen::Vector3f vp = n*a;
-								UWARN("Normals low complexity (%f): Limiting translation from (%f,%f) to (%f,%f)",
-										info.icpStructuralComplexity,
-										v[0], v[1], vp[0], vp[1]);
-								v= vp;
-							}
-							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);
-								Eigen::Vector3f vp = n1*a;
-								if(secondEigenValue >= _pointToPlaneMinComplexity)
-								{
-									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
-							{
-								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;
-
-							fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
-						}
-						else
-						{
-							UWARN("Even if complexity is low (%f), PointToPoint transformation is accepted \"as is\" (%s=2)",
-									info.icpStructuralComplexity,
-									Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
-							if(fromCloudRegistered->empty())
-								fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
-						}
+						icpT = constrainLowComplexityTransform(icpT, guess,
+								complexityVectors, secondEigenValue,
+								_pointToPlaneMinComplexity, info.icpStructuralComplexity);
+						fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
 					}
-					else if(fromCloudRegistered->empty())
+					else if(tooLowComplexityForPlaneToPlane)
+					{
+						UWARN("Even if complexity is low (%f), PointToPoint transformation is accepted \"as is\" (%s=2)",
+								info.icpStructuralComplexity,
+								Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
+					}
+					if(fromCloudRegistered->empty())
 						fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
 
 					util3d::computeVarianceAndCorrespondences(

corelib/src/icp/cccorelib.h

@@ -125,7 +125,16 @@ rtabmap::Transform icpCC(
 		*finalRMS = (float)finalError;
 	}
 
-	if(result != 1)
+	if(result == CCCoreLib::ICPRegistrationTools::ICP_NOTHING_TO_DO)
+	{
+		// CCCoreLib's "nothing to do" outcome means the clouds were already
+		// aligned (no transform needed) -- semantically identity, not a
+		// failure. Return identity instead of null so callers don't have to
+		// treat trivial alignment as a rejection.
+		UDEBUG("CCCoreLib: ICP_NOTHING_TO_DO -- returning identity");
+		return rtabmap::Transform::getIdentity();
+	}
+	if(result != CCCoreLib::ICPRegistrationTools::ICP_APPLY_TRANSFO)
 	{
 		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as result %d !=1", result);
 		UDEBUG(msg.c_str());

corelib/src/util3d_registration.cpp

@@ -438,7 +438,8 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
 			  pcl::PointCloud<PointT> & cloud_source_registered,
 			  float epsilon,
 			  bool icp2D,
-			  float ransacOutlierRatio)
+			  float ransacOutlierRatio,
+			  int * iterationsDone)
 {
 	pcl::IterativeClosestPoint<PointT, PointT> icp;
 	// Set the input source and target
@@ -470,6 +471,7 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
 	// Perform the alignment
 	icp.align (cloud_source_registered);
 	hasConverged = icp.hasConverged();
+	if(iterationsDone) *iterationsDone = icp.nr_iterations_;
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
@@ -482,9 +484,10 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 			  float epsilon,
 			  bool icp2D,
-			  float ransacOutlierRatio)
+			  float ransacOutlierRatio,
+			  int * iterationsDone)
 {
-	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio);
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 
 // return transform from source to target (All points must be finite!!!)
@@ -496,9 +499,10 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
 			  pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
 			  float epsilon,
 			  bool icp2D,
-			  float ransacOutlierRatio)
+			  float ransacOutlierRatio,
+			  int * iterationsDone)
 {
-	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio);
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 
 // return transform from source to target (All points/normals must be finite!!!)
@@ -512,7 +516,8 @@ Transform icpPointToPlaneImpl(
 		pcl::PointCloud<PointNormalT> & cloud_source_registered,
 		float epsilon,
 		bool icp2D,
-		float ransacOutlierRatio)
+		float ransacOutlierRatio,
+		int * iterationsDone)
 {
 	pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
 	// Set the input source and target
@@ -541,11 +546,17 @@ Transform icpPointToPlaneImpl(
 	// Perform the alignment
 	icp.align (cloud_source_registered);
 	hasConverged = icp.hasConverged();
+	if(iterationsDone) *iterationsDone = icp.nr_iterations_;
 	Transform t = Transform::fromEigen4f(icp.getFinalTransformation());
 
 	if(icp2D)
 	{
-		// FIXME probably an estimation approach already 2D like in icp() version above exists.
+		// PCL has no 2D-aware PointToPlane estimator (only the
+		// point-to-point pcl::registration::TransformationEstimation2D),
+		// so we run the full 6DoF PointToPlaneLLS above and then snap
+		// the result to 3DoF here.  The 6DoF LLS on planar (z=0) input
+		// is numerically fragile -- callers that need 2D PointToPlane
+		// reliably should use libpointmatcher (Icp/Strategy=1).
 		t = t.to3DoF();
 		pcl::transformPointCloudWithNormals(*cloud_source, cloud_source_registered, t.toEigen4f());
 	}
@@ -563,9 +574,10 @@ Transform icpPointToPlane(
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon,
 		bool icp2D,
-		float ransacOutlierRatio)
+		float ransacOutlierRatio,
+		int * iterationsDone)
 {
-	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio);
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 // return transform from source to target (All points/normals must be finite!!!)
 Transform icpPointToPlane(
@@ -577,9 +589,10 @@ Transform icpPointToPlane(
 		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
 		float epsilon,
 		bool icp2D,
-		float ransacOutlierRatio)
+		float ransacOutlierRatio,
+		int * iterationsDone)
 {
-	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio);
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
 }
 
 }

corelib/src/util3d_surface.cpp

@@ -107,6 +107,44 @@ float pcaEigenvalueAt(const cv::Mat & eigenvalues, int index)
 	index = std::min(index, eigenvalues.rows - 1);
 	return eigenvalues.at<float>(index, 0);
 }
+
+// Shared eigendecomposition of a normals matrix (each row is a normal).
+// centered=true  -> covariance (cv::PCA): measures spread *around the mean*.
+//                  Misses degeneracy when ≤ dim distinct viewpoint-flipped
+//                  normal directions sit on a single (dim-1)-D affine
+//                  subspace (e.g. 3 perpendicular surfaces with normals
+//                  +x/+y/+z all sum to mean (1/3,1/3,1/3) -> rank-2).
+// centered=false -> uncentered second-moment matrix (1/N) * Σ nᵢ nᵢᵀ:
+//                  smallest eigenvalue is 0 iff normals are confined to a
+//                  (dim-1)-D linear subspace, which is the actual ICP
+//                  observability question.  Recommended for the
+//                  PointToPlane low-complexity check.
+// Returns smallest eigenvalue * dim (so values scale ~[0,1]).
+float normalsComplexity(
+		const cv::Mat & normals,
+		bool centered,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	const int dim = normals.cols;
+	cv::Mat eigVals, eigVecs;
+	if(centered)
+	{
+		cv::PCA pca(normals, cv::Mat(), CV_PCA_DATA_AS_ROW);
+		eigVals = pca.eigenvalues;
+		eigVecs = pca.eigenvectors;
+	}
+	else
+	{
+		cv::Mat M;
+		cv::mulTransposed(normals, M, /*aTa=*/true);
+		M /= static_cast<float>(normals.rows);
+		cv::eigen(M, eigVals, eigVecs);
+	}
+	if(pcaEigenVectors) *pcaEigenVectors = eigVecs;
+	if(pcaEigenValues)  *pcaEigenValues  = eigVals;
+	return pcaEigenvalueAt(eigVals, dim - 1) * static_cast<float>(dim);
+}
 } // namespace
 
 void createPolygonIndexes(
@@ -3183,7 +3221,8 @@ float computeNormalsComplexity(
 		const LaserScan & scan,
 		const Transform & t,
 		cv::Mat * pcaEigenVectors,
-		cv::Mat * pcaEigenValues)
+		cv::Mat * pcaEigenValues,
+		bool centered)
 {
 	if(!scan.isEmpty() && (scan.hasNormals()))
 	{
@@ -3236,19 +3275,9 @@ float computeNormalsComplexity(
 		}
 		if(oi>1)
 		{
-			cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi)), cv::Mat(), CV_PCA_DATA_AS_ROW);
-
-			if(pcaEigenVectors)
-			{
-				*pcaEigenVectors = pca_analysis.eigenvectors;
-			}
-			if(pcaEigenValues)
-			{
-				*pcaEigenValues = pca_analysis.eigenvalues;
-			}
-			UASSERT((is2d && pca_analysis.eigenvalues.total()>=2) || (!is2d && pca_analysis.eigenvalues.total()>=3));
-			// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
-			return pcaEigenvalueAt(pca_analysis.eigenvalues, is2d?1:2)*(is2d?2.0f:3.0f);
+			return normalsComplexity(
+					cv::Mat(data_normals, cv::Range(0, oi)),
+					centered, pcaEigenVectors, pcaEigenValues);
 		}
 	}
 	else if(!scan.isEmpty())
@@ -3263,7 +3292,8 @@ float computeNormalsComplexity(
 		const Transform & t,
 		bool is2d,
 		cv::Mat * pcaEigenVectors,
-		cv::Mat * pcaEigenValues)
+		cv::Mat * pcaEigenValues,
+		bool centered)
 {
 	 //Construct a buffer used by the pca analysis
 	int sz = static_cast<int>(cloud.size()*2);
@@ -3297,19 +3327,9 @@ float computeNormalsComplexity(
 	}
 	if(oi>1)
 	{
-		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi)), 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 pcaEigenvalueAt(pca_analysis.eigenvalues, is2d?1:2)*(is2d?2.0f:3.0f);
+		return normalsComplexity(
+				cv::Mat(data_normals, cv::Range(0, oi)),
+				centered, pcaEigenVectors, pcaEigenValues);
 	}
 	return 0.0f;
 }
@@ -3319,7 +3339,8 @@ float computeNormalsComplexity(
 		const Transform & t,
 		bool is2d,
 		cv::Mat * pcaEigenVectors,
-		cv::Mat * pcaEigenValues)
+		cv::Mat * pcaEigenValues,
+		bool centered)
 {
 	 //Construct a buffer used by the pca analysis
 	int sz = static_cast<int>(normals.size()*2);
@@ -3353,19 +3374,9 @@ float computeNormalsComplexity(
 	}
 	if(oi>1)
 	{
-		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi)), 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 pcaEigenvalueAt(pca_analysis.eigenvalues, is2d?1:2)*(is2d?2.0f:3.0f);
+		return normalsComplexity(
+				cv::Mat(data_normals, cv::Range(0, oi)),
+				centered, pcaEigenVectors, pcaEigenValues);
 	}
 	return 0.0f;
 }
@@ -3375,7 +3386,8 @@ float computeNormalsComplexity(
 		const Transform & t,
 		bool is2d,
 		cv::Mat * pcaEigenVectors,
-		cv::Mat * pcaEigenValues)
+		cv::Mat * pcaEigenValues,
+		bool centered)
 {
 	 //Construct a buffer used by the pca analysis
 	int sz = static_cast<int>(cloud.size()*2);
@@ -3409,19 +3421,9 @@ float computeNormalsComplexity(
 	}
 	if(oi>1)
 	{
-		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi)), 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 pcaEigenvalueAt(pca_analysis.eigenvalues, is2d?1:2)*(is2d?2.0f:3.0f);
+		return normalsComplexity(
+				cv::Mat(data_normals, cv::Range(0, oi)),
+				centered, pcaEigenVectors, pcaEigenValues);
 	}
 	return 0.0f;
 }
@@ -3431,7 +3433,8 @@ float computeNormalsComplexity(
 		const Transform & t,
 		bool is2d,
 		cv::Mat * pcaEigenVectors,
-		cv::Mat * pcaEigenValues)
+		cv::Mat * pcaEigenValues,
+		bool centered)
 {
 	 //Construct a buffer used by the pca analysis
 	int sz = static_cast<int>(cloud.size()*2);
@@ -3465,19 +3468,9 @@ float computeNormalsComplexity(
 	}
 	if(oi>1)
 	{
-		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi)), 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 pcaEigenvalueAt(pca_analysis.eigenvalues, is2d?1:2)*(is2d?2.0f:3.0f);
+		return normalsComplexity(
+				cv::Mat(data_normals, cv::Range(0, oi)),
+				centered, pcaEigenVectors, pcaEigenValues);
 	}
 	return 0.0f;
 }

corelib/test/CMakeLists.txt

@@ -173,6 +173,11 @@ add_executable(test_registrationvis test_registrationvis.cpp)
 target_link_libraries(test_registrationvis gtest_main rtabmap_core)
 add_test(NAME test_registrationvis COMMAND test_registrationvis)
 
+#RegistrationIcp.h
+add_executable(test_registrationicp test_registrationicp.cpp)
+target_link_libraries(test_registrationicp gtest_main rtabmap_core)
+add_test(NAME test_registrationicp COMMAND test_registrationicp)
+
 #LocalGridMaker.h
 add_executable(test_localgridmaker test_localgridmaker.cpp)
 target_link_libraries(test_localgridmaker gtest_main rtabmap_core)

guilib/src/PreferencesDialog.cpp

@@ -1394,6 +1394,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str());
 	_ui->loopClosure_icpPointToPlaneGroundNormalsUp->setObjectName(Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
 	_ui->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str());
+	_ui->loopClosure_icpPointToPlaneComplexityCentered->setObjectName(Parameters::kIcpPointToPlaneComplexityCentered().c_str());
 	_ui->loopClosure_icpPointToPlaneLowComplexityStrategy->setObjectName(Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
 	_ui->loopClosure_icpDebugExportFormat->setObjectName(Parameters::kIcpDebugExportFormat().c_str());
 

guilib/src/ui/preferencesDialog.ui

@@ -24910,7 +24910,7 @@ Lower the ratio -&gt; higher the precision.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="23" column="1">
+                      <item row="24" column="1">
                        <widget class="QLabel" name="label_640">
                         <property name="text">
                          <string>Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are &quot;pcd&quot;, &quot;ply&quot; or &quot;vtk&quot;. If logger level is debug, from and to scans will stamped, so previous files won't be overwritten.</string>
@@ -24962,9 +24962,29 @@ Lower the ratio -&gt; higher the precision.</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="23" column="0">
+                      <item row="24" column="0">
                        <widget class="QLineEdit" name="loopClosure_icpDebugExportFormat"/>
                       </item>
+                      <item row="23" column="0">
+                       <widget class="QCheckBox" name="loopClosure_icpPointToPlaneComplexityCentered">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="23" column="1">
+                       <widget class="QLabel" name="label_icpPointToPlaneComplexityCentered">
+                        <property name="text">
+                         <string>Structural-complexity metric. If unchecked (default), use the uncentered second-moment matrix (1/N) * sum(n_i * n_i^T) — its smallest eigenvalue measures how well the surface normals span the ambient space, which correctly distinguishes perpendicular-surface scenes from corridors. If checked, use centered PCA (cv::PCA covariance) for backwards compatibility — but the centered metric is known to mis-classify perpendicular-surface scenes as degenerate when normals are consistently viewpoint-flipped.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="20" column="0">
                        <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneGroundNormalsUp">
                         <property name="decimals">