Added RANSAC rejection filter to PCL ICP

corelib/include/rtabmap/core/Parameters.h

@@ -831,7 +831,7 @@ class RTABMAP_CORE_EXPORT Parameters
     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(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.");
 
     // libpointmatcher

corelib/include/rtabmap/core/util3d_registration.h

@@ -214,10 +214,15 @@ Transform RTABMAP_CORE_EXPORT icp(
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
 		float epsilon = 0.0f,
-		bool icp2D = false);
+		bool icp2D = false,
+		float ransacOutlierRatio = 0.0f);
 /**
  * @brief Performs Iterative Closest Point (ICP) alignment between two point clouds and returns the resulting transform.
  * @see util3d::icp()
+ *
+ * @param ransacOutlierRatio If > 0 and < 1, install a PCL RANSAC
+ *   correspondence rejector with inlier threshold = ransacOutlierRatio *
+ *   maxCorrespondenceDistance. 0 disables the rejector (default).
  */
 Transform RTABMAP_CORE_EXPORT icp(
 		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
@@ -227,7 +232,8 @@ Transform RTABMAP_CORE_EXPORT icp(
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
 		float epsilon = 0.0f,
-		bool icp2D = false);
+		bool icp2D = false,
+		float ransacOutlierRatio = 0.0f);
 
 /**
  * @brief Performs Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
@@ -262,10 +268,15 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
 		float epsilon = 0.0f,
-		bool icp2D = false);
+		bool icp2D = false,
+		float ransacOutlierRatio = 0.0f);
 /**
  * @briefPerforms Iterative Closest Point (ICP) alignment using a point-to-plane error metric.
  * @see util3d::icpPointToPlane()
+ *
+ * @param ransacOutlierRatio If > 0 and < 1, install a PCL RANSAC
+ *   correspondence rejector with inlier threshold = ransacOutlierRatio *
+ *   maxCorrespondenceDistance. 0 disables the rejector (default).
  */
 Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
@@ -275,7 +286,8 @@ Transform RTABMAP_CORE_EXPORT icpPointToPlane(
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
 		float epsilon = 0.0f,
-		bool icp2D = false);
+		bool icp2D = false,
+		float ransacOutlierRatio = 0.0f);
 
 } // namespace util3d
 } // namespace rtabmap