Added Mem/LaserScanVoxelSize and OdomF2M/ScanSubtractAngle parameters. util3d::computeNormalsComplexity() now returns PCA's eigen vectors and values optionally. RegistrationIcp: detecting complexity of environment when PointToPLane is used, if too low, PointToPoint is done and movements are limited to main direction of the normals.

corelib/include/rtabmap/core/Memory.h

@@ -282,6 +282,7 @@ private:
 	int _imagePostDecimation;
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
+	float _laserScanVoxelSize;
 	int _laserScanNormalK;
 	int _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;

corelib/include/rtabmap/core/OdometryF2M.h

@@ -64,6 +64,7 @@ private:
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
+	float scanSubtractAngle_;
 	int bundleAdjustment_;
 	int bundleMaxFrames_;
 

corelib/include/rtabmap/core/Parameters.h

@@ -211,8 +211,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
     RTABMAP_PARAM(Mem, CompressionParallelized,     bool, true,     "Compression of sensor data is multi-threaded.");
     RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
     RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
     RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, false,    "Use odometry features.");
 
     // KeypointMemory (Keypoint-based)
@@ -396,6 +397,7 @@ class RTABMAP_EXP Parameters
     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, ScanSubtractRadius,  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(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
     RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
 

corelib/include/rtabmap/core/Statistics.h

@@ -134,6 +134,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
 	RTABMAP_STATS(TimingMem, Post_decimation, ms);
 	RTABMAP_STATS(TimingMem, Scan_downsampling, ms);
+	RTABMAP_STATS(TimingMem, Scan_voxel_filtering, ms);
 	RTABMAP_STATS(TimingMem, Scan_normals, ms);
 	RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
 

corelib/include/rtabmap/core/util3d_surface.h

@@ -263,16 +263,24 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
 float RTABMAP_EXP computeNormalsComplexity(
-		const cv::Mat & scan);
+		const cv::Mat & scan,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::Normal> & normals,
-		bool is2d = false);
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointNormal> & cloud,
-		bool is2d = false);
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
 float RTABMAP_EXP computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
-		bool is2d = false);
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
 
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,