Generalized ICP feature

[arjay55]

Hi,

Is there a Generalized ICP feature on RTABMAP? If there isn't, any advice on which section of the code to add this feature? PCL has a class for this. If it's not too hard, I will fork this and attempt to add this feature, or have this as feature request. Thanks. I am working on my research regarding high accuracy SLAM.

[matlabbe]

RTAB-Map implemented three ICP approaches, PCL default one, libpointmatcher and CCCorelib (the one used in CloudCompare). We can chose which one we want by using parameter Icp/Strategy parameter.

Param: Icp/Strategy = "1"  [ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).]

For Generalized ICP, do you mean https://pointclouds.org/documentation/classpcl_1_1_generalized_iterative_closest_point.html ?

For Icp/Strategy=0, we have wrapping functions for PointToPoint and PointToPLane approaches here:

rtabmap/corelib/src/util3d_registration.cpp

Lines 386 to 492 in 71bb0cf

	Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
	const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
	double maxCorrespondenceDistance,
	int maximumIterations,
	bool & hasConverged,
	pcl::PointCloud<PointT> & cloud_source_registered,
	float epsilon,
	bool icp2D)
	{
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	// Set the input source and target
	icp.setInputTarget (cloud_target);
	icp.setInputSource (cloud_source);
	if(icp2D)
	{
	typename pcl::registration::TransformationEstimation2D<PointT, PointT>::Ptr est;
	est.reset(new pcl::registration::TransformationEstimation2D<PointT, PointT>);
	icp.setTransformationEstimation(est);
	}
	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
	// Set the maximum number of iterations (criterion 1)
	icp.setMaximumIterations (maximumIterations);
	// Set the transformation epsilon (criterion 2)
	icp.setTransformationEpsilon (epsilon*epsilon);
	// Set the euclidean distance difference epsilon (criterion 3)
	//icp.setEuclideanFitnessEpsilon (1);
	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
	// Perform the alignment
	icp.align (cloud_source_registered);
	hasConverged = icp.hasConverged();
	return Transform::fromEigen4f(icp.getFinalTransformation());
	}
	// return transform from source to target (All points must be finite!!!)
	Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
	const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
	double maxCorrespondenceDistance,
	int maximumIterations,
	bool & hasConverged,
	pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
	float epsilon,
	bool icp2D)
	{
	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
	}
	// return transform from source to target (All points must be finite!!!)
	Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
	const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
	double maxCorrespondenceDistance,
	int maximumIterations,
	bool & hasConverged,
	pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
	float epsilon,
	bool icp2D)
	{
	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
	}
	// return transform from source to target (All points/normals must be finite!!!)
	template<typename PointNormalT>
	Transform icpPointToPlaneImpl(
	const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_source,
	const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_target,
	double maxCorrespondenceDistance,
	int maximumIterations,
	bool & hasConverged,
	pcl::PointCloud<PointNormalT> & cloud_source_registered,
	float epsilon,
	bool icp2D)
	{
	pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
	// Set the input source and target
	icp.setInputTarget (cloud_target);
	icp.setInputSource (cloud_source);
	typename pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>::Ptr est;
	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
	icp.setTransformationEstimation(est);
	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
	icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
	// Set the maximum number of iterations (criterion 1)
	icp.setMaximumIterations (maximumIterations);
	// Set the transformation epsilon (criterion 2)
	icp.setTransformationEpsilon (epsilon*epsilon);
	// Set the euclidean distance difference epsilon (criterion 3)
	//icp.setEuclideanFitnessEpsilon (1);
	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
	// Perform the alignment
	icp.align (cloud_source_registered);
	hasConverged = icp.hasConverged();
	Transform t = Transform::fromEigen4f(icp.getFinalTransformation());
	if(icp2D)
	{
	// FIXME probably an estimation approach already 2D like in icp() version above exists.
	t = t.to3DoF();
	}
	return t;
	}

These functions are used in RegistrationICP when Icp/Strategy=0. You could easily wrap a new function called util3d::gicp(...) using pcl::gicp(...), then check where util3d::icp and util3d::icpPointToPlane are called and add new if(strategy_ == 4) { // do GICP} else {//do current strategy=0 functions...}. If you need to expose GICP parameters in RTAB-Map, look how we prefixed specific Icp parameters:

rtabmap/corelib/include/rtabmap/core/Parameters.h

Lines 714 to 757 in 71bb0cf

	// ICP registration parameters
	#ifdef RTABMAP_POINTMATCHER
	RTABMAP_PARAM(Icp, Strategy, int, 1, "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
	#else
	RTABMAP_PARAM(Icp, Strategy, int, 0, "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
	#endif
	RTABMAP_PARAM(Icp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
	RTABMAP_PARAM(Icp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
	RTABMAP_PARAM(Icp, VoxelSize, float, 0.05, "Uniform sampling voxel size (0=disabled).");
	RTABMAP_PARAM(Icp, DownsamplingStep, int, 1, "Downsampling step size (1=no sampling). This is done before uniform sampling.");
	RTABMAP_PARAM(Icp, RangeMin, float, 0, "Minimum range filtering (0=disabled).");
	RTABMAP_PARAM(Icp, RangeMax, float, 0, "Maximum range filtering (0=disabled).");
	#ifdef RTABMAP_POINTMATCHER
	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1, "Max distance for point correspondences.");
	#else
	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");
	#endif
	RTABMAP_PARAM(Icp, ReciprocalCorrespondences, bool, true, "To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence.");
	RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations.");
	RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
	RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.1, "Ratio of matching correspondences to accept the transform.");
	RTABMAP_PARAM(Icp, Force4DoF, bool, false, uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
	#ifdef RTABMAP_POINTMATCHER
	RTABMAP_PARAM(Icp, PointToPlane, bool, true, "Use point to plane ICP.");
	#else
	RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
	#endif
	RTABMAP_PARAM(Icp, PointToPlaneK, int, 5, "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, 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, OutlierRatio, float, 0.85, uFormat("Outlier ratio used with %s>0. For libpointmatcher, this parameter set TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the \"finalOverlapRatio\". The value should be between 0 and 1.", kIcpStrategy().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.");
	// libpointmatcher
	RTABMAP_PARAM_STR(Icp, PMConfig, "", uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
	RTABMAP_PARAM(Icp, PMMatcherKnn, int, 1, "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
	RTABMAP_PARAM(Icp, PMMatcherEpsilon, float, 0.0, "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
	RTABMAP_PARAM(Icp, PMMatcherIntensity, bool, false, uFormat("KDTreeMatcher: among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
	RTABMAP_PARAM(Icp, CCSamplingLimit, unsigned int, 50000, "Maximum number of points per cloud (they are randomly resampled below this limit otherwise).");
	RTABMAP_PARAM(Icp, CCFilterOutFarthestPoints, bool, false, "If true, the algorithm will automatically ignore farthest points from the reference, for better convergence.");
	RTABMAP_PARAM(Icp, CCMaxFinalRMS, float, 0.2, "Maximum final RMS error.");