OdomF2M: added support to laser scan

corelib/include/rtabmap/core/OdometryF2F.h

@@ -51,6 +51,7 @@ private:
 private:
 	//Parameters:
 	float keyFrameThr_;
+	float scanKeyFrameThr_;
 
 	Registration * registrationPipeline_;
 	Signature refFrame_;

corelib/include/rtabmap/core/OdometryF2M.h

@@ -29,11 +29,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYF2M_H_
 
 #include <rtabmap/core/Odometry.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
 namespace rtabmap {
 
 class Signature;
-class RegistrationVis;
+class Registration;
 
 class RTABMAP_EXP OdometryF2M : public Odometry
 {
@@ -53,11 +55,15 @@ private:
 	int maximumMapSize_;
 	float keyFrameThr_;
 	int maxNewFeatures_;
+	float scanKeyFrameThr_;
+	int scanMaximumMapSize_;
+	float scanSubstractRadius_;
 	std::string fixedMapPath_;
 
-	RegistrationVis * regVis_;
+	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
+	std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr > scansBuffer_;
 };
 
 }

corelib/include/rtabmap/core/OdometryInfo.h

@@ -45,6 +45,7 @@ public:
 		variance(0.0f),
 		features(0),
 		localMapSize(0),
+		localScanMapSize(0),
 		timeEstimation(0.0f),
 		timeParticleFiltering(0.0f),
 		stamp(0),
@@ -63,6 +64,7 @@ public:
 		output.variance = variance;
 		output.features = features;
 		output.localMapSize = localMapSize;
+		output.localScanMapSize = localScanMapSize;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
@@ -80,6 +82,7 @@ public:
 	float variance;
 	int features;
 	int localMapSize;
+	int localScanMapSize;
 	float timeEstimation;
 	float timeParticleFiltering;
 	double stamp;
@@ -89,15 +92,16 @@ public:
 	Transform transformGroundTruth;
 	float distanceTravelled;
 
-	int type; // 0=BOW, 1=F2F, 2=ICP, 3=Mono
+	int type; // 0=F2M, 1=F2F
 
-	// BOW
+	// F2M
 	std::multimap<int, cv::KeyPoint> words;
 	std::vector<int> wordMatches;
 	std::vector<int> wordInliers;
 	std::map<int, cv::Point3f> localMap;
+	cv::Mat localScanMap;
 
-	// F2F && Mono
+	// F2F
 	std::vector<cv::Point2f> refCorners;
 	std::vector<cv::Point2f> newCorners;
 	std::vector<int> cornerInliers;

corelib/include/rtabmap/core/Parameters.h

@@ -360,12 +360,15 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, KalmanProcessNoise, 	float, 0.001,     "Process noise covariance value.");
 	RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,      "Process measurement covariance value.");
 	RTABMAP_PARAM(Odom, GuessMotion,            bool, true,       "Guess next transformation from the last motion computed.");
-	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.5,       "Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,       "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.7,       "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
 
 	// Odometry Bag-of-words
-	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 1000,   "Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
-	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,      "Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
-	RTABMAP_PARAM_STR(OdomF2M, FixedMapPath,    "",          "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
+	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
+	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
+	RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
+	RTABMAP_PARAM(OdomF2M, ScanSubstractRadius, float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
+	RTABMAP_PARAM_STR(OdomF2M, FixedMapPath,    "",           "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
 
 	// Odometry Mono
 	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");

corelib/include/rtabmap/core/util3d_filtering.h

@@ -233,6 +233,34 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius = 1);
 
+/**
+ * For convenience.
+ */
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
+
+/**
+ * Subtract a cloud from another one using radius filtering.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param cloud the input cloud to subtract.
+ * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @return the indices of the points satisfying the parameters.
+ */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle = M_PI/4.0f,
+		int minNeighborsInRadius = 1);
+
 /**
  * For convenience.
  */
@@ -240,7 +268,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
 		float radiusSearch,
-		float maxAngle,
+		float maxAngle = M_PI/4.0f,
 		int minNeighborsInRadius = 1);
 
 /**

corelib/include/rtabmap/core/util3d_registration.h

@@ -79,7 +79,7 @@ Transform RTABMAP_EXP icp(
 		int maximumIterations,
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
-		double epsilon = 0,
+		float epsilon = 0.0f,
 		bool icp2D = false);
 
 Transform RTABMAP_EXP icpPointToPlane(
@@ -88,7 +88,9 @@ Transform RTABMAP_EXP icpPointToPlane(
 		double maxCorrespondenceDistance,
 		int maximumIterations,
 		bool & hasConverged,
-		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered);
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
+		float epsilon = 0.0f,
+		bool icp2D = false);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		const cv::Mat & depth,

corelib/include/rtabmap/core/util3d_transforms.h

@@ -62,6 +62,9 @@ pcl::PointXYZ RTABMAP_EXP transformPoint(
 pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 		const pcl::PointXYZRGB & pt,
 		const Transform & transform);
+pcl::PointNormal RTABMAP_EXP transformPoint(
+		const pcl::PointNormal & point,
+		const Transform & transform);
 
 } // namespace util3d
 } // namespace rtabmap