Merge branch 'devel' of https://github.com/introlab/rtabmap

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 13)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 3)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

cmake_modules/FindORB_SLAM2.cmake

@@ -9,13 +9,15 @@
 
 find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
 find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
-find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib)
+find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
+find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
 
-IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
    SET(ORB_SLAM2_FOUND TRUE)
-   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
+   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
-   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY})
+   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY})
-ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
+ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
 
 IF (ORB_SLAM2_FOUND)
    # show which ORB_SLAM2 was found only if not quiet

corelib/include/rtabmap/core/CameraRGB.h

@@ -85,7 +85,8 @@ public:
 			int maxScanPts = 0,
 			int downsampleStep = 1,
 			float voxelSize = 0.0f,
-			int normalsK = 0, // compute normals if > 0
+			int normalsK = 0,        // compute normals if > 0
+			float normalsRadius = 0, // compute normals if > 0
 			const Transform & localTransform=Transform::getIdentity())
 	{
 		_scanPath = dir;
@@ -93,6 +94,7 @@ public:
 		_scanMaxPts = maxScanPts;
 		_scanDownsampleStep = downsampleStep;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 		_scanVoxelSize = voxelSize;
 		if(_scanDownsampleStep>1)
 		{
@@ -158,6 +160,7 @@ private:
 	int _scanDownsampleStep;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
 
 	bool _depthFromScan;
 	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical

corelib/include/rtabmap/core/CameraThread.h

@@ -73,13 +73,15 @@ public:
 			int decimation=4,
 			float maxDepth=4.0f,
 			float voxelSize = 0.0f,
-			int normalsK = 0)
+			int normalsK = 0,
+			int normalsRadius = 0.0f)
 	{
 		_scanFromDepth = enabled;
 		_scanDecimation=decimation;
 		_scanMaxDepth = maxDepth;
 		_scanVoxelSize = voxelSize;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 	}
 
 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -107,6 +109,7 @@ private:
 	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;

corelib/include/rtabmap/core/Memory.h

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

corelib/include/rtabmap/core/Odometry.h

@@ -68,6 +68,7 @@ public:
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
 	double previousStamp() const {return previousStamp_;}
+	unsigned int framesProcessed() const {return framesProcessed_;}
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -98,6 +99,7 @@ private:
 	Transform previousVelocityTransform_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
+	unsigned int framesProcessed_;
 
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;

corelib/include/rtabmap/core/OdometryDVO.h

@@ -53,10 +53,12 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_DVO
 	dvo::DenseTracker * dvo_;
 	dvo::core::RgbdImagePyramid * reference_;
 	dvo::core::RgbdCameraPyramid * camera_;
 	bool lost_;
+#endif
 	Transform motionFromKeyFrame_;
 	Transform previousLocalTransform_;
 

corelib/include/rtabmap/core/OdometryEvent.h

@@ -41,7 +41,7 @@ class OdometryEvent : public UEvent
 public:
 	OdometryEvent()
 	{
-		_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+		_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 	OdometryEvent(
 		const SensorData & data,
@@ -51,17 +51,17 @@ public:
 			_pose(pose),
 			_info(info)
 	{
-		if(_info.covariance.empty())
+		if(_info.reg.covariance.empty())
 		{
-			_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+			_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 		}
-		UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1);
+		UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
 	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
@@ -69,7 +69,7 @@ public:
 	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
 	const Transform & pose() const {return _pose;}
-	const cv::Mat & covariance() const {return _info.covariance;}
+	const cv::Mat & covariance() const {return _info.reg.covariance;}
 	std::vector<float> velocity() const {
 		if(_info.interval>0.0)
 		{

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/OdometryFovis.h

@@ -53,13 +53,15 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_FOVIS
 	fovis::VisualOdometry * fovis_;
 	fovis::Rectification * rect_;
 	fovis::StereoCalibration * stereoCalib_;
 	fovis::DepthImage * depthImage_;
 	fovis::StereoDepth * stereoDepth_;
-	ParametersMap fovisParameters_;
 	bool lost_;
+#endif
+	ParametersMap fovisParameters_;
 	Transform previousLocalTransform_;
 };
 

corelib/include/rtabmap/core/OdometryInfo.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <map>
 #include "rtabmap/core/Transform.h"
+#include "rtabmap/core/RegistrationInfo.h"
 #include <opencv2/features2d/features2d.hpp>
 
 namespace rtabmap {
@@ -39,9 +40,6 @@ class OdometryInfo
 public:
 	OdometryInfo() :
 		lost(true),
-		matches(0),
-		inliers(0),
-		icpInliersRatio(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
@@ -62,10 +60,7 @@ public:
 	{
 		OdometryInfo output;
 		output.lost = lost;
-		output.matches = matches;
+		output.reg = reg.copyWithoutData();
-		output.inliers = inliers;
-		output.icpInliersRatio = icpInliersRatio;
-		output.covariance = covariance.clone();
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
@@ -87,10 +82,7 @@ public:
 	}
 
 	bool lost;
-	int matches;
+	RegistrationInfo reg;
-	int inliers;
-	float icpInliersRatio;
-	cv::Mat covariance;
 	int features;
 	int localMapSize;
 	int localScanMapSize;
@@ -108,12 +100,10 @@ public:
 	Transform transformGroundTruth;
 	float distanceTravelled;
 
-	int type; // 0=F2M, 1=F2F
+	int type;
 
 	// F2M
 	std::multimap<int, cv::KeyPoint> words;
-	std::vector<int> wordMatches;
-	std::vector<int> wordInliers;
 	std::map<int, cv::Point3f> localMap;
 	cv::Mat localScanMap;
 

corelib/include/rtabmap/core/OdometryORBSLAM2.h

@@ -51,9 +51,11 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_ORB_SLAM2
 	ORBSLAM2System * orbslam2_;
 	ORB_SLAM2::System * system_;
 	bool firstFrame_;
+#endif
 	Transform originLocalTransform_;
 
 };

corelib/include/rtabmap/core/OdometryViso2.h

@@ -47,12 +47,14 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_VISO2
 	VisualOdometryStereo * viso2_;
 	int ref_frame_change_method_;       // Reference frame method (defautl 0): 0=under inliers threshold, 1=min pixel motion,
 	int ref_frame_inlier_threshold_;    // method 0. Change the reference frame if the number of inliers is low
 	double ref_frame_motion_threshold_; // method 1. Change the reference frame if last motion is small
 	bool lost_;
 	bool keep_reference_frame_;
+#endif
 	Transform reference_motion_;
 	Transform previousLocalTransform_;
 	ParametersMap viso2Parameters_;

corelib/include/rtabmap/core/Parameters.h

@@ -211,7 +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, LaserScanNormalK,            int, 0,         "If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors 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 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)
@@ -386,7 +388,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Odom, GuessMotion,            bool, false,  "Guess next transformation from the last motion computed.");
     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, VisKeyFrameThr,         int, 100,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. 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.");
+    RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[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.");
     RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,       "Decimation of the images before registration. 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(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
 
@@ -395,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).");
 
@@ -510,7 +513,9 @@ class RTABMAP_EXP Parameters
     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.2,   "Ratio of matching correspondences to accept the transform.");
     RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
-    RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
+    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 20,      "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, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
 
     // libpointmatcher
     RTABMAP_PARAM(Icp, PM,                       bool, false,   "Use libpointmatcher for ICP registration instead of PCL's implementation.");

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -64,7 +64,9 @@ private:
 	float _epsilon;
 	float _correspondenceRatio;
 	bool _pointToPlane;
-	int _pointToPlaneNormalNeighbors;
+	int _pointToPlaneK;
+	float _pointToPlaneRadius;
+	float _pointToPlaneMinComplexity;
 	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
 	float _libpointmatcherOutlierRatio;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -39,10 +39,26 @@ public:
 		matches(0),
 		icpInliersRatio(0),
 		icpTranslation(0.0f),
-		icpRotation(0.0f)
+		icpRotation(0.0f),
+		icpStructuralComplexity(0.0f)
+
 	{
 	}
 
+	RegistrationInfo copyWithoutData() const
+	{
+		RegistrationInfo output;
+		output.covariance = covariance.clone();
+		output.rejectedMsg = rejectedMsg;
+		output.inliers = inliers;
+		output.matches = matches;
+		output.icpInliersRatio = icpInliersRatio;
+		output.icpTranslation = icpTranslation;
+		output.icpRotation = icpRotation;
+		output.icpStructuralComplexity = icpStructuralComplexity;
+		return output;
+	}
+
 	cv::Mat covariance;
 	std::string rejectedMsg;
 
@@ -56,6 +72,7 @@ public:
 	float icpInliersRatio;
 	float icpTranslation;
 	float icpRotation;
+	float icpStructuralComplexity;
 };
 
 }

corelib/include/rtabmap/core/Statistics.h

@@ -77,7 +77,10 @@ class RTABMAP_EXP Statistics
 
 	RTABMAP_STATS(NeighborLinkRefining, Accepted,);
 	RTABMAP_STATS(NeighborLinkRefining, Inliers,);
-	RTABMAP_STATS(NeighborLinkRefining, Inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,);
 	RTABMAP_STATS(NeighborLinkRefining, Variance,);
 	RTABMAP_STATS(NeighborLinkRefining, Pts,);
 
@@ -131,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.h

@@ -192,15 +192,20 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
 
 // return CV_32FC3  (x,y,z)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
-// return CV_32FC6 (x,y,z,normal_z,normal_y,normalz)
+// return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
 // return CV_32FC4 (x,y,z,rgb)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
-// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz)
+// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
+// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
 // return CV_32FC2  (x,y)
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
+// return CV_32FC5  (x,y,normal_x, normal_y, normal_z)
+cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
+cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
 // For laserScan of type CV_32FC2, z is set to null.
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
 // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.

corelib/include/rtabmap/core/util3d_filtering.h

@@ -141,6 +141,12 @@ pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/include/rtabmap/core/util3d_surface.h

@@ -212,38 +212,76 @@ cv::Mat RTABMAP_EXP mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0);
 
-
+cv::Mat RTABMAP_EXP computeNormals(
+		const cv::Mat & laserScan,
+		int searchK,
+		float searchRadius);
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
-pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,
 		float normalSmoothingSize = 10.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
-pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float maxDepthChangeFactor = 0.02f,
 		float normalSmoothingSize = 10.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
+float RTABMAP_EXP computeNormalsComplexity(
+		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,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		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,
 		float searchRadius = 0.0f,

corelib/src/CMakeLists.txt

@@ -325,12 +325,12 @@ ENDIF(dvo_core_FOUND)
 
 IF(ORB_SLAM2_FOUND)
     SET(INCLUDE_DIRS
+        ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
 		${INCLUDE_DIRS}
-		${ORB_SLAM2_INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
+	    ${ORB_SLAM2_LIBRARIES}
 		${LIBRARIES}
-		${ORB_SLAM2_LIBRARIES}
 	)
 ENDIF(ORB_SLAM2_FOUND)
 

corelib/src/CameraRGB.cpp

@@ -70,6 +70,7 @@ CameraImages::CameraImages() :
 		_scanDownsampleStep(1),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_scanNormalsRadius(0),
 		_depthFromScan(false),
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
@@ -99,6 +100,7 @@ CameraImages::CameraImages(const std::string & path,
 	_scanDownsampleStep(1),
 	_scanVoxelSize(0.0f),
 	_scanNormalsK(0),
+	_scanNormalsRadius(0),
 	_depthFromScan(false),
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
@@ -685,9 +687,9 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				cloud = util3d::voxelize(cloud, _scanVoxelSize);
 				UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size());
 			}
-			if(_scanNormalsK > 0 && cloud->size())
+			if((_scanNormalsK > 0 || _scanNormalsRadius) && cloud->size())
 			{
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius);
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 				pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 				scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());

corelib/src/CameraThread.cpp

@@ -58,6 +58,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_scanMinDepth(0.0f),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_scanNormalsRadius(0.0f),
 		_stereoDense(new StereoBM(parameters)),
 		_distortionModel(0),
 		_bilateralFiltering(false),
@@ -297,7 +298,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 			UASSERT(_scanDecimation >= 1);
 			UTimer timer;
 			pcl::IndicesPtr validIndices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
 					data,
 					_scanDecimation,
 					_scanMaxDepth,
@@ -316,18 +317,18 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 				}
 				else if(!cloud->is_dense)
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
 					cloud = denseCloud;
 				}
 
 				if(cloud->size())
 				{
-					if(_scanNormalsK>0)
+					if(_scanNormalsK>0 || _scanNormalsRadius>0.0f)
 					{
 						Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
-						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
-						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
 					}

corelib/src/Graph.cpp

@@ -1603,7 +1603,7 @@ int findNearestNode(
 		kdTree->nearestKSearch(pt, 1, ind, dist);
 		if(ind.size() && dist.size() && ind[0] >= 0)
 		{
-			UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
+			//UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
 			id = ids[ind[0]];
 		}
 	}

corelib/src/Memory.cpp

@@ -88,7 +88,9 @@ Memory::Memory(const ParametersMap & parameters) :
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
+	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
+	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -439,7 +441,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
 	Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
+	Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
 	Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
+	Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
@@ -2404,6 +2408,18 @@ Transform Memory::computeIcpTransformMulti(
 	UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
 
 	UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
+	if(ULogger::level() == ULogger::kDebug)
+	{
+		std::string ids;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(iter->first != fromId)
+			{
+				ids += uNumber2Str(iter->first) + " ";
+			}
+		}
+		UDEBUG("%d vs %s", fromId, ids.c_str());
+	}
 
 	// make sure that all laser scans are loaded
 	std::list<Signature*> depthToLoad;
@@ -2436,6 +2452,8 @@ Transform Memory::computeIcpTransformMulti(
 		std::string msg;
 		int maxPoints = fromScan.cols;
 		pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
+		bool is2D = true;
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			if(iter->first != fromId)
@@ -2445,14 +2463,33 @@ Transform Memory::computeIcpTransformMulti(
 				{
 					cv::Mat scan;
 					s->sensorData().uncompressData(0, 0, &scan);
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
+					if(!scan.empty())
-							scan,
-							s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
-					if(scan.cols > maxPoints)
 					{
-						maxPoints = scan.cols;
+						if(scan.channels() != 2 && scan.channels() != 5)
+						{
+							is2D = false;
+						}
+
+						if(scan.channels() >= 5)
+						{
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormal = util3d::laserScanToPointCloudNormal(
+									scan,
+									s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
+							*assembledToNormalClouds += *cloudNormal;
+						}
+						else
+						{
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
+									scan,
+									s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
+							*assembledToClouds += *cloud;
+						}
+
+						if(scan.cols > maxPoints)
+						{
+							maxPoints = scan.cols;
+						}
 					}
-					*assembledToClouds += *cloud;
 				}
 				else
 				{
@@ -2460,15 +2497,22 @@ Transform Memory::computeIcpTransformMulti(
 				}
 			}
 		}
-		if(assembledToClouds->size())
+
+		cv::Mat assembledScan;
+		if(assembledToNormalClouds->size())
 		{
-			assembledData.setLaserScanRaw(
+			assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
-					util3d::laserScanFromPointCloud(*assembledToClouds),
-					LaserScanInfo(
-							fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
-							fromS->sensorData().laserScanInfo().maxRange(),
-							Transform::getIdentity())); // scans are in base frame
 		}
+		else if(assembledToClouds->size())
+		{
+			assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
+		}
+		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
+		assembledData.setLaserScanRaw(assembledScan,
+				LaserScanInfo(
+					fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
+					fromS->sensorData().laserScanInfo().maxRange(),
+					is2D?Transform(0,0,fromS->sensorData().laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));
 
 		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
 		std::vector<int> inliersV;
@@ -3268,7 +3312,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						data.depthOrRightRaw().rows,
 						data.depthOrRightRaw().type(),
 						CV_16UC1, CV_32FC1, CV_8UC1).c_str());
-	UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(4) || data.laserScanRaw().type() == CV_32FC(6));
+	UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(4) || data.laserScanRaw().type() == CV_32FC(5) || data.laserScanRaw().type() == CV_32FC(6) || data.laserScanRaw().type() == CV_32FC(7));
 
 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().size() == 0 &&
@@ -3718,13 +3762,41 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
 		UDEBUG("time downsampling scan = %fs", t);
 	}
-	if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3 && !isIntermediateNode)
+	if(!laserScan.empty() && _laserScanVoxelSize > 0.0f && !isIntermediateNode)
 	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+		float pointsBeforeFiltering = laserScan.cols;
-		float x,y,z;
+		if(laserScan.channels() == 4 || laserScan.channels() == 7)
-		data.laserScanInfo().localTransform().getTranslation(x,y,z);
+		{
-		pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _laserScanNormalK, Eigen::Vector3f(x,y,z));
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
-		laserScan = util3d::laserScanFromPointCloud(*cloud, *normals);
+			cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
+			laserScan = util3d::laserScanFromPointCloud(*cloud);
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+			cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
+			if(laserScan.channels() == 2 || laserScan.channels() == 5)
+			{
+				laserScan = util3d::laserScan2dFromPointCloud(*cloud);
+			}
+			else
+			{
+				laserScan = util3d::laserScanFromPointCloud(*cloud);
+			}
+		}
+		float ratio = float(laserScan.cols) / pointsBeforeFiltering;
+		maxLaserScanMaxPts = int(float(maxLaserScanMaxPts) * ratio);
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemScan_voxel_filtering(), t*1000.0f);
+		UDEBUG("time voxel filtering scan = %fs", t);
+	}
+	if(!laserScan.empty() &&
+		(_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) &&
+		laserScan.channels() > 1 && laserScan.channels() < 5 &&
+		!isIntermediateNode)
+	{
+		laserScan = util3d::computeNormals(laserScan, _laserScanNormalK, _laserScanNormalRadius);
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_normals(), t*1000.0f);
 		UDEBUG("time normals scan = %fs", t);

corelib/src/OccupancyGrid.cpp

@@ -207,7 +207,7 @@ void OccupancyGrid::createLocalMap(
 	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
 			node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
 
-	if(node.sensorData().laserScanRaw().channels() == 2 && !occupancyFromCloud_)
+	if((node.sensorData().laserScanRaw().channels() == 2 || node.sensorData().laserScanRaw().channels() == 5) && !occupancyFromCloud_)
 	{
 		UDEBUG("2D laser scan");
 		//2D

corelib/src/Odometry.cpp

@@ -102,7 +102,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
-		distanceTravelled_(0)
+		distanceTravelled_(0),
+		framesProcessed_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 
@@ -168,6 +169,7 @@ void Odometry::reset(const Transform & initialPose)
 	_resetCurrentCount = 0;
 	previousStamp_ = 0;
 	distanceTravelled_ = 0;
+	framesProcessed_ = 0;
 	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
@@ -544,6 +546,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			distanceTravelled_ += t.getNorm();
 			info->distanceTravelled = distanceTravelled_;
 		}
+		++framesProcessed_;
 
 		return _pose *= t; // update
 	}

corelib/src/OdometryDVO.cpp

@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryDVO.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
@@ -43,10 +42,12 @@ namespace rtabmap {
 
 OdometryDVO::OdometryDVO(const ParametersMap & parameters) :
 	Odometry(parameters),
+#ifdef RTABMAP_DVO
 	dvo_(0),
 	reference_(0),
 	camera_(0),
 	lost_(false),
+#endif
 	motionFromKeyFrame_(Transform::getIdentity())
 {
 }
@@ -270,7 +271,7 @@ Transform OdometryDVO::computeTransform(
 	if(info)
 	{
 		info->type = (int)kTypeDVO;
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 	}
 
 	UINFO("Odom update time = %fs", timer.elapsed());

corelib/src/OdometryF2F.cpp

@@ -83,6 +83,7 @@ Transform OdometryF2F::computeTransform(
 		return output;
 	}
 
+	bool addKeyFrame = false;
 	RegistrationInfo regInfo;
 
 	UASSERT(!this->getPose().isNull());
@@ -99,8 +100,8 @@ Transform OdometryF2F::computeTransform(
 		output = registrationPipeline_->computeTransformationMod(
 				tmpRefFrame,
 				newFrame,
-				// special case for ICP-only odom, set guess to identity if we just started
+				// special case for ICP-only odom, set guess to identity if we just started or reset
-				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
+				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(),
 				&regInfo);
 
 		if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
@@ -185,7 +186,7 @@ Transform OdometryF2F::computeTransform(
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().size();
-			if(features == 0)
+			if(registrationPipeline_->isImageRequired() && features == 0)
 			{
 				newFrame = Signature(data);
 				// this will generate features only for the first frame or if optical flow was used (no 3d words)
@@ -209,6 +210,8 @@ Transform OdometryF2F::computeTransform(
 
 				//reset motion
 				lastKeyFramePose_.setNull();
+
+				addKeyFrame = true;
 			}
 			else
 			{
@@ -243,12 +246,17 @@ Transform OdometryF2F::computeTransform(
 
 	if(info)
 	{
-		info->type = 1;
+		info->type = kTypeF2F;
-		info->covariance = regInfo.covariance;
-		info->inliers = regInfo.inliers;
-		info->icpInliersRatio = regInfo.icpInliersRatio;
-		info->matches = regInfo.matches;
 		info->features = newFrame.sensorData().keypoints().size();
+		info->keyFrameAdded = addKeyFrame;
+		if(this->isInfoDataFilled())
+		{
+			info->reg = regInfo;
+		}
+		else
+		{
+			info->reg = regInfo.copyWithoutData();
+		}
 	}
 
 	UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",

corelib/src/OdometryF2M.cpp

@@ -65,6 +65,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
+	scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
 	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	map_(new Signature(-1)),
@@ -80,6 +81,10 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
+	if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_))
+	{
+		scanSubtractAngle_ *= M_PI/180.0f;
+	}
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
 	UASSERT(bundleMaxFrames_ >= 0);
@@ -186,9 +191,10 @@ Transform OdometryF2M::computeTransform(
 			Transform transform = regPipeline_->computeTransformationMod(
 					tmpMap,
 					*lastFrame_,
-					// special case for ICP-only odom, set guess to identity if we just started
+					// special case for ICP-only odom, set guess to identity if we just started or reset
-					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
+					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
 					&regInfo);
+
 			if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
 			{
 				tmpMap = *map_;
@@ -591,7 +597,7 @@ Transform OdometryF2M::computeTransform(
 
 					if(lastFrame_->sensorData().laserScanRaw().cols)
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform());
 						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
 
 						pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -604,7 +610,7 @@ Transform OdometryF2M::computeTransform(
 									mapCloudNormals,
 									pcl::IndicesPtr(new std::vector<int>),
 									scanSubtractRadius_,
-									0.0f);
+									scanSubtractAngle_);
 							newPoints = frameCloudNormalsIndices->size();
 						}
 						else
@@ -623,17 +629,6 @@ Transform OdometryF2M::computeTransform(
 									newPoints,
 									scanMaximumMapSize_);
 
-							if(newPoints < 20)
-							{
-								UWARN("The number of new scan points added to local odometry "
-									  "map is low (%d), you may want to decrease the parameter \"%s\" "
-									  "(current value=%f and ICP inliers ratio is %f)",
-										newPoints,
-										Parameters::kOdomScanKeyFrameThr().c_str(),
-										scanKeyFrameThr_,
-										regInfo.icpInliersRatio);
-							}
-
 							if(scansBuffer_.size() > 1 &&
 								int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
 							{
@@ -691,7 +686,16 @@ Transform OdometryF2M::computeTransform(
 									*mapCloudNormals += *scansBuffer_.back().first;
 								}
 							}
-							mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
+							if(mapScan.channels() == 2 || mapScan.channels() == 5)
+							{
+								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
+								mapScan = util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint);
+							}
+							else
+							{
+								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
+								mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint);
+							}
 							modified=true;
 						}
 					}
@@ -702,7 +706,18 @@ Transform OdometryF2M::computeTransform(
 				{
 					*map_ = tmpMap;
 
-					map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
+					if(mapScan.channels() == 2 || mapScan.channels() == 5)
+					{
+
+						map_->sensorData().setLaserScanRaw(mapScan,
+									LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
+					}
+					else
+					{
+						map_->sensorData().setLaserScanRaw(mapScan,
+								LaserScanInfo(0, 0.0f, newFramePose.translation()));
+					}
+
 					map_->setWords(mapWords);
 					map_->setWords3(mapPoints);
 				 	map_->setWordsDescriptors(mapDescriptors);
@@ -717,7 +732,7 @@ Transform OdometryF2M::computeTransform(
 				if(this->isInfoDataFilled())
 				{
 					info->localMap = uMultimapToMap(tmpMap.getWords3());
-					info->localScanMap = tmpMap.sensorData().laserScanRaw();
+					info->localScanMap = util3d::transformLaserScan(tmpMap.sensorData().laserScanRaw(), tmpMap.sensorData().laserScanInfo().localTransform());
 				}
 			}
 		}
@@ -831,7 +846,18 @@ Transform OdometryF2M::computeTransform(
 					frameValid = true;
 					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
 					scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
-					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
+					if(lastFrame_->sensorData().laserScanRaw().channels() == 2 || lastFrame_->sensorData().laserScanRaw().channels() == 5)
+					{
+						Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
+						map_->sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
+								LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
+					}
+					else
+					{
+						Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
+						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
+								LaserScanInfo(0, 0.0f, newFramePose.translation()));
+					}
 					addKeyFrame = true;
 				}
 				else
@@ -854,7 +880,7 @@ Transform OdometryF2M::computeTransform(
 				if(this->isInfoDataFilled())
 				{
 					info->localMap = uMultimapToMap(map_->getWords3());
-					info->localScanMap = map_->sensorData().laserScanRaw();
+					info->localScanMap = util3d::transformLaserScan(map_->sensorData().laserScanRaw(), map_->sensorData().laserScanInfo().localTransform());
 				}
 			}
 		}
@@ -873,10 +899,6 @@ Transform OdometryF2M::computeTransform(
 
 	if(info)
 	{
-		info->covariance = regInfo.covariance;
-		info->inliers = regInfo.inliers;
-		info->matches = regInfo.matches;
-		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->features = nFeatures;
 		info->localKeyFrames = (int)bundlePoses_.size();
 		info->keyFrameAdded = addKeyFrame;
@@ -886,8 +908,11 @@ Transform OdometryF2M::computeTransform(
 
 		if(this->isInfoDataFilled())
 		{
-			info->wordMatches = regInfo.matchesIDs;
+			info->reg = regInfo;
-			info->wordInliers = regInfo.inliersIDs;
+		}
+		else
+		{
+			info->reg = regInfo.copyWithoutData();
 		}
 	}
 

corelib/src/OdometryFovis.cpp

@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryFovis.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
@@ -40,13 +39,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
-	Odometry(parameters),
+	Odometry(parameters)
+#ifdef RTABMAP_FOVIS
+    ,
 	fovis_(0),
 	rect_(0),
 	stereoCalib_(0),
 	depthImage_(0),
 	stereoDepth_(0),
 	lost_(false)
+#endif
 {
 	fovisParameters_ = Parameters::filterParameters(parameters, "OdomFovis");
 	if(parameters.find(Parameters::kOdomVisKeyFrameThr()) != parameters.end())
@@ -381,9 +383,9 @@ Transform OdometryFovis::computeTransform(
 		info->type = (int)kTypeFovis;
 		info->keyFrameAdded = fovis_->getChangeReferenceFrames();
 		info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
-		info->matches = fovis_->getMotionEstimator()->getNumMatches();
+		info->reg.matches = fovis_->getMotionEstimator()->getNumMatches();
-		info->inliers = fovis_->getMotionEstimator()->getNumInliers();
+		info->reg.inliers = fovis_->getMotionEstimator()->getNumInliers();
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 
 		if(this->isInfoDataFilled())
 		{

corelib/src/OdometryMono.cpp

@@ -352,7 +352,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 
 					if(this->isInfoDataFilled() && info)
 					{
-						info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
+						info->reg.matchesIDs.insert(info->reg.matchesIDs.end(), matches.begin(), matches.end());
 					}
 					correspondences = (int)matches.size();
 
@@ -397,10 +397,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 
 							if(this->isInfoDataFilled() && info && inliersV.size())
 							{
-								info->wordInliers.resize(inliersV.size());
+								info->reg.inliersIDs.resize(inliersV.size());
 								for(unsigned int i=0; i<inliersV.size(); ++i)
 								{
-									info->wordInliers[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
+									info->reg.inliersIDs[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
 								}
 							}
 
@@ -976,7 +976,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		if(info)
 		{
 			// a very high variance tells that the new pose is not linked with the previous one
-			info->covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
+			info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
 		}
 
 			// generate kpts
@@ -1013,8 +1013,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 	if(this->isInfoDataFilled() && info)
 	{
 		//info->variance = variance;
-		info->inliers = inliers;
+		info->reg.inliers = inliers;
-		info->matches = correspondences;
+		info->reg.matches = correspondences;
 		info->features = nFeatures;
 		info->localMapSize = (int)localMap_.size();
 		info->localMap = localMap_;

corelib/src/OdometryORBSLAM2.cpp

@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryORBSLAM2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
@@ -746,10 +745,13 @@ public:
 namespace rtabmap {
 
 OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
-	Odometry(parameters),
+	Odometry(parameters)
+#ifdef RTABMAP_ORB_SLAM2
+    ,
 	orbslam2_(0),
 	system_(0),
 	firstFrame_(true)
+#endif
 {
 #ifdef RTABMAP_ORB_SLAM2
 	orbslam2_ = new ORBSLAM2System(parameters);
@@ -891,15 +893,15 @@ Transform OdometryORBSLAM2::computeTransform(
 	{
 		info->lost = t.isNull();
 		info->type = (int)kTypeORBSLAM2;
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 		info->localMapSize = totalMapPoints;
 		info->localKeyFrames = totalKfs;
 
 		if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap)
 		{
 			const std::vector<cv::KeyPoint> & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys;
-			info->wordMatches.resize(kpts.size());
+			info->reg.matchesIDs.resize(kpts.size());
-			info->wordInliers.resize(kpts.size());
+			info->reg.inliersIDs.resize(kpts.size());
 			int oi = 0;
 			for (unsigned int i = 0; i < kpts.size(); ++i)
 			{
@@ -915,14 +917,15 @@ Transform OdometryORBSLAM2::computeTransform(
 				info->words.insert(std::make_pair(wordId, kpts[i]));
 				if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
 				{
-					info->wordMatches[oi] = wordId;
+					info->reg.matchesIDs[oi] = wordId;
-					info->wordInliers[oi] = wordId;
+					info->reg.inliersIDs[oi] = wordId;
 					++oi;
 				}
 			}
-			info->wordMatches.resize(oi);
+			info->reg.matchesIDs.resize(oi);
-			info->wordInliers.resize(oi);
+			info->reg.inliersIDs.resize(oi);
-			info->inliers = oi;
+			info->reg.inliers = oi;
+			info->reg.matches = oi;
 
 			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)

corelib/src/OdometryViso2.cpp

@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryViso2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
@@ -53,15 +52,19 @@ namespace rtabmap {
 
 OdometryViso2::OdometryViso2(const ParametersMap & parameters) :
 	Odometry(parameters),
+#ifdef RTABMAP_VISO2
 	viso2_(0),
 	ref_frame_change_method_(0),
 	ref_frame_inlier_threshold_(Parameters::defaultOdomVisKeyFrameThr()),
 	ref_frame_motion_threshold_(5.0),
 	lost_(false),
 	keep_reference_frame_(false),
+#endif
 	reference_motion_(Transform::getIdentity())
 {
+#ifdef RTABMAP_VISO2
 	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), ref_frame_inlier_threshold_);
+#endif
 	viso2Parameters_ = Parameters::filterParameters(parameters, "OdomViso2");
 }
 
@@ -273,11 +276,11 @@ Transform OdometryViso2::computeTransform(
 	{
 		info->type = (int)kTypeViso2;
 		info->keyFrameAdded = !keep_reference_frame_;
-		info->matches = viso2_->getNumberOfMatches();
+		info->reg.matches = viso2_->getNumberOfMatches();
-		info->inliers = viso2_->getNumberOfInliers();
+		info->reg.inliers = viso2_->getNumberOfInliers();
 		if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
 		{
-			info->covariance = covariance;
+			info->reg.covariance = covariance;
 		}
 
 		if(this->isInfoDataFilled())

corelib/src/OptimizerG2O.cpp

@@ -37,19 +37,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_motion_estimation.h>
 #include <rtabmap/core/util3d.h>
 
-#ifdef RTABMAP_G2O
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
-#include "g2o/config.h"
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/core/block_solver.h"
 #include "g2o/core/factory.h"
 #include "g2o/core/optimization_algorithm_factory.h"
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
+#include "g2o/core/robust_kernel_impl.h"
 #include "g2o/core/linear_solver.h"
+
+#ifdef RTABMAP_G2O
 #include "g2o/types/sba/types_sba.h"
+#include "g2o/solvers/eigen/linear_solver_eigen.h"
+#include "g2o/config.h"
 #include "g2o/types/slam2d/types_slam2d.h"
 #include "g2o/types/slam3d/types_slam3d.h"
-#include "g2o/core/robust_kernel_impl.h"
 #ifdef G2O_HAVE_CSPARSE
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
 #endif
@@ -57,14 +60,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef G2O_HAVE_CHOLMOD
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #endif
-#include "g2o/solvers/eigen/linear_solver_eigen.h"
-
 enum {
     PARAM_OFFSET=0,
 };
+#endif // RTABMAP_G2O
+
+#ifdef RTABMAP_ORB_SLAM2
+#include "g2o/types/types_sba.h"
+#include "g2o/types/types_six_dof_expmap.h"
+#include "g2o/solvers/linear_solver_eigen.h"
+#endif
 
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
 typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
+#ifdef RTABMAP_G2O
 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
 #ifdef G2O_HAVE_CSPARSE
 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
@@ -79,14 +88,15 @@ typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearChol
 #include "vertigo/g2o/edge_se3Switchable.h"
 #include "vertigo/g2o/vertex_switchLinear.h"
 #endif
+#endif
 
-#endif // end RTABMAP_G2O
+#endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
 
 namespace rtabmap {
 
 bool OptimizerG2O::available()
 {
-#ifdef RTABMAP_G2O
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
 	return true;
 #else
 	return false;
@@ -123,6 +133,13 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 	UASSERT(pixelVariance_ > 0.0);
 	UASSERT(baseline_ >= 0.0);
 
+#ifdef RTABMAP_ORB_SLAM2
+	if(solver_ != 3)
+	{
+		UWARN("g2o built with ORB_SLAM2 has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
+		solver_ = 3;
+	}
+#else
 #ifndef G2O_HAVE_CHOLMOD
 	if(solver_ == 2)
 	{
@@ -138,6 +155,8 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 		solver_ = 1;
 	}
 #endif
+#endif
+
 }
 
 std::map<int, Transform> OptimizerG2O::optimize(
@@ -612,8 +631,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		UWARN("This method should be called at least with 1 pose!");
 	}
 	UDEBUG("Optimizing graph...end!");
+#else
+#ifdef RTABMAP_ORB_SLAM2
+	UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM2, only SBA is available.");
 #else
 	UERROR("Not built with G2O support!");
+#endif
 #endif
 	return optimizedPoses;
 }
@@ -628,7 +651,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		std::set<int> * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
-#ifdef RTABMAP_G2O
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
 	UDEBUG("Optimizing graph...");
 
 	optimizedPoses.clear();
@@ -638,6 +661,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
 		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
 
+#ifdef RTABMAP_ORB_SLAM2
+		linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+#else
 		if(solver_ == 3)
 		{
 			//eigen
@@ -663,14 +689,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			//pcg
 			linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
 		}
+#endif
 
 		g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
 
+#ifndef RTABMAP_ORB_SLAM2
 		if(optimizer_ == 1)
 		{
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr));
 		}
 		else
+#endif
 		{
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
 		}
@@ -686,9 +715,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 			// Add node's pose
 			UASSERT(!camPose.isNull());
+#ifdef RTABMAP_ORB_SLAM2
+			g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
+#else
 			g2o::VertexCam * vCam = new g2o::VertexCam();
+#endif
 
 			Eigen::Affine3d a = camPose.toEigen3d();
+#ifdef RTABMAP_ORB_SLAM2
+			a = a.inverse();
+			vCam->setEstimate(g2o::SE3Quat(a.rotation(), a.translation()));
+#else
 			g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
 			cam.setKcam(
 					iterModel->second.fx(),
@@ -697,6 +734,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					iterModel->second.cy(),
 					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
 			vCam->setEstimate(cam);
+#endif
 			vCam->setId(iter->first);
 
 			// negative root means that all other poses should be fixed instead of the root
@@ -718,6 +756,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			++iter;
 		}
 
+#ifndef RTABMAP_ORB_SLAM2
 		UDEBUG("fill edges to g2o...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
@@ -759,6 +798,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				}
 			}
 		}
+#endif
 
 		UDEBUG("fill 3D points to g2o...");
 		const int stepVertexId = poses.rbegin()->first+1;
@@ -775,7 +815,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				vpt3d->setMarginalized(true);
 				optimizer.addVertex(vpt3d);
 
-				//UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
+				UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
 
 				// set observations
 				for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
@@ -786,25 +826,62 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						const cv::Point3f & pt = jter->second;
 						double depth = pt.z;
 
-						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
+						UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
 
 						g2o::OptimizableGraph::Edge * e;
+						double baseline = 0.0;
+#ifdef RTABMAP_ORB_SLAM2
+						g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
+						std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
+
+						cv::Point3f t = util3d::transformPoint(pt3d, Transform::fromEigen3d(vcam->estimate()).inverse());
+						UDEBUG("in cam %d frame=(%f,%f,%f)", camId, t.x, t.y, t.z);
+
+						cv::Point3f t2 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()).inverse());
+						UDEBUG("in cam2 %d frame=(%f,%f,%f)",camId, t2.x, t2.y, t2.z);
+
+						g2o::Vector3d t3 = vcam->estimate().map(g2o::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+						UDEBUG("in cam3 %d frame=(%f,%f,%f)",camId, t3[0], t3[1], t3[2]);
+
+						cv::Point3f t4 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()));
+						UDEBUG("in cam4 %d frame=(%f,%f,%f)",camId, t4.x, t4.y, t4.z);
+
+						UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
+						baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
+#else
 						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
+						baseline = vcam->estimate().baseline;
+#endif
 						double variance = pixelVariance_;
-						if(uIsFinite(depth) && depth > 0.0 && vcam->estimate().baseline > 0.0)
+						if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
 						{
 							// stereo edge
+#ifdef RTABMAP_ORB_SLAM2
+							g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
+							float disparity = baseline * iterModel->second.fx() / depth;
+							Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
+							es->setMeasurement(obs);
+							//variance *= log(exp(1)+disparity);
+							es->setInformation(Eigen::Matrix3d::Identity() / variance);
+							es->fx = iterModel->second.fx();
+							es->fy = iterModel->second.fy();
+							es->cx = iterModel->second.cx();
+							es->cy = iterModel->second.cy();
+							es->bf = baseline*es->fx;
+							e = es;
+#else
 							g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
-							float disparity = vcam->estimate().baseline * vcam->estimate().Kcam(0,0) / depth;
+							float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
 							Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
 							es->setMeasurement(obs);
 							//variance *= log(exp(1)+disparity);
 							es->setInformation(Eigen::Matrix3d::Identity() / variance);
 							e = es;
+#endif
 						}
 						else
 						{
-							if(vcam->estimate().baseline > 0.0)
+							if(baseline > 0.0)
 							{
 								UWARN("Stereo camera model detected but current "
 										"observation (pt=%d to cam=%d) has null depth (%f m), adding "
@@ -812,15 +889,28 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 										vpt3d->id()-stepVertexId, camId, depth);
 							}
 							// mono edge
+#ifdef RTABMAP_ORB_SLAM2
+							g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
+							Eigen::Vector2d obs( pt.x, pt.y);
+							em->setMeasurement(obs);
+							em->setInformation(Eigen::Matrix2d::Identity() / variance);
+							em->fx = iterModel->second.fx();
+							em->fy = iterModel->second.fy();
+							em->cx = iterModel->second.cx();
+							em->cy = iterModel->second.cy();
+							e = em;
+
+#else
 							g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
 							Eigen::Vector2d obs( pt.x, pt.y);
 							em->setMeasurement(obs);
 							em->setInformation(Eigen::Matrix2d::Identity() / variance);
 							e = em;
+#endif
 						}
 						e->setVertex(0, vpt3d);
 						e->setVertex(1, vcam);
-
+						UDEBUG("");
 						if(robustKernelDelta_ > 0.0)
 						{
 							g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
@@ -875,8 +965,20 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					{
 						(*iter)->setLevel(1);
 						++outliersCount;
-						double d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
+						double d = 0.0;
+#ifdef RTABMAP_ORB_SLAM2
+						if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
+						{
+							d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
+						}
+						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+#else
+						if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
+						{
+							d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
+						}
 						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+#endif
 
 						const cv::Point3f & pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
@@ -909,11 +1011,19 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		// update poses
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
+#ifdef RTABMAP_ORB_SLAM2
+			const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
+#else
 			const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
+#endif
 			if(v)
 			{
 				Transform t = Transform::fromEigen3d(v->estimate());
 
+#ifdef RTABMAP_ORB_SLAM2
+				t=t.inverse();
+#endif
+
 				// remove model local transform
 				t *= models.at(iter->first).localTransform().inverse();
 				UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());

corelib/src/Parameters.cpp

@@ -225,6 +225,10 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.13.3
+		removedParameters_.insert(std::make_pair("Icp/PointToPlaneNormalNeighbors", std::make_pair(true,  Parameters::kIcpPointToPlaneK())));
+
+
 		// 0.13.1
 		removedParameters_.insert(std::make_pair("Rtabmap/VhStrategy",            std::make_pair(true,  Parameters::kVhEpEnabled())));
 
@@ -326,7 +330,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("LccIcp3/Iterations",                  std::make_pair(false, Parameters::kIcpIterations())));
 		removedParameters_.insert(std::make_pair("LccIcp3/CorrespondenceRatio",         std::make_pair(false, Parameters::kIcpCorrespondenceRatio())));
 		removedParameters_.insert(std::make_pair("LccIcp3/PointToPlane",                std::make_pair(true,  Parameters::kIcpPointToPlane())));
-		removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true,  Parameters::kIcpPointToPlaneNormalNeighbors())));
+		removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true,  Parameters::kIcpPointToPlaneK())));
 
 		removedParameters_.insert(std::make_pair("LccIcp2/MaxCorrespondenceDistance",   std::make_pair(true,  Parameters::kIcpMaxCorrespondenceDistance())));
 		removedParameters_.insert(std::make_pair("LccIcp2/Iterations",                  std::make_pair(true,  Parameters::kIcpIterations())));

corelib/src/RegistrationIcp.cpp

@@ -36,16 +36,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UTimer.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/vtk_io.h>
 #include <pcl/conversions.h>
 
 #ifdef RTABMAP_POINTMATCHER
+#include <fstream>
 #include "pointmatcher/PointMatcher.h"
 typedef PointMatcher<float> PM;
 typedef PM::DataPoints DP;
 
-DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
+DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, bool is2D)
 {
 	UDEBUG("");
 	typedef DP::Label Label;
@@ -65,8 +64,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
 	isFeature.push_back(true);
 	featLabels.push_back(Label("y", 1));
 	isFeature.push_back(true);
-	featLabels.push_back(Label("z", 1));
+	if(!is2D)
-	isFeature.push_back(true);
+	{
+		featLabels.push_back(Label("z", 1));
+		isFeature.push_back(true);
+	}
 	featLabels.push_back(Label("pad", 1));
 
 	// create cloud
@@ -74,20 +76,21 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
 	cloud.getFeatureViewByName("pad").setConstant(1);
 
 	// fill cloud
-	View viewX(cloud.getFeatureViewByName("x"));
+	View view(cloud.getFeatureViewByName("x"));
-	View viewY(cloud.getFeatureViewByName("y"));
-	View viewZ(cloud.getFeatureViewByName("z"));
 	for(unsigned int i=0; i<pclCloud->size(); ++i)
 	{
-		viewX(0, i) = pclCloud->at(i).x;
+		view(0, i) = pclCloud->at(i).x;
-		viewY(0, i) = pclCloud->at(i).y;
+		view(1, i) = pclCloud->at(i).y;
-		viewZ(0, i) = pclCloud->at(i).z;
+		if(!is2D)
+		{
+			view(2, i) = pclCloud->at(i).z;
+		}
 	}
 
 	return cloud;
 }
 
-DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
+DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
 {
 	UDEBUG("");
 	typedef DP::Label Label;
@@ -107,8 +110,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
 	isFeature.push_back(true);
 	featLabels.push_back(Label("y", 1));
 	isFeature.push_back(true);
-	featLabels.push_back(Label("z", 1));
+	if(!is2D)
-	isFeature.push_back(true);
+	{
+		featLabels.push_back(Label("z", 1));
+		isFeature.push_back(true);
+	}
 
 	descLabels.push_back(Label("normals", 3));
 	isFeature.push_back(false);
@@ -122,17 +128,18 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
 	cloud.getFeatureViewByName("pad").setConstant(1);
 
 	// fill cloud
-	View viewX(cloud.getFeatureViewByName("x"));
+	View view(cloud.getFeatureViewByName("x"));
-	View viewY(cloud.getFeatureViewByName("y"));
-	View viewZ(cloud.getFeatureViewByName("z"));
 	View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
 	View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
 	View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
 	for(unsigned int i=0; i<pclCloud->size(); ++i)
 	{
-		viewX(0, i) = pclCloud->at(i).x;
+		view(0, i) = pclCloud->at(i).x;
-		viewY(0, i) = pclCloud->at(i).y;
+		view(1, i) = pclCloud->at(i).y;
-		viewZ(0, i) = pclCloud->at(i).z;
+		if(!is2D)
+		{
+			view(2, i) = pclCloud->at(i).z;
+		}
 		viewNormalX(0, i) = pclCloud->at(i).normal_x;
 		viewNormalY(0, i) = pclCloud->at(i).normal_y;
 		viewNormalZ(0, i) = pclCloud->at(i).normal_z;
@@ -153,14 +160,13 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
 	pclCloud.is_dense = true;
 
 		// fill cloud
-	ConstView viewX(cloud.getFeatureViewByName("x"));
+	ConstView view(cloud.getFeatureViewByName("x"));
-	ConstView viewY(cloud.getFeatureViewByName("y"));
+	bool is3D = cloud.featureExists("z");
-	ConstView viewZ(cloud.getFeatureViewByName("z"));
 	for(unsigned int i=0; i<pclCloud.size(); ++i)
 	{
-		pclCloud.at(i).x = viewX(0, i);
+		pclCloud.at(i).x = view(0, i);
-		pclCloud.at(i).y = viewY(0, i);
+		pclCloud.at(i).y = view(1, i);
-		pclCloud.at(i).z = viewZ(0, i);
+		pclCloud.at(i).z = is3D?view(2, i):0;
 	}
 }
 
@@ -176,17 +182,16 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
 	pclCloud.is_dense = true;
 
 		// fill cloud
-	ConstView viewX(cloud.getFeatureViewByName("x"));
+	ConstView view(cloud.getFeatureViewByName("x"));
-	ConstView viewY(cloud.getFeatureViewByName("y"));
+	bool is3D = cloud.featureExists("z");
-	ConstView viewZ(cloud.getFeatureViewByName("z"));
 	ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
 	ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
 	ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
 	for(unsigned int i=0; i<pclCloud.size(); ++i)
 	{
-		pclCloud.at(i).x = viewX(0, i);
+		pclCloud.at(i).x = view(0, i);
-		pclCloud.at(i).y = viewY(0, i);
+		pclCloud.at(i).y = view(1, i);
-		pclCloud.at(i).z = viewZ(0, i);
+		pclCloud.at(i).z = is3D?view(2, i):0;
 		pclCloud.at(i).normal_x = viewNormalX(0, i);
 		pclCloud.at(i).normal_y = viewNormalY(0, i);
 		pclCloud.at(i).normal_z = viewNormalZ(0, i);
@@ -225,7 +230,9 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_epsilon(Parameters::defaultIcpEpsilon()),
 	_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
-	_pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()),
+	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
+	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
+	_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
 	_libpointmatcher(Parameters::defaultIcpPM()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
@@ -257,7 +264,10 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
 	Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
-	Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
+	UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
 
 	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
 	Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
@@ -319,8 +329,20 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 			icp->outlierFilters.clear();
 			icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
 			params.clear();
+			if(_pointToPlane)
+			{
+				params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
+				icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params));
+				params.clear();
 
-			icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create(_pointToPlane?"PointToPlaneErrorMinimizer":"PointToPointErrorMinimizer"));
+				params["force2D"] = force3DoF()?"1":"0";
+				icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params));
+				params.clear();
+			}
+			else
+			{
+				icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
+			}
 
 			icp->transformationCheckers.clear();
 			params["maxIterationCount"] = uNumber2Str(_maxIterations);
@@ -332,6 +354,11 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 			params["smoothLength"] =    uNumber2Str(4);
 			icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
 			params.clear();
+
+			params["maxRotationNorm"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
+			params["maxTranslationNorm"] =    uNumber2Str(_maxTranslation<=0.0f?std::numeric_limits<float>::max():_maxTranslation);
+			icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("BoundTransformationChecker", params));
+			params.clear();
 		}
 	}
 #endif
@@ -342,7 +369,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
 	UASSERT(_epsilon >= 0.0f);
 	UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
-	UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
+	UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str());
 }
 
 Transform RegistrationIcp::computeTransformationImpl(
@@ -354,7 +381,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
 	UDEBUG("Voxel size=%f", _voxelSize);
 	UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
-	UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
+	UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
+	UDEBUG("Normal radius=%d", _pointToPlaneRadius);
 	UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
 	UDEBUG("Max Iterations=%d", _maxIterations);
 	UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
@@ -403,199 +431,46 @@ Transform RegistrationIcp::computeTransformationImpl(
 			float correspondencesRatio = 0.0f;
 			int correspondences = 0;
 			double variance = 1.0;
+			bool transformComputed = false;
+			bool tooLowComplexityForPlaneToPlane = false;
+			cv::Mat complexityVectors;
 
 			if( _pointToPlane &&
 				_voxelSize == 0.0f &&
-				fromScan.channels() == 6 &&
+				fromScan.channels() >= 5 &&
-				toScan.channels() == 6)
+				toScan.channels() >= 5 &&
+				!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
 			{
 				//special case if we have already normals computed and there is no filtering
-				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
-				pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
 
-				UDEBUG("Conversion time = %f s", timer.ticks());
+				cv::Mat complexityVectorsFrom, complexityVectorsTo;
-				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+				double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
-#ifdef RTABMAP_POINTMATCHER
+				double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
-				if(_libpointmatcher)
+				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
+				info.icpStructuralComplexity = complexity;
+				if(complexity < _pointToPlaneMinComplexity)
 				{
-					// Load point clouds
+					tooLowComplexityForPlaneToPlane = true;
-					DP data = pclToDP(fromCloudNormals);
+					complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
-					DP ref = pclToDP(toCloudNormals);
+					UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
-
-					// Compute the transformation to express data in ref
-					PM::TransformationParameters T;
-					try
-					{
-						UASSERT(_libpointmatcherICP != 0);
-						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);
-						UDEBUG("libpointmatcher icp...done!");
-						icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
-
-						float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
-						UDEBUG("match ratio: %f", matchRatio);
-
-						if(!icpT.isNull())
-						{
-							fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
-							hasConverged = true;
-						}
-					}
-					catch(const std::exception & e)
-					{
-						UWARN("libpointmatcher has failed: %s", e.what());
-					}
 				}
 				else
-#endif
 				{
-					icpT = util3d::icpPointToPlane(
+					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
-							fromCloudNormals,
+					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
-							toCloudNormals,
-						   _maxCorrespondenceDistance,
-						   _maxIterations,
-						   hasConverged,
-						   *fromCloudNormalsRegistered,
-						   _epsilon,
-						   this->force3DoF());
-				}
 
-				if(!icpT.isNull() && hasConverged)
-				{
-					util3d::computeVarianceAndCorrespondences(
-							fromCloudNormalsRegistered,
-							toCloudNormals,
-							_maxCorrespondenceDistance,
-							variance,
-							correspondences);
-				}
-			}
-			else
-			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
-				UDEBUG("Conversion time = %f s", timer.ticks());
-
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
-				if(_voxelSize > 0.0f)
-				{
-					int pointsBeforeFiltering = fromCloudFiltered->size();
-					fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
-					maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
-
-					pointsBeforeFiltering = toCloudFiltered->size();
-					toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
-					maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
-
-					UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s",
-							_voxelSize,
-							float(fromCloudFiltered->size()) / float(pointsBeforeFiltering),
-							float(toCloudFiltered->size()) / float(pointsBeforeFiltering),
-							timer.ticks());
-				}
-
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-				if(_pointToPlane) // ICP Point To Plane, only in 3D
-				{
-					pcl::PointCloud<pcl::Normal>::Ptr normals;
-
-					normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
-					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-					pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
-
-					normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
-					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-					pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
-
-					std::vector<int> indices;
-					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
 					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
-
+					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
-					// update output scans
-					fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
-					toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
-
-					UDEBUG("Compute normals time = %f s", timer.ticks());
-
-					if(toCloudNormals->size() && fromCloudNormals->size())
-					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
-
-#ifdef RTABMAP_POINTMATCHER
-						if(_libpointmatcher)
-						{
-							// Load point clouds
-							DP data = pclToDP(fromCloudNormals);
-							DP ref = pclToDP(toCloudNormals);
-
-							// Compute the transformation to express data in ref
-							PM::TransformationParameters T;
-							try
-							{
-								UASSERT(_libpointmatcherICP != 0);
-								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);
-								UDEBUG("libpointmatcher icp...done!");
-								icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
-
-								float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
-								UDEBUG("match ratio: %f", matchRatio);
-
-								if(!icpT.isNull())
-								{
-									fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
-									hasConverged = true;
-								}
-							}
-							catch(const std::exception & e)
-							{
-								UWARN("libpointmatcher has failed: %s", e.what());
-							}
-						}
-						else
-#endif
-						{
-							icpT = util3d::icpPointToPlane(
-									fromCloudNormals,
-									toCloudNormals,
-								   _maxCorrespondenceDistance,
-								   _maxIterations,
-								   hasConverged,
-								   *fromCloudNormalsRegistered,
-								   _epsilon,
-								   this->force3DoF());
-						}
 
 
-						if(!icpT.isNull() && hasConverged)
+					UDEBUG("Conversion time = %f s", timer.ticks());
-						{
+					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
-							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
-									toCloudNormals,
-									_maxCorrespondenceDistance,
-									variance,
-									correspondences);
-						}
-					}
-				}
-				else // ICP Point to Point
-				{
-					if(_voxelSize > 0.0f)
-					{
-						// update output scans
-						fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
-						toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
-					}
-
 #ifdef RTABMAP_POINTMATCHER
 					if(_libpointmatcher)
 					{
 						// Load point clouds
-						DP data = pclToDP(fromCloudFiltered);
+						DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
-						DP ref = pclToDP(toCloudFiltered);
+						DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
 
 						// Compute the transformation to express data in ref
 						PM::TransformationParameters T;
@@ -605,6 +480,311 @@ 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);
+							icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+							UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
+
+							float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+							UDEBUG("match ratio: %f", matchRatio);
+
+							if(!icpT.isNull())
+							{
+								fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+								hasConverged = true;
+							}
+						}
+						catch(const std::exception & e)
+						{
+							UWARN("libpointmatcher has failed: %s", e.what());
+						}
+					}
+					else
+#endif
+					{
+						icpT = util3d::icpPointToPlane(
+								fromCloudNormals,
+								toCloudNormals,
+							   _maxCorrespondenceDistance,
+							   _maxIterations,
+							   hasConverged,
+							   *fromCloudNormalsRegistered,
+							   _epsilon,
+							   this->force3DoF());
+					}
+
+					if(!icpT.isNull() && hasConverged)
+					{
+						util3d::computeVarianceAndCorrespondences(
+								fromCloudNormalsRegistered,
+								toCloudNormals,
+								_maxCorrespondenceDistance,
+								variance,
+								correspondences);
+					}
+					transformComputed = true;
+				}
+			}
+
+			if(!transformComputed)
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
+				UDEBUG("Conversion time = %f s", timer.ticks());
+
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
+				if(_voxelSize > 0.0f)
+				{
+					float pointsBeforeFiltering = (float)fromCloudFiltered->size();
+					fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
+					float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering;
+					maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom);
+
+					pointsBeforeFiltering = (float)toCloudFiltered->size();
+					toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
+					float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering;
+					maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo);
+
+					UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s",
+							_voxelSize,
+							ratioFrom,
+							(int)fromCloudFiltered->size(),
+							maxLaserScansFrom,
+							ratioTo,
+							(int)toCloudFiltered->size(),
+							maxLaserScansTo,
+							timer.ticks());
+				}
+
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
+				if(_pointToPlane && // ICP Point To Plane
+					!tooLowComplexityForPlaneToPlane && // if previously rejected above
+					!((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D
+				{
+					Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
+					pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
+					if(fromScan.channels() == 2 || fromScan.channels() == 5)
+					{
+						if(_voxelSize > 0.0f)
+						{
+							normalsFrom = util3d::computeNormals2D(
+									fromCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
+						else
+						{
+							normalsFrom = util3d::computeFastOrganizedNormals2D(
+									fromCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
+					}
+					else
+					{
+						normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
+					}
+
+					Transform toT = guess * toLocalTransform;
+					Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
+					pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
+					if(toScan.channels() == 2 || toScan.channels() == 5)
+					{
+						if(_voxelSize > 0.0f)
+						{
+							normalsTo = util3d::computeNormals2D(
+									toCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointTo);
+						}
+						else
+						{
+							normalsTo = util3d::computeFastOrganizedNormals2D(
+									toCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointTo);
+						}
+					}
+					else
+					{
+						normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
+					}
+
+					cv::Mat complexityVectorsFrom, complexityVectorsTo;
+					double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5, &complexityVectorsFrom);
+					double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5, &complexityVectorsTo);
+					float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
+					info.icpStructuralComplexity = complexity;
+					if(complexity < _pointToPlaneMinComplexity)
+					{
+						tooLowComplexityForPlaneToPlane = true;
+						complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
+						UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
+
+						pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
+
+						std::vector<int> indices;
+						toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
+						fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+
+						// update output scans
+						if(fromScan.channels() == 2 || fromScan.channels() == 5)
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						else
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						if(toScan.channels() == 2 || toScan.channels() == 5)
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						else
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
+
+						if(toCloudNormals->size() && fromCloudNormals->size())
+						{
+							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+
+#ifdef RTABMAP_POINTMATCHER
+							if(_libpointmatcher)
+							{
+								// Load point clouds
+								DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
+								DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
+
+								// Compute the transformation to express data in ref
+								PM::TransformationParameters T;
+								try
+								{
+									UASSERT(_libpointmatcherICP != 0);
+									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);
+									UDEBUG("libpointmatcher icp...done!");
+									icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+
+									float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+									UDEBUG("match ratio: %f", matchRatio);
+
+									if(!icpT.isNull())
+									{
+										fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+										hasConverged = true;
+									}
+								}
+								catch(const std::exception & e)
+								{
+									UWARN("libpointmatcher has failed: %s", e.what());
+								}
+							}
+							else
+#endif
+							{
+								icpT = util3d::icpPointToPlane(
+										fromCloudNormals,
+										toCloudNormals,
+									   _maxCorrespondenceDistance,
+									   _maxIterations,
+									   hasConverged,
+									   *fromCloudNormalsRegistered,
+									   _epsilon,
+									   this->force3DoF());
+							}
+
+							if(!icpT.isNull() && hasConverged)
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudNormalsRegistered,
+										toCloudNormals,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
+							}
+						}
+						transformComputed = true;
+					}
+				}
+
+				if(!transformComputed) // ICP Point to Point
+				{
+					if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher))
+					{
+						UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
+					}
+
+					if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane)
+					{
+						// update output scans
+						if(fromScan.channels() == 2 || fromScan.channels() == 5)
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						else
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						if(toScan.channels() == 2 || toScan.channels() == 5)
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						else
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+					}
+
+#ifdef RTABMAP_POINTMATCHER
+					if(_libpointmatcher)
+					{
+						// Load point clouds
+						DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);
+						DP ref = pclToDP(toCloudFiltered, toScan.channels() == 2 || toScan.channels() == 5);
+
+						// Compute the transformation to express data in ref
+						PM::TransformationParameters T;
+						try
+						{
+							UASSERT(_libpointmatcherICP != 0);
+							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.)");
+							if(_pointToPlane)
+							{
+								// temporary set PointToPointErrorMinimizer
+								PM::ICP & icpTmp = icp;
+								icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
+
+								for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();)
+								{
+									if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0)
+									{
+										iter = icpTmp.outlierFilters.erase(iter);
+									}
+									else
+									{
+										++iter;
+									}
+								}
+
+								T = icpTmp(data, ref);
+							}
+							else
+							{
+								T = icp(data, ref);
+							}
 							UDEBUG("libpointmatcher icp...done!");
 							icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
 
@@ -638,6 +818,39 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 					if(!icpT.isNull() && hasConverged)
 					{
+						if(tooLowComplexityForPlaneToPlane)
+						{
+							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);
+								v = n*a;
+							}
+							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);
+								v = n1*a;
+								v += n2*b;
+							}
+							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;
+						}
+
 						util3d::computeVarianceAndCorrespondences(
 								fromCloudRegistered,
 								toCloudFiltered,

corelib/src/Rtabmap.cpp

@@ -1116,14 +1116,17 @@ bool Rtabmap::process(
 						else
 						{
 							UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
-							if(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0)
+							if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
 							{
 								_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 							}
 						}
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
-						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
 					}
 				}
@@ -1228,9 +1231,7 @@ bool Rtabmap::process(
 		//============================================================
 		if(_proximityByTime &&
 		   rehearsedId == 0 && // don't do it if rehearsal happened
-		   signature->getWords3().size() &&
 		   _memory->isIncremental() && // don't do it in localization mode
-		   !signature->isBadSignature() &&
 		   signature->getWeight()>=0)
 		{
 			const std::set<int> & stm = _memory->getStMem();
@@ -1511,7 +1512,7 @@ bool Rtabmap::process(
 							++immunizedGlobally;
 						}
 
-						UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
+						//UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
 					}
 					neighbors.erase(iter++);
 				}
@@ -1557,7 +1558,7 @@ bool Rtabmap::process(
 						{
 							++nbDirectNeighborsInDb;
 						}
-						UDEBUG("nt=%d m=%d", iter->first, iter->second);
+						//UDEBUG("nt=%d m=%d", iter->first, iter->second);
 					}
 					neighbors.erase(iter++);
 				}
@@ -1698,7 +1699,7 @@ bool Rtabmap::process(
 							{
 								++immunizedLocally;
 							}
-							UDEBUG("local node %d on path immunized=1", iter->first);
+							//UDEBUG("local node %d on path immunized=1", iter->first);
 						}
 					}
 				}
@@ -1745,7 +1746,7 @@ bool Rtabmap::process(
 					{
 						++immunizedLocally;
 					}
-					UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
+					//UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
 				}
 			}
 		}
@@ -2006,7 +2007,7 @@ bool Rtabmap::process(
 						//find the nearest pose on the path
 						int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
 						UASSERT(nearestId > 0);
-						UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
+						//UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
 
 						// nearest pose must be close and not linked to current location
 						if(!signature->hasLink(nearestId) &&
@@ -2101,7 +2102,7 @@ bool Rtabmap::process(
 						}
 						else
 						{
-							UDEBUG("Path %d ignored", nearestId);
+							//UDEBUG("Path %d ignored", nearestId);
 						}
 					}
 				}
@@ -2996,7 +2997,7 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
 
 					if(valid)
 					{
-						UDEBUG("%d <- %d", nearestId, jter->first);
+						//UDEBUG("%d <- %d", nearestId, jter->first);
 						path.insert(*jter);
 						poses.erase(jter);
 					}

corelib/src/RtabmapThread.cpp

@@ -336,7 +336,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
 					if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
 					{
 						OdometryInfo infoCov;
-						infoCov.covariance = e->info().odomCovariance;
+						infoCov.reg.covariance = e->info().odomCovariance;
 						this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 					}
 					else
@@ -347,7 +347,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
 				else
 				{ 
 					OdometryInfo infoCov;
-					infoCov.covariance = e->info().odomCovariance;
+					infoCov.reg.covariance = e->info().odomCovariance;
 					this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 				}
 				
@@ -570,7 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		if(!lastPose_.isIdentity() &&
 						(odomEvent.pose().isIdentity() ||
-						odomEvent.info().covariance.at<double>(0,0)>=9999))
+						odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
 		{
 			if(odomEvent.pose().isIdentity())
 			{
@@ -578,20 +578,20 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			}
 			else
 			{
-				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().covariance.at<double>(0,0));
+				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0));
 			}
 			pushNewState(kStateTriggeringMap);
 			covariance_ = cv::Mat();
 		}
 
-		if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
+		if(uIsFinite(odomEvent.info().reg.covariance.at<double>(0,0)) &&
-			odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
+			odomEvent.info().reg.covariance.at<double>(0,0) != 1.0 &&
-			odomEvent.info().covariance.at<double>(0,0)>0.0)
+			odomEvent.info().reg.covariance.at<double>(0,0)>0.0)
 		{
 			// Use largest covariance error (to be independent of the odometry frame rate)
-			if(covariance_.empty() || odomEvent.info().covariance.at<double>(0,0) > covariance_.at<double>(0,0))
+			if(covariance_.empty() || odomEvent.info().reg.covariance.at<double>(0,0) > covariance_.at<double>(0,0))
 			{
-				covariance_ = odomEvent.info().covariance;
+				covariance_ = odomEvent.info().reg.covariance;
 			}
 		}
 
@@ -614,7 +614,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			covariance_ = cv::Mat::eye(6,6,CV_64FC1);
 		}
 		OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
-		odomInfo.covariance = covariance_;
+		odomInfo.reg.covariance = covariance_;
 		if(ignoreFrame)
 		{
 			// set negative id so rtabmap will detect it as an intermediate node

corelib/src/SensorData.cpp

@@ -195,7 +195,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -300,7 +300,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -406,7 +406,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = right;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -496,7 +496,7 @@ void SensorData::setOccupancyGrid(
 
 	if(!ground.empty())
 	{
-		if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6))
+		if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(5) || ground.type() == CV_32FC(6) || ground.type() == CV_32FC(7))
 		{
 			_groundCellsRaw = ground;
 			ctGround.start();
@@ -509,7 +509,7 @@ void SensorData::setOccupancyGrid(
 	}
 	if(!obstacles.empty())
 	{
-		if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6))
+		if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(5) || obstacles.type() == CV_32FC(6) || obstacles.type() == CV_32FC(7))
 		{
 			_obstacleCellsRaw = obstacles;
 			ctObstacles.start();

corelib/src/util3d.cpp

@@ -1363,6 +1363,46 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 	return laserScan;
 }
 
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
+{
+	UASSERT(cloud.size() == normals.size());
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		float * ptr = laserScan.ptr<float>(0, i);
+		if(!nullTransform)
+		{
+			pcl::PointXYZRGBNormal pt;
+			pt.x = cloud.at(i).x;
+			pt.y = cloud.at(i).y;
+			pt.z = cloud.at(i).z;
+			pt.normal_x = normals.at(i).normal_x;
+			pt.normal_y = normals.at(i).normal_y;
+			pt.normal_z = normals.at(i).normal_z;
+			pt = util3d::transformPoint(pt, transform);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.z;
+			ptr[4] = pt.normal_x;
+			ptr[5] = pt.normal_y;
+			ptr[6] = pt.normal_z;
+		}
+		else
+		{
+			ptr[0] = cloud.at(i).x;
+			ptr[1] = cloud.at(i).y;
+			ptr[2] = cloud.at(i).z;
+			ptr[4] = normals.at(i).normal_x;
+			ptr[5] = normals.at(i).normal_y;
+			ptr[6] = normals.at(i).normal_z;
+		}
+		int * ptrInt = (int*)ptr;
+		ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
+	}
+	return laserScan;
+}
+
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
@@ -1419,12 +1459,79 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
 	return laserScan;
 }
 
+cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
+	bool nullTransform = transform.isNull();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		float * ptr = laserScan.ptr<float>(0, i);
+		if(!nullTransform)
+		{
+			pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.normal_x;
+			ptr[3] = pt.normal_y;
+			ptr[4] = pt.normal_z;
+		}
+		else
+		{
+			const pcl::PointNormal & pt = cloud.at(i);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.normal_x;
+			ptr[3] = pt.normal_y;
+			ptr[4] = pt.normal_z;
+		}
+
+	}
+	return laserScan;
+}
+
+cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
+{
+	UASSERT(cloud.size() == normals.size());
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		float * ptr = laserScan.ptr<float>(0, i);
+		if(!nullTransform)
+		{
+			pcl::PointNormal pt;
+			pt.x = cloud.at(i).x;
+			pt.y = cloud.at(i).y;
+			pt.z = cloud.at(i).z;
+			pt.normal_x = normals.at(i).normal_x;
+			pt.normal_y = normals.at(i).normal_y;
+			pt.normal_z = normals.at(i).normal_z;
+			pt = util3d::transformPoint(pt, transform);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.normal_x;
+			ptr[3] = pt.normal_y;
+			ptr[4] = pt.normal_z;
+		}
+		else
+		{
+			ptr[0] = cloud.at(i).x;
+			ptr[1] = cloud.at(i).y;
+			ptr[2] = normals.at(i).normal_x;
+			ptr[3] = normals.at(i).normal_y;
+			ptr[4] = normals.at(i).normal_z;
+		}
+	}
+	return laserScan;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)  || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6)  || laserScan.type() == CV_32FC(7));
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
@@ -1440,10 +1547,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
 
 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
@@ -1458,10 +1566,11 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
@@ -1477,10 +1586,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
 
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
@@ -1496,12 +1606,12 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
 pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZ output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
@@ -1511,16 +1621,22 @@ pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
 pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointNormal output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
-	if(laserScan.channels() == 6)
+	if(laserScan.channels() == 5)
+	{
+		output.normal_x = ptr[2];
+		output.normal_y = ptr[3];
+		output.normal_z = ptr[4];
+	}
+	else if(laserScan.channels() == 6)
 	{
 		output.normal_x = ptr[3];
 		output.normal_y = ptr[4];
@@ -1538,12 +1654,12 @@ pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
 pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZRGB output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
@@ -1566,16 +1682,22 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
 pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZRGBNormal output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
-	if(laserScan.channels() == 6)
+	if(laserScan.channels() == 5)
+	{
+		output.normal_x = ptr[2];
+		output.normal_y = ptr[3];
+		output.normal_z = ptr[4];
+	}
+	else if(laserScan.channels() == 6)
 	{
 		output.normal_x = ptr[3];
 		output.normal_y = ptr[4];
@@ -1606,12 +1728,13 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int
 void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
 {
 	UASSERT(!laserScan.empty());
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 
 	const float * ptr = laserScan.ptr<float>(0, 0);
 	min.x = max.x = ptr[0];
 	min.y = max.y = ptr[1];
-	min.z = max.z = laserScan.channels() >= 3?ptr[2]:0.0f;
+	bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
+	min.z = max.z = is3d?ptr[2]:0.0f;
 	for(int i=1; i<laserScan.cols; ++i)
 	{
 		ptr = laserScan.ptr<float>(0, i);
@@ -1622,7 +1745,7 @@ void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max
 		if(ptr[1] < min.y) min.y = ptr[1];
 		else if(ptr[1] > max.y) max.y = ptr[1];
 
-		if(laserScan.channels() >= 3)
+		if(is3d)
 		{
 			if(ptr[2] < min.z) min.z = ptr[2];
 			else if(ptr[2] > max.z) max.z = ptr[2];
@@ -1689,7 +1812,7 @@ cv::Mat projectCloudToCamera(
 {
 	UASSERT(!cameraTransform.isNull());
 	UASSERT(!laserScan.empty());
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
 
 	float fx = cameraMatrixK.at<double>(0,0);
@@ -1700,46 +1823,25 @@ cv::Mat projectCloudToCamera(
 	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
 	Transform t = cameraTransform.inverse();
 
-	const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
-	const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
-	const cv::Vec4f* vec4Ptr = laserScan.ptr<cv::Vec4f>();
-	const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
-	const float* vec7Ptr = laserScan.ptr<float>();
-
 	int count = 0;
 	for(int i=0; i<laserScan.cols; ++i)
 	{
+		const float* ptr = laserScan.ptr<float>(0, i);
+
 		// Get 3D from laser scan
 		cv::Point3f ptScan;
-		if(laserScan.type() == CV_32FC2)
+		if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
 		{
-			ptScan.x = vec2Ptr[i][0];
+			// 2D scans
-			ptScan.y = vec2Ptr[i][1];
+			ptScan.x = ptr[0];
+			ptScan.y = ptr[1];
 			ptScan.z = 0;
 		}
-		else if(laserScan.type() == CV_32FC3)
+		else // 3D scans
 		{
-			ptScan.x = vec3Ptr[i][0];
+			ptScan.x = ptr[0];
-			ptScan.y = vec3Ptr[i][1];
+			ptScan.y = ptr[1];
-			ptScan.z = vec3Ptr[i][2];
+			ptScan.z = ptr[2];
-		}
-		else if(laserScan.type() == CV_32FC(4))
-		{
-			ptScan.x = vec4Ptr[i][0];
-			ptScan.y = vec4Ptr[i][1];
-			ptScan.z = vec4Ptr[i][2];
-		}
-		else if(laserScan.type() == CV_32FC(6))
-		{
-			ptScan.x = vec6Ptr[i][0];
-			ptScan.y = vec6Ptr[i][1];
-			ptScan.z = vec6Ptr[i][2];
-		}
-		else // 7f
-		{
-			ptScan.x = (vec7Ptr+i*7)[0];
-			ptScan.y = (vec7Ptr+i*7)[1];
-			ptScan.z = (vec7Ptr+i*7)[2];
 		}
 		ptScan = util3d::transformPoint(ptScan, t);
 

corelib/src/util3d_filtering.cpp

@@ -369,6 +369,26 @@ pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(
 	return output;
 }
 
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PassThrough<pcl::PointXYZRGBNormal> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
 pcl::IndicesPtr cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

corelib/src/util3d_registration.cpp

@@ -244,8 +244,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-	est->setInputTarget(cloudB);
+	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
-	est->setInputSource(cloudA);
+	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
 
@@ -277,8 +277,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-	est->setInputTarget(cloudB);
+	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
-	est->setInputSource(cloudA);
+	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
 

corelib/src/util3d_surface.cpp

@@ -1994,18 +1994,52 @@ cv::Mat mergeTextures(
 	return globalTextures;
 }
 
+cv::Mat computeNormals(
+		const cv::Mat & laserScan,
+		int searchK,
+		float searchRadius)
+{
+	if(laserScan.empty() || laserScan.channels()<2 || laserScan.channels()>4)
+	{
+		return laserScan;
+	}
+
+	pcl::PointCloud<pcl::Normal>::Ptr normals;
+	if(laserScan.channels() < 4)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+		if(laserScan.channels() == 2)
+		{
+			normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
+			return util3d::laserScan2dFromPointCloud(*cloud, *normals);
+		}
+		else
+		{
+			normals = util3d::computeNormals(cloud, searchK, searchRadius);
+			return util3d::laserScanFromPointCloud(*cloud, *normals);
+		}
+	}
+	else // 4 channels
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
+		normals = util3d::computeNormals(cloud, searchK, searchRadius);
+		return util3d::laserScanFromPointCloud(*cloud, *normals);
+	}
+}
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return computeNormals(cloud, indices, normalKSearch, viewPoint);
+	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
 }
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
@@ -2032,7 +2066,8 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 	//	n.setIndices(indices);
 	//}
 	n.setSearchMethod (tree);
-	n.setKSearch (normalKSearch);
+	n.setKSearch (searchK);
+	n.setRadiusSearch (searchRadius);
 	n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
 	n.compute (*normals);
 
@@ -2041,16 +2076,18 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return computeNormals(cloud, indices, normalKSearch, viewPoint);
+	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
 }
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
@@ -2077,13 +2114,182 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 	//	n.setIndices(indices);
 	//}
 	n.setSearchMethod (tree);
-	n.setKSearch (normalKSearch);
+	n.setKSearch (searchK);
+	n.setRadiusSearch(searchRadius);
 	n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
 	n.compute (*normals);
 
 	return normals;
 }
 
+pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK,
+		float searchRadius,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(searchK>0 || searchRadius>0.0f);
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+	tree->setInputCloud (cloud);
+
+	normals->resize(cloud->size());
+
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	// assuming that points are ordered
+	for(unsigned int i=0; i<cloud->size(); ++i)
+	{
+		const pcl::PointXYZ & pt = cloud->at(i);
+		std::vector<Eigen::Vector3f> neighborNormals;
+		Eigen::Vector3f direction;
+		direction[0] = viewPoint[0] - pt.x;
+		direction[1] = viewPoint[1] - pt.y;
+		direction[2] = viewPoint[2] - pt.z;
+
+		std::vector<int> k_indices;
+		std::vector<float> k_sqr_distances;
+		if(searchRadius>0.0f)
+		{
+			tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK);
+		}
+		else
+		{
+			tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances);
+		}
+
+		for(unsigned int j=0; j<k_indices.size(); ++j)
+		{
+			if(k_indices.at(j) != (int)i)
+			{
+				const pcl::PointXYZ & pt2 = cloud->at(k_indices.at(j));
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+		}
+
+		if(neighborNormals.empty())
+		{
+			normals->at(i).normal_x = bad_point;
+			normals->at(i).normal_y = bad_point;
+			normals->at(i).normal_z = bad_point;
+		}
+		else
+		{
+			Eigen::Vector3f meanNormal(0,0,0);
+			for(unsigned int j=0; j<neighborNormals.size(); ++j)
+			{
+				meanNormal+=neighborNormals[j];
+			}
+			meanNormal /= (float)neighborNormals.size();
+			meanNormal.normalize();
+			normals->at(i).normal_x = meanNormal[0];
+			normals->at(i).normal_y = meanNormal[1];
+			normals->at(i).normal_z = meanNormal[2];
+		}
+	}
+
+	return normals;
+}
+
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK,
+		float searchRadius,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(searchK>0);
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+	normals->resize(cloud->size());
+	searchRadius *= searchRadius; // squared distance
+
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	// assuming that points are ordered
+	for(int i=0; i<(int)cloud->size(); ++i)
+	{
+		int li = i-searchK;
+		if(li<0)
+		{
+			li=0;
+		}
+		int hi = i+searchK;
+		if(hi>=(int)cloud->size())
+		{
+			hi=(int)cloud->size()-1;
+		}
+
+		// get points before not too far
+		const pcl::PointXYZ & pt = cloud->at(i);
+		std::vector<Eigen::Vector3f> neighborNormals;
+		Eigen::Vector3f direction;
+		direction[0] = viewPoint[0] - cloud->at(i).x;
+		direction[1] = viewPoint[1] - cloud->at(i).y;
+		direction[2] = viewPoint[2] - cloud->at(i).z;
+		for(int j=i-1; j>=li; --j)
+		{
+			const pcl::PointXYZ & pt2 = cloud->at(j);
+			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
+			{
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+			else
+			{
+				break;
+			}
+		}
+		for(int j=i+1; j<=hi; ++j)
+		{
+			const pcl::PointXYZ & pt2 = cloud->at(j);
+			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
+			{
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+			else
+			{
+				break;
+			}
+		}
+
+		if(neighborNormals.empty())
+		{
+			normals->at(i).normal_x = bad_point;
+			normals->at(i).normal_y = bad_point;
+			normals->at(i).normal_z = bad_point;
+		}
+		else
+		{
+			Eigen::Vector3f meanNormal(0,0,0);
+			for(unsigned int j=0; j<neighborNormals.size(); ++j)
+			{
+				meanNormal+=neighborNormals[j];
+			}
+			meanNormal /= (float)neighborNormals.size();
+			meanNormal.normalize();
+			normals->at(i).normal_x = meanNormal[0];
+			normals->at(i).normal_y = meanNormal[1];
+			normals->at(i).normal_z = meanNormal[2];
+		}
+	}
+
+	return normals;
+}
+
 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor,
@@ -2132,6 +2338,204 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 	return normals;
 }
 
+float computeNormalsComplexity(
+		const cv::Mat & scan,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7))
+	{
+		 //Construct a buffer used by the pca analysis
+		int sz = static_cast<int>(scan.cols*2);
+		bool is2d = scan.channels() == 5;
+		cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+		int oi = 0;
+		for (int i = 0; i < scan.cols; ++i)
+		{
+			const float * ptrScan = scan.ptr<float>(0, i);
+			if(scan.channels() == 5)
+			{
+				if(uIsFinite(ptrScan[2]) && uIsFinite(ptrScan[3]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[2];
+					ptr[1] = ptrScan[3];
+				}
+			}
+			else if(scan.channels() == 6)
+			{
+				if(uIsFinite(ptrScan[3]) && uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[3];
+					ptr[1] = ptrScan[4];
+					ptr[2] = ptrScan[5];
+				}
+			}
+			else
+			{
+				if(uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]) && uIsFinite(ptrScan[6]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[4];
+					ptr[1] = ptrScan[5];
+					ptr[2] = ptrScan[6];
+				}
+			}
+		}
+		if(oi>1)
+		{
+			cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), 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 pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+		}
+	}
+	else if(!scan.empty())
+	{
+		UERROR("Scan doesn't have normals!");
+	}
+	return 0.0f;
+}
+
+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(cloud.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointNormal & pt = cloud.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), 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 pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::Normal> & normals,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(normals.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	for (unsigned int i = 0; i < normals.size(); ++i)
+	{
+		const pcl::Normal & pt = normals.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), 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 pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(cloud.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointXYZRGBNormal & pt = cloud.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), 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 pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float searchRadius,

corelib/src/util3d_transforms.cpp

@@ -38,61 +38,82 @@ namespace util3d
 
 cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 
 	cv::Mat output = laserScan.clone();
 
 	if(!transform.isNull() && !transform.isIdentity())
 	{
+		Eigen::Affine3f transform3f = transform.toEigen3f();
 		for(int i=0; i<laserScan.cols; ++i)
 		{
+			const float * ptr = laserScan.ptr<float>(0, i);
+			float * out = output.ptr<float>(0, i);
 			if(laserScan.type() == CV_32FC2)
 			{
-				pcl::PointXYZ pt(
+				pcl::PointXYZ pt(ptr[0], ptr[1], 0);
-						laserScan.at<cv::Vec2f>(i)[0],
+				pt = pcl::transformPoint(pt, transform3f);
-						laserScan.at<cv::Vec2f>(i)[1], 0);
+				out[0] = pt.x;
-				pt = util3d::transformPoint(pt, transform);
+				out[1] = pt.y;
-				output.at<cv::Vec2f>(i)[0] = pt.x;
-				output.at<cv::Vec2f>(i)[1] = pt.y;
 			}
-			else if(laserScan.type() == CV_32FC3)
+			else if(laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4))
 			{
-				pcl::PointXYZ pt(
+				const float * ptr = laserScan.ptr<float>(0, i);
-						laserScan.at<cv::Vec3f>(i)[0],
+				pcl::PointXYZ pt(ptr[0], ptr[1], ptr[2]);
-						laserScan.at<cv::Vec3f>(i)[1],
+				pt = pcl::transformPoint(pt, transform3f);
-						laserScan.at<cv::Vec3f>(i)[2]);
+				out[0] = pt.x;
-				pt = util3d::transformPoint(pt, transform);
+				out[1] = pt.y;
-				output.at<cv::Vec3f>(i)[0] = pt.x;
+				out[2] = pt.z;
-				output.at<cv::Vec3f>(i)[1] = pt.y;
-				output.at<cv::Vec3f>(i)[2] = pt.z;
 			}
-			else if(laserScan.type() == CV_32FC(4))
+			else if(laserScan.type() == CV_32FC(5))
-			{
-				pcl::PointXYZ pt(
-						laserScan.at<cv::Vec4f>(i)[0],
-						laserScan.at<cv::Vec4f>(i)[1],
-						laserScan.at<cv::Vec4f>(i)[2]);
-				pt = util3d::transformPoint(pt, transform);
-				output.at<cv::Vec4f>(i)[0] = pt.x;
-				output.at<cv::Vec4f>(i)[1] = pt.y;
-				output.at<cv::Vec4f>(i)[2] = pt.z;
-			}
-			else
 			{
 				pcl::PointNormal pt;
-				pt.x=laserScan.at<cv::Vec6f>(i)[0];
+				pt.x=ptr[0];
-				pt.y=laserScan.at<cv::Vec6f>(i)[1];
+				pt.y=ptr[1];
-				pt.z=laserScan.at<cv::Vec6f>(i)[2];
+				pt.z=0;
-				pt.normal_x=laserScan.at<cv::Vec6f>(i)[3];
+				pt.normal_x=ptr[2];
-				pt.normal_y=laserScan.at<cv::Vec6f>(i)[4];
+				pt.normal_y=ptr[3];
-				pt.normal_z=laserScan.at<cv::Vec6f>(i)[5];
+				pt.normal_z=ptr[4];
 				pt = util3d::transformPoint(pt, transform);
-				output.at<cv::Vec6f>(i)[0] = pt.x;
+				out[0] = pt.x;
-				output.at<cv::Vec6f>(i)[1] = pt.y;
+				out[1] = pt.y;
-				output.at<cv::Vec6f>(i)[2] = pt.z;
+				out[2] = pt.normal_x;
-				output.at<cv::Vec6f>(i)[3] = pt.normal_x;
+				out[3] = pt.normal_y;
-				output.at<cv::Vec6f>(i)[4] = pt.normal_y;
+				out[4] = pt.normal_z;
-				output.at<cv::Vec6f>(i)[5] = pt.normal_z;
+			}
+			else if(laserScan.type() == CV_32FC(6))
+			{
+				pcl::PointNormal pt;
+				pt.x=ptr[0];
+				pt.y=ptr[1];
+				pt.z=ptr[2];
+				pt.normal_x=ptr[3];
+				pt.normal_y=ptr[4];
+				pt.normal_z=ptr[5];
+				pt = util3d::transformPoint(pt, transform);
+				out[0] = pt.x;
+				out[1] = pt.y;
+				out[2] = pt.z;
+				out[3] = pt.normal_x;
+				out[4] = pt.normal_y;
+				out[5] = pt.normal_z;
+			}
+			else // 7 channels
+			{
+				pcl::PointNormal pt;
+				pt.x=ptr[0];
+				pt.y=ptr[1];
+				pt.z=ptr[2];
+				pt.normal_x=ptr[4];
+				pt.normal_y=ptr[5];
+				pt.normal_z=ptr[6];
+				pt = util3d::transformPoint(pt, transform);
+				out[0] = pt.x;
+				out[1] = pt.y;
+				out[2] = pt.z;
+				out[4] = pt.normal_x;
+				out[5] = pt.normal_y;
+				out[6] = pt.normal_z;
 			}
 		}
 	}
@@ -189,7 +210,9 @@ pcl::PointXYZRGB transformPoint(
 		const pcl::PointXYZRGB & pt,
 		const Transform & transform)
 {
-	return pcl::transformPoint(pt, transform.toEigen3f());
+	pcl::PointXYZRGB ptRGB = pcl::transformPoint(pt, transform.toEigen3f());
+	ptRGB.rgb = pt.rgb;
+	return ptRGB;
 }
 pcl::PointNormal transformPoint(
 		const pcl::PointNormal & point,
@@ -223,6 +246,8 @@ pcl::PointXYZRGBNormal transformPoint(
 	ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
 	ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
 	ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
+
+	ret.rgb = point.rgb;
 	return ret;
 }
 

guilib/include/rtabmap/gui/CloudViewer.h

@@ -274,8 +274,14 @@ public:
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
 	void setGridShown(bool shown);
+	void setNormalsShown(bool shown);
 	void setGridCellCount(unsigned int count);
 	void setGridCellSize(float size);
+	bool isNormalsShown() const;
+	int getNormalsStep() const;
+	float getNormalsScale() const;
+	void setNormalsStep(int step);
+	void setNormalsScale(float scale);
 
 public slots:
 	void setDefaultBackgroundColor(const QColor & color);
@@ -318,6 +324,9 @@ private:
     QAction * _aShowGrid;
     QAction * _aSetGridCellCount;
     QAction * _aSetGridCellSize;
+    QAction * _aShowNormals;
+	QAction * _aSetNormalsStep;
+	QAction * _aSetNormalsScale;
     QAction * _aSetBackgroundColor;
     QAction * _aSetRenderingRate;
     QAction * _aSetLighting;
@@ -336,6 +345,8 @@ private:
     QColor _frustumColor;
     unsigned int _gridCellCount;
     float _gridCellSize;
+    int _normalsStep;
+    float _normalsScale;
     cv::Vec3d _lastCameraOrientation;
     cv::Vec3d _lastCameraPose;
     QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -164,9 +164,11 @@ public:
 	double getCeilingFilteringHeight() const;
 	double getFloorFilteringHeight() const;
 	int getNormalKSearch() const;
+	double getNormalRadiusSearch() const;
 	double getScanCeilingFilteringHeight() const;
 	double getScanFloorFilteringHeight() const;
 	int getScanNormalKSearch() const;
+	double getScanNormalRadiusSearch() const;
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	bool isOctomapUpdated() const;
 	bool isOctomapShown() const;
@@ -241,6 +243,7 @@ public:
 	double getSourceScanFromDepthMaxDepth() const;
 	double getSourceScanVoxelSize() const;
 	int getSourceScanNormalsK() const;
+	double getSourceScanNormalsRadius() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
 	Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null
@@ -298,7 +301,6 @@ private slots:
 	void addParameter(double value);
 	void addParameter(const QString & value);
 	void updatePredictionPlot();
-	void updateOdometryVisibility();
 	void updateKpROI();
 	void updateStereoDisparityVisibility();
 	void useOdomFeatures();

guilib/src/CloudViewer.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/CloudViewer.h"
 
 #include <rtabmap/core/Version.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
@@ -190,6 +191,9 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_aShowGrid(0),
 		_aSetGridCellCount(0),
 		_aSetGridCellSize(0),
+		_aShowNormals(0),
+		_aSetNormalsStep(0),
+		_aSetNormalsScale(0),
 		_aSetBackgroundColor(0),
 		_aSetRenderingRate(0),
 		_aSetLighting(0),
@@ -203,6 +207,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_frustumColor(Qt::gray),
 		_gridCellCount(50),
 		_gridCellSize(1),
+		_normalsStep(1),
+		_normalsScale(0.2),
 		_lastCameraOrientation(0,0,0),
 		_lastCameraPose(0,0,0),
 		_defaultBgColor(Qt::black),
@@ -310,6 +316,10 @@ void CloudViewer::createMenu()
 	_aShowGrid->setCheckable(true);
 	_aSetGridCellCount = new QAction("Set cell count...", this);
 	_aSetGridCellSize = new QAction("Set cell size...", this);
+	_aShowNormals = new QAction("Show normals", this);
+	_aShowNormals->setCheckable(true);
+	_aSetNormalsStep = new QAction("Set normals step...", this);
+	_aSetNormalsScale = new QAction("Set normals scale...", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);	
 	_aSetRenderingRate = new QAction("Set rendering rate...", this);
 	_aSetLighting = new QAction("Lighting", this);
@@ -352,12 +362,18 @@ void CloudViewer::createMenu()
 	gridMenu->addAction(_aSetGridCellCount);
 	gridMenu->addAction(_aSetGridCellSize);
 
+	QMenu * normalsMenu = new QMenu("Normals", this);
+	normalsMenu->addAction(_aShowNormals);
+	normalsMenu->addAction(_aSetNormalsStep);
+	normalsMenu->addAction(_aSetNormalsScale);
+
 	//menus
 	_menu = new QMenu(this);
 	_menu->addMenu(cameraMenu);
 	_menu->addMenu(trajectoryMenu);
 	_menu->addMenu(frustumMenu);
 	_menu->addMenu(gridMenu);
+	_menu->addMenu(normalsMenu);
 	_menu->addAction(_aSetBackgroundColor);
 	_menu->addAction(_aSetRenderingRate);
 	_menu->addAction(_aSetLighting);
@@ -400,6 +416,10 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("grid_cell_count", this->getGridCellCount());
 	settings.setValue("grid_cell_size", (double)this->getGridCellSize());
 
+	settings.setValue("normals", this->isNormalsShown());
+	settings.setValue("normals_step", this->getNormalsStep());
+	settings.setValue("normals_scale", (double)this->getNormalsScale());
+
 	settings.setValue("trajectory_shown", this->isTrajectoryShown());
 	settings.setValue("trajectory_size", this->getTrajectorySize());
 
@@ -439,6 +459,10 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setGridCellCount(settings.value("grid_cell_count", this->getGridCellCount()).toUInt());
 	this->setGridCellSize(settings.value("grid_cell_size", this->getGridCellSize()).toFloat());
 
+	this->setNormalsShown(settings.value("normals", this->isNormalsShown()).toBool());
+	this->setNormalsStep(settings.value("normals_step", this->getNormalsStep()).toInt());
+	this->setNormalsScale(settings.value("normals_scale", this->getNormalsScale()).toFloat());
+
 	this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
 	this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
 
@@ -472,11 +496,22 @@ bool CloudViewer::updateCloudPose(
 {
 	if(_addedClouds.contains(id))
 	{
-		UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
+		//UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
-		if(_addedClouds.find(id).value() == pose ||
+		bool samePose = _addedClouds.find(id).value() == pose;
-		   _visualizer->updatePointCloudPose(id, pose.toEigen3f()))
+		Eigen::Affine3f posef = pose.toEigen3f();
+		if(samePose ||
+		   _visualizer->updatePointCloudPose(id, posef))
 		{
 			_addedClouds.find(id).value() = pose;
+			if(!samePose)
+			{
+				std::string idNormals = id+"-normals";
+				if(_addedClouds.find(idNormals)!=_addedClouds.end())
+				{
+					_visualizer->updatePointCloudPose(idNormals, posef);
+					_addedClouds.find(idNormals).value() = pose;
+				}
+			}
 			return true;
 		}
 	}
@@ -501,6 +536,18 @@ bool CloudViewer::addCloud(
 	Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
 	Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().rotation());
 
+	if(haveNormals && _aShowNormals->isChecked())
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal>);
+		pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz);
+		std::string idNormals = id + "-normals";
+		if(_visualizer->addPointCloudNormals<pcl::PointNormal>(cloud_xyz, _normalsStep, _normalsScale, idNormals, 0))
+		{
+			_visualizer->updatePointCloudPose(idNormals, pose.toEigen3f());
+			_addedClouds.insert(idNormals, pose);
+		}
+	}
+
 	// add random color channel
 	 pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
 	 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
@@ -1626,7 +1673,9 @@ void CloudViewer::removeAllClouds()
 bool CloudViewer::removeCloud(const std::string & id)
 {
 	bool success = _visualizer->removePointCloud(id);
+	_visualizer->removePointCloud(id+"-normals");
 	_addedClouds.remove(id); // remove after visualizer
+	_addedClouds.remove(id+"-normals");
 	return success;
 }
 
@@ -1929,6 +1978,12 @@ void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
 	if(iter != cloudActorMap->end())
 	{
 		iter->second.actor->SetVisibility(isVisible?1:0);
+
+		iter = cloudActorMap->find(id+"-normals");
+		if(iter != cloudActorMap->end())
+		{
+			iter->second.actor->SetVisibility(isVisible&&_aShowNormals->isChecked()?1:0);
+		}
 	}
 	else
 	{
@@ -1992,20 +2047,6 @@ void CloudViewer::setCameraLockZ(bool enabled)
 	_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
 	_aLockViewZ->setChecked(enabled);
 }
-
-void CloudViewer::setGridShown(bool shown)
-{
-	_aShowGrid->setChecked(shown);
-	if(shown)
-	{
-		this->addGrid();
-	}
-	else
-	{
-		this->removeGrid();
-	}
-}
-
 bool CloudViewer::isCameraTargetLocked() const
 {
 	return _aLockCamera->isChecked();
@@ -2022,6 +2063,23 @@ bool CloudViewer::isCameraLockZ() const
 {
 	return _aLockViewZ->isChecked();
 }
+double CloudViewer::getRenderingRate() const
+{
+	return _renderingRate;
+}
+
+void CloudViewer::setGridShown(bool shown)
+{
+	_aShowGrid->setChecked(shown);
+	if(shown)
+	{
+		this->addGrid();
+	}
+	else
+	{
+		this->removeGrid();
+	}
+}
 bool CloudViewer::isGridShown() const
 {
 	return _aShowGrid->isChecked();
@@ -2034,11 +2092,6 @@ float CloudViewer::getGridCellSize() const
 {
 	return _gridCellSize;
 }
-double CloudViewer::getRenderingRate() const
-{
-	return _renderingRate;
-}
-
 void CloudViewer::setGridCellCount(unsigned int count)
 {
 	if(count > 0)
@@ -2110,6 +2163,54 @@ void CloudViewer::removeGrid()
 	_gridLines.clear();
 }
 
+void CloudViewer::setNormalsShown(bool shown)
+{
+	_aShowNormals->setChecked(shown);
+	QList<std::string> ids = _addedClouds.keys();
+	for(QList<std::string>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+	{
+		std::string idNormals = *iter + "-normals";
+		if(_addedClouds.find(idNormals) != _addedClouds.end())
+		{
+			this->setCloudVisibility(idNormals, this->getCloudVisibility(*iter) && shown);
+		}
+	}
+}
+bool CloudViewer::isNormalsShown() const
+{
+	return _aShowNormals->isChecked();
+}
+int CloudViewer::getNormalsStep() const
+{
+	return _normalsStep;
+}
+float CloudViewer::getNormalsScale() const
+{
+	return _normalsScale;
+}
+void CloudViewer::setNormalsStep(int step)
+{
+	if(step > 0)
+	{
+		_normalsStep = step;
+	}
+	else
+	{
+		UERROR("Cannot set normals step <= 0, step=%d", step);
+	}
+}
+void CloudViewer::setNormalsScale(float scale)
+{
+	if(scale > 0)
+	{
+		_normalsScale= scale;
+	}
+	else
+	{
+		UERROR("Cannot set normals scale <= 0, value=%f", scale);
+	}
+}
+
 Eigen::Vector3f rotatePointAroundAxe(
 		const Eigen::Vector3f & point,
 		const Eigen::Vector3f & axis,
@@ -2405,6 +2506,29 @@ void CloudViewer::handleAction(QAction * a)
 			this->setGridCellSize(value);
 		}
 	}
+	else if(a == _aShowNormals)
+	{
+		this->setNormalsShown(_aShowNormals->isChecked());
+		this->update();
+	}
+	else if(a == _aSetNormalsStep)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set normals step"), tr("Step"), _normalsStep, 1, 10000, 1, &ok);
+		if(ok)
+		{
+			this->setNormalsStep(value);
+		}
+	}
+	else if(a == _aSetNormalsScale)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set normals scale"), tr("Scale (m)"), _normalsScale, 0.01, 10, 2, &ok);
+		if(ok)
+		{
+			this->setNormalsScale(value);
+		}
+	}
 	else if(a == _aSetBackgroundColor)
 	{
 		QColor color = this->getDefaultBackgroundColor();

guilib/src/DatabaseViewer.cpp

@@ -2469,7 +2469,8 @@ void DatabaseViewer::update(int value,
 				labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
 				if(s!=0.0)
 				{
-					stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
+					stamp->setText(QString::number(s, 'f'));
+					stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
 				}
 				if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
 				{
@@ -2697,7 +2698,16 @@ void DatabaseViewer::update(int value,
 					//add scan
 					if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().cols)
 					{
-						if(data.laserScanRaw().channels() == 6)
+						if(data.laserScanRaw().channels() == 7)
+						{
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanInfo().localTransform());
+							if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+							{
+								scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+							}
+							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
+						}
+						else if(data.laserScanRaw().channels() == 6)
 						{
 							pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanInfo().localTransform());
 							if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
@@ -2706,6 +2716,15 @@ void DatabaseViewer::update(int value,
 							}
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
+						else if(data.laserScanRaw().channels() == 4)
+						{
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanInfo().localTransform());
+							if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+							{
+								scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+							}
+							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
+						}
 						else
 						{
 							pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
@@ -3757,7 +3776,7 @@ void DatabaseViewer::updateConstraintView(
 			constraintsViewer_->removeCloud("scan1");
 			if(!dataFrom.laserScanRaw().empty())
 			{
-				if(dataFrom.laserScanRaw().channels() == 6)
+				if(dataFrom.laserScanRaw().channels() >= 5)
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr scan;
 					scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
@@ -3780,7 +3799,7 @@ void DatabaseViewer::updateConstraintView(
 			}
 			if(!dataTo.laserScanRaw().empty())
 			{
-				if(dataTo.laserScanRaw().channels() == 6)
+				if(dataTo.laserScanRaw().channels() >= 5)
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr scan;
 					scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
@@ -3804,8 +3823,8 @@ void DatabaseViewer::updateConstraintView(
 		}
 
 		//update coordinate
-
 		constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2);
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
 		constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2);
 		constraintsViewer_->removeCoordinate("to_coordinate_gt");
 		if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
@@ -3813,6 +3832,7 @@ void DatabaseViewer::updateConstraintView(
 			constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
 					pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1);
 		}
+#endif
 
 		constraintsViewer_->clearTrajectory();
 
@@ -4020,19 +4040,19 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				UINFO("rotational_min=%f", rotational_min);
 				UINFO("rotational_max=%f", rotational_max);
 
-				ui_->toolBox_statistics->updateStat("GT/translational_rmse/", translational_rmse, false);
+				ui_->toolBox_statistics->updateStat("GT/translational rmse/", translational_rmse, false);
-				ui_->toolBox_statistics->updateStat("GT/translational_mean/", translational_mean, false);
+				ui_->toolBox_statistics->updateStat("GT/translational mean/", translational_mean, false);
-				ui_->toolBox_statistics->updateStat("GT/translational_median/", translational_median, false);
+				ui_->toolBox_statistics->updateStat("GT/translational median/", translational_median, false);
-				ui_->toolBox_statistics->updateStat("GT/translational_std/", translational_std, false);
+				ui_->toolBox_statistics->updateStat("GT/translational std/", translational_std, false);
-				ui_->toolBox_statistics->updateStat("GT/translational_min/", translational_min, false);
+				ui_->toolBox_statistics->updateStat("GT/translational min/", translational_min, false);
-				ui_->toolBox_statistics->updateStat("GT/translational_max/", translational_max, false);
+				ui_->toolBox_statistics->updateStat("GT/translational max/", translational_max, false);
 
-				ui_->toolBox_statistics->updateStat("GT/rotational_rmse/", rotational_rmse, false);
+				ui_->toolBox_statistics->updateStat("GT/rotational rmse/", rotational_rmse, false);
-				ui_->toolBox_statistics->updateStat("GT/rotational_mean/", rotational_mean, false);
+				ui_->toolBox_statistics->updateStat("GT/rotational mean/", rotational_mean, false);
-				ui_->toolBox_statistics->updateStat("GT/rotational_median/", rotational_median, false);
+				ui_->toolBox_statistics->updateStat("GT/rotational median/", rotational_median, false);
-				ui_->toolBox_statistics->updateStat("GT/rotational_std/", rotational_std, false);
+				ui_->toolBox_statistics->updateStat("GT/rotational std/", rotational_std, false);
-				ui_->toolBox_statistics->updateStat("GT/rotational_min/", rotational_min, false);
+				ui_->toolBox_statistics->updateStat("GT/rotational min/", rotational_min, false);
-				ui_->toolBox_statistics->updateStat("GT/rotational_max/", rotational_max, false);
+				ui_->toolBox_statistics->updateStat("GT/rotational max/", rotational_max, false);
 			}
 		}
 

guilib/src/EditDepthArea.h

@@ -57,12 +57,12 @@ public slots:
     void resetChanges();
 
 protected:
-    void mousePressEvent(QMouseEvent *event) override;
+    virtual void mousePressEvent(QMouseEvent *event);
-    void mouseMoveEvent(QMouseEvent *event) override;
+    virtual void mouseMoveEvent(QMouseEvent *event);
-    void mouseReleaseEvent(QMouseEvent *event) override;
+    virtual void mouseReleaseEvent(QMouseEvent *event);
-    void paintEvent(QPaintEvent *event) override;
+    virtual void paintEvent(QPaintEvent *event);
-    void resizeEvent(QResizeEvent *event) override;
+    virtual void resizeEvent(QResizeEvent *event);
-    void contextMenuEvent(QContextMenuEvent * e) override;
+    virtual void contextMenuEvent(QContextMenuEvent * e);
 
 private:
     void drawLineTo(const QPoint &endPoint);

guilib/src/ExportCloudsDialog.cpp

@@ -88,6 +88,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_fromDepth, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_normalRadiusSearch, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
@@ -255,6 +256,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("from_depth", _ui->checkBox_fromDepth->isChecked());
 	settings.setValue("binary", _ui->checkBox_binary->isChecked());
 	settings.setValue("normals_k", _ui->spinBox_normalKSearch->value());
+	settings.setValue("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value());
 
 	settings.setValue("regenerate", _ui->checkBox_regenerate->isChecked());
 	settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
@@ -371,6 +373,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->checkBox_fromDepth->setChecked(settings.value("from_depth", _ui->checkBox_fromDepth->isChecked()).toBool());
 	_ui->checkBox_binary->setChecked(settings.value("binary", _ui->checkBox_binary->isChecked()).toBool());
 	_ui->spinBox_normalKSearch->setValue(settings.value("normals_k", _ui->spinBox_normalKSearch->value()).toInt());
+	_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
 
 	_ui->checkBox_regenerate->setChecked(settings.value("regenerate", _ui->checkBox_regenerate->isChecked()).toBool());
 	_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
@@ -487,6 +490,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->checkBox_fromDepth->setChecked(true);
 	_ui->checkBox_binary->setChecked(true);
 	_ui->spinBox_normalKSearch->setValue(20);
+	_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
 
 	_ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false);
 	_ui->spinBox_decimation->setValue(1);
@@ -1384,7 +1388,7 @@ bool ExportCloudsDialog::getExportedClouds(
 				// recompute normals
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value());
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value());
 
 				UASSERT(assembledCloud->size() == normals->size());
 				for(unsigned int i=0; i<normals->size(); ++i)
@@ -2521,7 +2525,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							viewPoint[2] = data.stereoCameraModel().localTransform().z();
 						}
 
-						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 						pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 
 						if(_ui->checkBox_subtraction->isChecked() &&
@@ -2592,7 +2596,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 						}
 						else
 						{
-							normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+							normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 						}
 						pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 					}
@@ -2673,7 +2677,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 					_progressDialog->appendText(tr("Cached cloud %1 is not found in cached data, the view point for normal computation will not be set (%2/%3).").arg(iter->first).arg(index).arg(poses.size()), Qt::darkYellow);
 				}
 
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 				pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 			}
 			else if(!_ui->checkBox_fromDepth->isChecked() && uContains(cachedScans, iter->first))
@@ -2731,7 +2735,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 					}
 					else
 					{
-						normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+						normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 					}
 					pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 				}

guilib/src/GraphViewer.cpp

@@ -673,7 +673,7 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
 			std::map<int,float>::const_iterator jter = posterior.find(iter.key());
 			if(jter != posterior.end())
 			{
-				UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f);
+				//UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f);
 				iter.value()->setColor(QColor::fromHsvF((1-jter->second/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
 			}
 			else

guilib/src/MainWindow.cpp

@@ -560,6 +560,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->updateStat("Odometry/Inliers/", false);
 	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
 	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
+	_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false);
+	_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false);
+	_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
 	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
 	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
 	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
@@ -812,7 +815,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
 			if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
 			{
 				OdometryInfo odomInfo;
-				odomInfo.covariance = cameraEvent->info().odomCovariance;
+				odomInfo.reg.covariance = cameraEvent->info().odomCovariance;
 				if (!_processingOdometry && !_processingStatistics)
 				{
 					_processingOdometry = true; // if we receive too many odometry events!
@@ -927,11 +930,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		pose = _lastOdomPose;
 		lost = true;
 	}
-	else if(odom.info().inliers>0 &&
+	else if(odom.info().reg.inliers>0 &&
 			_preferencesDialog->getOdomQualityWarnThr() &&
-			odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
+			odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr())
 	{
-		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr());
 		lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed;
 		_cloudViewer->setBackgroundColor(Qt::darkYellow);
 		_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
@@ -1202,9 +1205,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				if(_preferencesDialog->isOdomOnlyInliersShown())
 				{
 					std::multimap<int, cv::KeyPoint> kpInliers;
-					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
 					{
-						kpInliers.insert(*odom.info().words.find(odom.info().wordInliers[i]));
+						kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
 					}
 					_ui->imageView_odometry->setFeatures(
 							kpInliers,
@@ -1271,13 +1274,13 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			{
 				if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
 				{
-					for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
 					{
-						_ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
+						_ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
 					}
-					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
 					{
-						_ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
+						_ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
 					}
 				}
 			}
@@ -1323,15 +1326,18 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	}
 
 	//Process info
-	_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
@@ -2651,9 +2657,9 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 					if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
 					{
 						//normals required
-						if(_preferencesDialog->getNormalKSearch() > 0)
+						if(_preferencesDialog->getNormalKSearch() > 0 || _preferencesDialog->getNormalRadiusSearch() > 0)
 						{
-							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
+							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
 							pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
 						}
 						else
@@ -2790,7 +2796,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 
 				if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
 				{
-					pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
+					pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
 					pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
 				}
 
@@ -2880,22 +2886,48 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(0));
 		}
 
-		if(scan.channels() == 6)
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
+		if(scan.channels() == 7 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
 		{
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, iter->sensorData().laserScanInfo().localTransform());
-			cloud = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
-			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+		else if((scan.channels() == 5 || scan.channels() == 6) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
+		{
+			cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
+		else if(scan.channels() == 4)
+		{
+			cloudRGB = util3d::laserScanToPointCloudRGB(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
+		else
+		{
+			cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
+
+		if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+		{
+			if(cloud.get())
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
 			}
+			if(cloudRGB.get())
+			{
+				cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0));
+			}
+		}
 
-			// Do ceiling/floor filtering
+		// Do ceiling/floor filtering
-			if(cloud->size() &&
+		if((scan.channels() > 2 && scan.channels() != 5) && // don't filter 2D scans
-			   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
+		   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
-			   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
+		   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
+		{
+			if(cloudRGBWithNormals.get())
 			{
 				// perform in /map frame
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGBWithNormals, pose);
 				cloudTransformed = rtabmap::util3d::passThrough(
 						cloudTransformed,
 						"z",
@@ -2903,53 +2935,35 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
 
 				//transform back in sensor frame
-				cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
+				cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
 			}
+			if(cloudWithNormals.get())
+			{
+				// perform in /map frame
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose);
+				cloudTransformed = rtabmap::util3d::passThrough(
+						cloudTransformed,
+						"z",
+						_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
+						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
 
-			QColor color = Qt::gray;
+				//transform back in sensor frame
-			if(mapId >= 0)
+				cloudWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
-			{
-				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 			}
-			if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
+			if(cloudRGB.get())
 			{
-				UERROR("Adding cloud %d to viewer failed!", nodeId);
+				// perform in /map frame
-			}
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose);
-			else
+				cloudTransformed = rtabmap::util3d::passThrough(
-			{
+						cloudTransformed,
-				if(nodeId > 0)
+						"z",
-				{
+						_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
-					if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
-					{
-						//reconvert the voxelized cloud
-						scan = util3d::laserScanFromPointCloud(*cloud);
-					}
-					else
-					{
-						scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
-					}
-					_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
-				}
-				_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
-				_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
-			}
-		}
-		else
-		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-			cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
-			bool filtered = false;
-			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
-			{
-				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
-				filtered = true;
-			}
 
-			// Do ceiling/floor filtering
+				//transform back in sensor frame
-			if(scan.channels() > 2 && // don't filter 2D scans
+				cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse());
-				cloud->size() &&
+			}
-			   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
+			if(cloud.get())
-			   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
 			{
 				// perform in /map frame
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
@@ -2961,78 +2975,109 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 
 				//transform back in sensor frame
 				cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
-				filtered = true;
 			}
+		}
 
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
+		if(	(cloud.get() || cloudRGB.get()) &&
-			if(scan.channels() > 2 && // don't compute normals for 2D scans
+		   (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
-				cloud->size() &&
+		{
-			   _preferencesDialog->getScanNormalKSearch() > 0)
+			Eigen::Vector3f scanViewpoint(
-			{
+				iter->sensorData().laserScanInfo().localTransform().x(),
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch());
+				iter->sensorData().laserScanInfo().localTransform().y(),
-				cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
+				iter->sensorData().laserScanInfo().localTransform().z());
-				pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
-				filtered = true;
-			}
 
-			QColor color = Qt::gray;
+			pcl::PointCloud<pcl::Normal>::Ptr normals;
-			if(mapId >= 0)
+			if(cloud.get() && cloud->size())
 			{
-				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+				if(scan.channels() == 2 || scan.channels() == 5)
-			}
-			if(cloudWithNormals.get())
-			{
-				if(!_cloudViewer->addCloud(scanName, cloudWithNormals, pose, color))
 				{
-					UERROR("Adding cloud %d to viewer failed!", nodeId);
+					normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
 				}
 				else
 				{
-					if(nodeId > 0)
+					normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
-					{
-						//reconvert the voxelized cloud
-						scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
-						_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
-					}
-
-					_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
-					_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 				}
+				cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
+				cloud.reset();
 			}
 			else
 			{
-				if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
+				UASSERT(cloudRGB.get() && cloudRGB->size()); // Assuming 4 channels cannot be 2D
+				normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
+				cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+				pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
+				cloudRGB.reset();
+			}
+		}
+
+		QColor color = Qt::gray;
+		if(mapId >= 0)
+		{
+			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+		}
+		bool added = false;
+		if(cloudRGBWithNormals.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
+			if(added && nodeId > 0)
+			{
+				scan = util3d::laserScanFromPointCloud(*cloudRGBWithNormals);
+			}
+		}
+		else if(cloudWithNormals.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
+			if(added && nodeId > 0)
+			{
+				if(scan.channels() == 5)
 				{
-					UERROR("Adding cloud %d to viewer failed!", nodeId);
+					scan = util3d::laserScan2dFromPointCloud(*cloudWithNormals);
 				}
 				else
 				{
-					if(nodeId > 0)
+					scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
-					{
-						if(filtered)
-						{
-							//reconvert the voxelized cloud
-							if(scan.channels() == 2)
-							{
-								scan = util3d::laserScan2dFromPointCloud(*cloud);
-							}
-							else
-							{
-								scan = util3d::laserScanFromPointCloud(*cloud);
-							}
-						}
-						else
-						{
-							scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
-						}
-						_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
-					}
-
-					_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
-					_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 				}
 			}
 		}
+		else if(cloudRGB.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
+			if(added && nodeId > 0)
+			{
+				scan = util3d::laserScanFromPointCloud(*cloudRGB);
+			}
+		}
+		else
+		{
+			UASSERT(cloud.get());
+			added = _cloudViewer->addCloud(scanName, cloud, pose, color);
+			if(added && nodeId > 0)
+			{
+				if(scan.channels() == 2)
+				{
+					scan = util3d::laserScan2dFromPointCloud(*cloud);
+				}
+				else
+				{
+					scan = util3d::laserScanFromPointCloud(*cloud);
+				}
+			}
+		}
+		if(!added)
+		{
+			UERROR("Adding cloud %d to viewer failed!", nodeId);
+		}
+		else
+		{
+			if(nodeId > 0)
+			{
+				_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
+			}
+
+			_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
+			_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
+		}
 	}
 }
 
@@ -3269,35 +3314,35 @@ Transform MainWindow::alignPosesToGroundTruth(
 
 			if((_preferencesDialog->isTimeUsedInFigures() && stamp > 0.0) || (refId && refId>=0))
 			{
-				_ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures());
 
-				_ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures());
 			}
 			else
 			{
-				_ui->statsToolBox->updateStat("GT/translational_rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_median/", translational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational median/", translational_median, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_std/", translational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational std/", translational_std, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_min/", translational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational min/", translational_min, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/translational_max/", translational_max, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational max/", translational_max, _preferencesDialog->isCacheSavedInFigures());
 
-				_ui->statsToolBox->updateStat("GT/rotational_rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_median/", rotational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational median/", rotational_median, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_std/", rotational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational std/", rotational_std, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_min/", rotational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational min/", rotational_min, _preferencesDialog->isCacheSavedInFigures());
-				_ui->statsToolBox->updateStat("GT/rotational_max/", rotational_max, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational max/", rotational_max, _preferencesDialog->isCacheSavedInFigures());
 			}
 		}
 	}
@@ -3308,43 +3353,58 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 {
 	UINFO("Update visibility %d", nodeId);
 	QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
-	if(_preferencesDialog->isCloudsShown(0))
+	Transform pose;
+	if(_currentGTPosesMap.size() &&
+		_ui->actionAnchor_clouds_to_ground_truth->isChecked() &&
+		_currentGTPosesMap.find(nodeId)!=_currentGTPosesMap.end())
 	{
-		std::string cloudName = uFormat("cloud%d", nodeId);
+		pose = _currentGTPosesMap.at(nodeId);
-		if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
+	}
-		{
+	else if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
-			createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
+	{
-		}
+		pose = _currentPosesMap.at(nodeId);
-		else if(viewerClouds.contains(cloudName))
-		{
-			if(visible && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
-			{
-				//make sure the transformation was done
-				_cloudViewer->updateCloudPose(cloudName, _currentPosesMap.find(nodeId)->second);
-			}
-			_cloudViewer->setCloudVisibility(cloudName, visible);
-		}
 	}
 
-	if(_preferencesDialog->isScansShown(0))
+	if(!pose.isNull() || !visible)
 	{
-		std::string scanName = uFormat("scan%d", nodeId);
+		if(_preferencesDialog->isCloudsShown(0))
-		if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
 		{
-			createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
+			std::string cloudName = uFormat("cloud%d", nodeId);
-		}
+			if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
-		else if(viewerClouds.contains(scanName))
-		{
-			if(visible && _currentPosesMap.find(nodeId) != _currentPosesMap.end())
 			{
-				//make sure the transformation was done
+				createAndAddCloudToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
-				_cloudViewer->updateCloudPose(scanName, _currentPosesMap.find(nodeId)->second);
+			}
+			else if(viewerClouds.contains(cloudName))
+			{
+				if(visible)
+				{
+					//make sure the transformation was done
+					_cloudViewer->updateCloudPose(cloudName, pose);
+				}
+				_cloudViewer->setCloudVisibility(cloudName, visible);
 			}
-			_cloudViewer->setCloudVisibility(scanName, visible);
 		}
-	}
 
-	_cloudViewer->update();
+		if(_preferencesDialog->isScansShown(0))
+		{
+			std::string scanName = uFormat("scan%d", nodeId);
+			if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
+			{
+				createAndAddScanToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1));
+			}
+			else if(viewerClouds.contains(scanName))
+			{
+				if(visible)
+				{
+					//make sure the transformation was done
+					_cloudViewer->updateCloudPose(scanName, pose);
+				}
+				_cloudViewer->setCloudVisibility(scanName, visible);
+			}
+		}
+
+		_cloudViewer->update();
+	}
 }
 
 void MainWindow::updateGraphView()
@@ -4496,7 +4556,8 @@ void MainWindow::startDetection()
 			_preferencesDialog->getSourceScanFromDepthDecimation(),
 			_preferencesDialog->getSourceScanFromDepthMaxDepth(),
 			_preferencesDialog->getSourceScanVoxelSize(),
-			_preferencesDialog->getSourceScanNormalsK());
+			_preferencesDialog->getSourceScanNormalsK(),
+			_preferencesDialog->getSourceScanNormalsRadius());
 	if(_preferencesDialog->isDepthFilteringAvailable())
 	{
 		if(_preferencesDialog->isBilateralFiltering())
@@ -7046,6 +7107,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		if(_camera)
 		{
 			_camera->start();
+			ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 		}
 		break;
 
@@ -7077,6 +7139,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			if(_camera)
 			{
 				_camera->start();
+				ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 			}
 		}
 		else if(_state == kDetecting)

guilib/src/OdometryViewer.cpp

@@ -179,7 +179,7 @@ void OdometryViewer::clear()
 void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 {
 	processingData_ = true;
-	int quality = odom.info().inliers;
+	int quality = odom.info().reg.inliers;
 
 	bool lost = false;
 	bool lostStateChanged = false;
@@ -193,11 +193,11 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 
 		lost = true;
 	}
-	else if(odom.info().inliers>0 &&
+	else if(odom.info().reg.inliers>0 &&
 			qualityWarningThr_ &&
-			odom.info().inliers < qualityWarningThr_)
+			odom.info().reg.inliers < qualityWarningThr_)
 	{
-		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_);
 		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
 		imageView_->setBackgroundColor(Qt::darkYellow);
 		cloudView_->setBackgroundColor(Qt::darkYellow);
@@ -425,13 +425,13 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 			{
 				if(imageView_->isFeaturesShown())
 				{
-					for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
 					{
-						imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
+						imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
 					}
-					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
 					{
-						imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
+						imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
 					}
 				}
 			}

guilib/src/PreferencesDialog.cpp

@@ -211,6 +211,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->loopClosure_bundle->setItemData(1, 0, Qt::UserRole - 1);
 		_ui->groupBoxx_g2o->setEnabled(false);
 	}
+#ifdef RTABMAP_ORB_SLAM2
+	else
+	{
+		// only graph optimization is disabled, g2o (from ORB_SLAM2) is valid only for SBA
+		_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
+	}
+#endif
 	if(!OptimizerG2O::isCSparseAvailable())
 	{
 		_ui->comboBox_g2o_solver->setItemData(0, 0, Qt::UserRole - 1);
@@ -410,9 +417,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_ceilingFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_floorFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_normalRadiusSearch, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_ceilingFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_floorFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_normalKSearch_scan, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_normalRadiusSearch_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -585,6 +594,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_cameraImages_scanNormalsK, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_cameraImages_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 
 	//Rtabmap basic
 	connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
@@ -649,7 +659,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_imagePreDecimation->setObjectName(Parameters::kMemImagePreDecimation().c_str());
 	_ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str());
 	_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
+	_ui->general_doubleSpinBox_laserScanVoxelSize->setObjectName(Parameters::kMemLaserScanVoxelSize().c_str());
 	_ui->general_spinBox_laserScanNormalK->setObjectName(Parameters::kMemLaserScanNormalK().c_str());
+	_ui->general_doubleSpinBox_laserScanNormalRadius->setObjectName(Parameters::kMemLaserScanNormalRadius().c_str());
 	_ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str());
 
 	// Database
@@ -857,7 +869,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpEpsilon->setObjectName(Parameters::kIcpEpsilon().c_str());
 	_ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str());
 	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
-	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str());
+	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str());
+	_ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str());
+	_ui->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str());
 
 	_ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str());
 	_ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str());
@@ -902,7 +916,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
 	connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryType, SLOT(setCurrentIndex(int)));
-	connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOdometryVisibility()));
 	_ui->odom_strategy->setCurrentIndex(Parameters::defaultOdomStrategy());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
 	_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
@@ -920,6 +933,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
 	_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str());
+	_ui->doubleSpinBox_odom_f2m_scanAngle->setObjectName(Parameters::kOdomF2MScanSubtractAngle().c_str());
 	_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());
 	_ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str());
 
@@ -1374,10 +1388,12 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_ceilingFilterHeight->setValue(0);
 		_ui->doubleSpinBox_floorFilterHeight->setValue(0);
 		_ui->spinBox_normalKSearch->setValue(10);
+		_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
 
 		_ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(0);
 		_ui->doubleSpinBox_floorFilterHeight_scan->setValue(0);
 		_ui->spinBox_normalKSearch_scan->setValue(0);
+		_ui->doubleSpinBox_normalRadiusSearch_scan->setValue(0.0);
 
 		_ui->checkBox_showGraphs->setChecked(true);
 		_ui->checkBox_showFrustums->setChecked(false);
@@ -1542,6 +1558,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
 		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.025f);
 		_ui->spinBox_cameraImages_scanNormalsK->setValue(20);
+		_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(0.0);
 
 		_ui->groupBox_depthFromScan->setChecked(false);
 		_ui->groupBox_depthFromScan_fillHoles->setChecked(true);
@@ -1620,7 +1637,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
 		{
 			_ui->odom_registration->setCurrentIndex(3);
-			updateOdometryVisibility();
 		}
 	}
 }
@@ -1769,9 +1785,11 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->doubleSpinBox_ceilingFilterHeight->setValue(settings.value("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()).toDouble());
 	_ui->doubleSpinBox_floorFilterHeight->setValue(settings.value("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()).toDouble());
 	_ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
+	_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
 	_ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(settings.value("scanCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight_scan->value()).toDouble());
 	_ui->doubleSpinBox_floorFilterHeight_scan->setValue(settings.value("scanFloorHeight", _ui->doubleSpinBox_floorFilterHeight_scan->value()).toDouble());
 	_ui->spinBox_normalKSearch_scan->setValue(settings.value("scanNormalKSearch", _ui->spinBox_normalKSearch_scan->value()).toInt());
+	_ui->doubleSpinBox_normalRadiusSearch_scan->setValue(settings.value("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value()).toDouble());
 
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
 	_ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool());
@@ -1933,6 +1951,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(settings.value("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value()).toDouble());
 	_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("voxelSize", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
 	_ui->spinBox_cameraImages_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value()).toInt());
+	_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(settings.value("normalsRadius", _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()).toDouble());
 	settings.endGroup();//ScanFromDepth
 
 	settings.beginGroup("DepthFromScan");
@@ -2155,9 +2174,11 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("cloudCeilingHeight",     _ui->doubleSpinBox_ceilingFilterHeight->value());
 	settings.setValue("cloudFloorHeight",       _ui->doubleSpinBox_floorFilterHeight->value());
 	settings.setValue("normalKSearch",          _ui->spinBox_normalKSearch->value());
+	settings.setValue("normalRadiusSearch",     _ui->doubleSpinBox_normalRadiusSearch->value());
 	settings.setValue("scanCeilingHeight",      _ui->doubleSpinBox_ceilingFilterHeight_scan->value());
 	settings.setValue("scanFloorHeight",        _ui->doubleSpinBox_floorFilterHeight_scan->value());
 	settings.setValue("scanNormalKSearch",      _ui->spinBox_normalKSearch_scan->value());
+	settings.setValue("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value());
 
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
 	settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked());
@@ -2321,6 +2342,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("maxDepth", 			_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
 	settings.setValue("voxelSize", 			_ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
 	settings.setValue("normalsK", 			_ui->spinBox_cameraImages_scanNormalsK->value());
+	settings.setValue("normalsRadius", 		_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
 	settings.endGroup();
 
 	settings.beginGroup("DepthFromScan");
@@ -2421,6 +2443,7 @@ bool PreferencesDialog::validateForm()
 					   "with TORO. GTSAM is set instead for graph optimization strategy."));
 			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeGTSAM);
 		}
+#ifndef RTABMAP_ORB_SLAM2
 		else if(Optimizer::isAvailable(Optimizer::kTypeG2O))
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
@@ -2428,8 +2451,13 @@ bool PreferencesDialog::validateForm()
 					   "with TORO. g2o is set instead for graph optimization strategy."));
 			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeG2O);
 		}
+#endif
 	}
+#ifdef RTABMAP_ORB_SLAM2
+	if(_ui->graphOptimization_type->currentIndex() == 1)
+#else
 	if(_ui->graphOptimization_type->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+#endif
 	{
 		if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 		{
@@ -2448,6 +2476,7 @@ bool PreferencesDialog::validateForm()
 	}
 	if(_ui->graphOptimization_type->currentIndex() == 2 && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 	{
+#ifndef RTABMAP_ORB_SLAM2
 		if(Optimizer::isAvailable(Optimizer::kTypeG2O))
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
@@ -2455,7 +2484,9 @@ bool PreferencesDialog::validateForm()
 					   "with GTSAM. g2o is set instead for graph optimization strategy."));
 			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeG2O);
 		}
-		else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
+		else
+#endif
+			if(Optimizer::isAvailable(Optimizer::kTypeTORO))
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
 					tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built "
@@ -3474,7 +3505,9 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 					ok = false;
 				}
 #endif
+#ifndef RTABMAP_ORB_SLAM2
 				if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+#endif
 				{
 					if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
 					{
@@ -3936,18 +3969,6 @@ void PreferencesDialog::setupKpRoiPanel()
 	_ui->doubleSpinBox_kp_roi3->setValue(strings[3].toDouble()*100.0);
 }
 
-void PreferencesDialog::updateOdometryVisibility()
-{
-	UASSERT(_ui->odom_strategy->count() == 6);
-	_ui->groupBox_odomF2M->setVisible(_ui->odom_strategy->currentIndex()==0);
-	_ui->groupBox_odomF2F->setVisible(_ui->odom_strategy->currentIndex()==1);
-	_ui->groupBox_odomFovis->setVisible(_ui->odom_strategy->currentIndex()==2);
-	_ui->groupBox_odomViso2->setVisible(_ui->odom_strategy->currentIndex()==3);
-	_ui->groupBox_odomDVO->setVisible(_ui->odom_strategy->currentIndex()==4);
-	_ui->groupBox_odomORBSLAM2->setVisible(_ui->odom_strategy->currentIndex()==5);
-	_ui->groupBox_odomMono->setVisible(_ui->odom_strategy->currentIndex()==6);
-}
-
 void PreferencesDialog::updateKpROI()
 {
 	QStringList strings;
@@ -4286,6 +4307,10 @@ int PreferencesDialog::getNormalKSearch() const
 {
 	return _ui->spinBox_normalKSearch->value();
 }
+double PreferencesDialog::getNormalRadiusSearch() const
+{
+	return _ui->doubleSpinBox_normalRadiusSearch->value();
+}
 double PreferencesDialog::getScanCeilingFilteringHeight() const
 {
 	return _ui->doubleSpinBox_ceilingFilterHeight_scan->value();
@@ -4298,6 +4323,10 @@ int PreferencesDialog::getScanNormalKSearch() const
 {
 	return _ui->spinBox_normalKSearch_scan->value();
 }
+double PreferencesDialog::getScanNormalRadiusSearch() const
+{
+	return _ui->doubleSpinBox_normalRadiusSearch_scan->value();
+}
 
 bool PreferencesDialog::isGraphsShown() const
 {
@@ -4636,6 +4665,10 @@ int PreferencesDialog::getSourceScanNormalsK() const
 {
 	return _ui->spinBox_cameraImages_scanNormalsK->value();
 }
+double PreferencesDialog::getSourceScanNormalsRadius() const
+{
+	return _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value();
+}
 
 Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 {
@@ -4743,6 +4776,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
+						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
 						this->getLaserLocalTransform());
 		((CameraRGBDImages*)camera)->setTimestamps(
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
@@ -4788,6 +4822,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
+						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
 						this->getLaserLocalTransform());
 		((CameraStereoImages*)camera)->setTimestamps(
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
@@ -4893,6 +4928,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
+						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
 						this->getLaserLocalTransform());
 		((CameraImages*)camera)->setDepthFromScan(
 				_ui->groupBox_depthFromScan->isChecked(),
@@ -5138,7 +5174,8 @@ void PreferencesDialog::testOdometry()
 			_ui->spinBox_cameraScanFromDepth_decimation->value(),
 			_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
 			_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
-			_ui->spinBox_cameraImages_scanNormalsK->value());
+			_ui->spinBox_cameraImages_scanNormalsK->value(),
+			_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
 	if(isDepthFilteringAvailable())
 	{
 		if(_ui->groupBox_bilateral->isChecked())
@@ -5186,7 +5223,8 @@ void PreferencesDialog::testCamera()
 				_ui->spinBox_cameraScanFromDepth_decimation->value(),
 				_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
 				_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
-				_ui->spinBox_cameraImages_scanNormalsK->value());
+				_ui->spinBox_cameraImages_scanNormalsK->value(),
+				_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
 		if(isDepthFilteringAvailable())
 		{
 			if(_ui->groupBox_bilateral->isChecked())

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-2684</y>
+        <y>0</y>
-        <width>773</width>
+        <width>778</width>
-        <height>4103</height>
+        <height>4058</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -52,21 +52,21 @@
            </property>
           </widget>
          </item>
-         <item row="8" column="0">
+         <item row="9" column="0">
           <widget class="QCheckBox" name="checkBox_regenerate">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="9" column="0">
+         <item row="10" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="8" column="1">
+         <item row="9" column="1">
           <widget class="QLabel" name="label_regenerate">
            <property name="text">
             <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
@@ -76,7 +76,14 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="1">
+         <item row="12" column="0">
+          <widget class="QCheckBox" name="checkBox_gainCompensation">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="1">
           <widget class="QLabel" name="label_voxel">
            <property name="text">
             <string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
@@ -86,13 +93,6 @@
            </property>
           </widget>
          </item>
-         <item row="11" column="0">
-          <widget class="QCheckBox" name="checkBox_gainCompensation">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="4" column="1">
           <widget class="QLabel" name="label_binaryFile_2">
            <property name="text">
@@ -103,7 +103,7 @@
            </property>
           </widget>
          </item>
-         <item row="11" column="1">
+         <item row="12" column="1">
           <widget class="QLabel" name="label_gainCompensation">
            <property name="text">
             <string>Gain compensation. Normalize brightness of images.</string>
@@ -133,7 +133,7 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="1">
+         <item row="13" column="1">
           <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
             <string>Meshing.</string>
@@ -143,7 +143,14 @@
            </property>
           </widget>
          </item>
-         <item row="9" column="1">
+         <item row="11" column="0">
+          <widget class="QCheckBox" name="checkBox_smoothing">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="10" column="1">
           <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
             <string>Cloud filtering. Remove sparse points that are far from surfaces.</string>
@@ -153,8 +160,8 @@
            </property>
           </widget>
          </item>
-         <item row="10" column="0">
+         <item row="13" column="0">
-          <widget class="QCheckBox" name="checkBox_smoothing">
+          <widget class="QCheckBox" name="checkBox_meshing">
            <property name="text">
             <string/>
            </property>
@@ -170,14 +177,7 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="0">
+         <item row="11" column="1">
-          <widget class="QCheckBox" name="checkBox_meshing">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="10" column="1">
           <widget class="QLabel" name="label_binaryFile_10">
            <property name="text">
             <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -187,7 +187,7 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="0">
+         <item row="8" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
            <property name="suffix">
             <string> m</string>
@@ -206,6 +206,13 @@
            </property>
           </widget>
          </item>
+         <item row="4" column="0">
+          <widget class="QCheckBox" name="checkBox_assemble">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
          <item row="6" column="1">
           <widget class="QLabel" name="label_normal">
            <property name="text">
@@ -216,23 +223,6 @@
            </property>
           </widget>
          </item>
-         <item row="3" column="0">
-          <widget class="QCheckBox" name="checkBox_binary">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QCheckBox" name="checkBox_assemble">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="5" column="0">
           <widget class="QComboBox" name="comboBox_frame">
            <item>
@@ -257,6 +247,16 @@
            </item>
           </widget>
          </item>
+         <item row="3" column="0">
+          <widget class="QCheckBox" name="checkBox_binary">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
          <item row="0" column="1">
           <widget class="QLabel" name="label_binaryFile_11">
            <property name="text">
@@ -277,6 +277,23 @@
            </property>
           </widget>
          </item>
+         <item row="7" column="1">
+          <widget class="QLabel" name="label_normal_2">
+           <property name="text">
+            <string>Set the search radius for the normal estimation.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+          </widget>
+         </item>
         </layout>
        </item>
        <item>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-629</y>
             <width>678</width>
             <height>2739</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>18</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -512,6 +512,31 @@ Show a yellow background when the number of odometry inliers goes under this thr
                       <layout class="QVBoxLayout" name="verticalLayout_112">
                        <item>
                         <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
+                         <item row="3" column="1">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
+                           <property name="decimals">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <double>100.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.100000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="1">
+                          <widget class="QLineEdit" name="lineEdit_roiRatios_odom"/>
+                         </item>
+                         <item row="5" column="0">
+                          <widget class="QLineEdit" name="lineEdit_roiRatios"/>
+                         </item>
                          <item row="0" column="0">
                           <widget class="QLabel" name="label_154">
                            <property name="text">
@@ -597,7 +622,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="0">
+                         <item row="13" column="0">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
                            <property name="suffix">
                             <string/>
@@ -616,7 +641,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="1">
+                         <item row="13" column="1">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
                            <property name="suffix">
                             <string/>
@@ -635,7 +660,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="2">
+                         <item row="13" column="2">
                           <widget class="QLabel" name="label_155">
                            <property name="text">
                             <string>Opacity.</string>
@@ -648,7 +673,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="2">
+                         <item row="14" column="2">
                           <widget class="QLabel" name="label_157">
                            <property name="text">
                             <string>Point size (1..64).</string>
@@ -712,25 +737,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="decimals">
-                            <number>1</number>
-                           </property>
-                           <property name="maximum">
-                            <double>100.000000000000000</double>
-                           </property>
-                           <property name="singleStep">
-                            <double>0.100000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>0.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="2">
                           <widget class="QLabel" name="label_132">
                            <property name="text">
@@ -798,12 +804,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="0">
-                          <widget class="QLineEdit" name="lineEdit_roiRatios"/>
-                         </item>
-                         <item row="5" column="1">
-                          <widget class="QLineEdit" name="lineEdit_roiRatios_odom"/>
-                         </item>
                          <item row="5" column="2">
                           <widget class="QLabel" name="label_353">
                            <property name="text">
@@ -858,7 +858,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="0">
+                         <item row="14" column="0">
                           <widget class="QSpinBox" name="spinBox_ptsize">
                            <property name="minimum">
                             <number>1</number>
@@ -871,7 +871,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="1">
+                         <item row="14" column="1">
                           <widget class="QSpinBox" name="spinBox_ptsize_odom">
                            <property name="minimum">
                             <number>1</number>
@@ -1010,6 +1010,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
+                         <item row="12" column="2">
+                          <widget class="QLabel" name="label_427">
+                           <property name="text">
+                            <string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="12" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>
@@ -1117,7 +1137,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <string>Laser Scan</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_79" columnstretch="0,0,1">
-                       <item row="8" column="1">
+                       <item row="9" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1127,7 +1147,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="2">
+                       <item row="9" column="2">
                         <widget class="QLabel" name="label_158">
                          <property name="text">
                           <string>Scan point size (1..64).</string>
@@ -1169,7 +1189,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="2">
+                       <item row="8" column="2">
                         <widget class="QLabel" name="label_156">
                          <property name="text">
                           <string>Scan opacity.</string>
@@ -1195,7 +1215,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
+                       <item row="9" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1235,7 +1255,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="0">
+                       <item row="8" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
                          <property name="suffix">
                           <string/>
@@ -1267,7 +1287,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="1">
+                       <item row="8" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
                          <property name="suffix">
                           <string/>
@@ -1440,6 +1460,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="7" column="2">
+                        <widget class="QLabel" name="label_428">
+                         <property name="text">
+                          <string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch_scan">
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5283,6 +5323,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="3" column="1">
+                          <widget class="QLabel" name="label_425">
+                           <property name="text">
+                            <string>Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_cameraImages_scanNormalsRadius">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -5995,6 +6058,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_10">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_42" columnstretch="0,1">
+                      <item row="14" column="0">
+                       <widget class="QSpinBox" name="spinBox_imagePreDecimation">
+                        <property name="minimum">
+                         <number>-16</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="15" column="0">
+                       <widget class="QSpinBox" name="spinBox_imagePostDecimation">
+                        <property name="minimum">
+                         <number>-16</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QSpinBox" name="general_spinBox_maxStMemSize">
                         <property name="minimum">
@@ -6053,32 +6136,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="0">
-                       <widget class="QSpinBox" name="general_spinBox_maxRetrieved">
-                        <property name="maximum">
-                         <number>999</number>
-                        </property>
-                        <property name="singleStep">
-                         <number>1</number>
-                        </property>
-                        <property name="value">
-                         <number>2</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_retrieved_3">
-                        <property name="text">
-                         <string>Bad signatures are ignored.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="8" column="0">
                        <widget class="QCheckBox" name="general_checkBox_initWMWithAllNodes">
                         <property name="text">
@@ -6089,16 +6146,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
+                      <item row="2" column="0">
-                       <widget class="QLabel" name="label_retrieved_5">
+                       <widget class="QSpinBox" name="general_spinBox_maxRetrieved">
-                        <property name="text">
+                        <property name="maximum">
-                         <string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
+                         <number>999</number>
                         </property>
-                        <property name="wordWrap">
+                        <property name="singleStep">
-                         <bool>true</bool>
+                         <number>1</number>
                         </property>
-                        <property name="textInteractionFlags">
+                        <property name="value">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         <number>2</number>
                         </property>
                        </widget>
                       </item>
@@ -6122,10 +6179,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
+                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_retrieved_11">
+                       <widget class="QLabel" name="label_retrieved_3">
                         <property name="text">
-                         <string>Create map labels. The first node of a map will be labelled as &quot;map#&quot; where # is the map ID.</string>
+                         <string>Bad signatures are ignored.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6145,26 +6202,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="16" column="0">
+                      <item row="9" column="1">
-                       <widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
+                       <widget class="QLabel" name="label_retrieved_5">
-                        <property name="minimumSize">
+                        <property name="text">
-                         <size>
+                         <string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
-                          <width>50</width>
-                          <height>0</height>
-                         </size>
                         </property>
-                        <property name="minimum">
+                        <property name="wordWrap">
-                         <number>1</number>
+                         <bool>true</bool>
                         </property>
-                        <property name="maximum">
+                        <property name="textInteractionFlags">
-                         <number>9999</number>
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                         </property>
                        </widget>
                       </item>
-                      <item row="11" column="1">
+                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_retrieved_12">
+                       <widget class="QLabel" name="label_retrieved_11">
                         <property name="text">
-                         <string>Raw descriptors kept in memory.</string>
+                         <string>Create map labels. The first node of a map will be labelled as &quot;map#&quot; where # is the map ID.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6184,6 +6238,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="11" column="1">
+                       <widget class="QLabel" name="label_retrieved_12">
+                        <property name="text">
+                         <string>Raw descriptors kept in memory.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="14" column="1">
                        <widget class="QLabel" name="label_retrieved_13">
                         <property name="text">
@@ -6233,6 +6300,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="3" column="0">
                        <widget class="QCheckBox" name="general_checkBox_transferSortingByWeightId">
                         <property name="text">
@@ -6256,16 +6333,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="15" column="0">
-                       <widget class="QSpinBox" name="spinBox_imagePostDecimation">
-                        <property name="minimum">
-                         <number>-16</number>
-                        </property>
-                        <property name="maximum">
-                         <number>16</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="1">
                        <widget class="QLabel" name="label_29">
                         <property name="text">
@@ -6279,6 +6346,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="12" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="1" column="1">
                        <widget class="QLabel" name="label_ratioRecent">
                         <property name="text">
@@ -6292,16 +6369,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="15" column="1">
                        <widget class="QLabel" name="label_retrieved_6">
                         <property name="text">
@@ -6328,21 +6395,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="16" column="1">
+                      <item row="13" column="0">
-                       <widget class="QLabel" name="label_retrieved_8">
+                       <widget class="QCheckBox" name="general_checkBox_compressionParallelized">
-                        <property name="text">
-                         <string>If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="12" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
                         <property name="text">
                          <string/>
                         </property>
@@ -6351,45 +6405,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="14" column="0">
+                      <item row="10" column="0">
-                       <widget class="QSpinBox" name="spinBox_imagePreDecimation">
+                       <widget class="QCheckBox" name="general_checkBox_saveIntermediateNodeData">
-                        <property name="minimum">
-                         <number>-16</number>
-                        </property>
-                        <property name="maximum">
-                         <number>16</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="17" column="0">
-                       <widget class="QSpinBox" name="general_spinBox_laserScanNormalK">
-                        <property name="minimumSize">
-                         <size>
-                          <width>50</width>
-                          <height>0</height>
-                         </size>
-                        </property>
-                        <property name="minimum">
-                         <number>0</number>
-                        </property>
-                        <property name="maximum">
-                         <number>99</number>
-                        </property>
-                        <property name="value">
-                         <number>0</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="17" column="1">
-                       <widget class="QLabel" name="label_retrieved_14">
                         <property name="text">
-                         <string>If &gt; 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.</string>
+                         <string/>
                         </property>
-                        <property name="wordWrap">
+                        <property name="checked">
-                         <bool>true</bool>
+                         <bool>false</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                         </property>
                        </widget>
                       </item>
@@ -6406,16 +6428,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_compressionParallelized">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="10" column="1">
                        <widget class="QLabel" name="label_retrieved_16">
                         <property name="text">
@@ -6429,18 +6441,139 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_saveIntermediateNodeData">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
                      </layout>
                     </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_27">
+                      <property name="title">
+                       <string>Laser scan filtering</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
+                         <property name="minimumSize">
+                          <size>
+                           <width>50</width>
+                           <height>0</height>
+                          </size>
+                         </property>
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>9999</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_retrieved_8">
+                         <property name="text">
+                          <string>Downsampling step. If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanNormalRadius">
+                         <property name="minimumSize">
+                          <size>
+                           <width>50</width>
+                           <height>0</height>
+                          </size>
+                         </property>
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QSpinBox" name="general_spinBox_laserScanNormalK">
+                         <property name="minimumSize">
+                          <size>
+                           <width>50</width>
+                           <height>0</height>
+                          </size>
+                         </property>
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>99</number>
+                         </property>
+                         <property name="value">
+                          <number>0</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_retrieved_14">
+                         <property name="text">
+                          <string>Normal K. If &gt; 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_retrieved_17">
+                         <property name="text">
+                          <string>Normal Radius. If &gt; 0 and laser scans don't have normals, normals will be computed with radius search when creating a signature.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_retrieved_18">
+                         <property name="text">
+                          <string>Voxel size. If &gt; 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 Normal K or Normal Radius parameters below.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanVoxelSize">
+                         <property name="minimumSize">
+                          <size>
+                           <width>50</width>
+                           <height>0</height>
+                          </size>
+                         </property>
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
                     <item>
                      <widget class="QGroupBox" name="groupBox_rehearsal2">
                       <property name="title">
@@ -9947,7 +10080,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_odometryType">
                       <property name="currentIndex">
-                       <number>5</number>
+                       <number>0</number>
                       </property>
                       <widget class="QWidget" name="page_52">
                        <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -10043,6 +10176,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
+                             <item row="2" column="0">
+                              <widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
+                               <property name="maximum">
+                                <number>999999999</number>
+                               </property>
+                              </widget>
+                             </item>
                              <item row="1" column="1">
                               <widget class="QLabel" name="label_194">
                                <property name="text">
@@ -10069,14 +10209,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="2" column="0">
+                             <item row="5" column="1">
-                              <widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
-                               <property name="maximum">
-                                <number>999999999</number>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="4" column="1">
                               <widget class="QLabel" name="label_357">
                                <property name="text">
                                 <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
@@ -10089,7 +10222,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="5" column="1">
+                             <item row="6" column="0">
+                              <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
+                               <property name="maximum">
+                                <number>999999</number>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="6" column="1">
                               <widget class="QLabel" name="label_358">
                                <property name="text">
                                 <string>[Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).</string>
@@ -10102,7 +10242,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="4" column="0">
+                             <item row="5" column="0">
                               <widget class="QComboBox" name="odom_f2m_bundleStrategy">
                                <property name="sizeAdjustPolicy">
                                 <enum>QComboBox::AdjustToContents</enum>
@@ -10124,10 +10264,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </item>
                               </widget>
                              </item>
-                             <item row="5" column="0">
+                             <item row="4" column="1">
-                              <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
+                              <widget class="QLabel" name="label_430">
+                               <property name="text">
+                                <string>[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when Radius above is &gt;0). 0 means any angle.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="0">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanAngle">
+                               <property name="suffix">
+                                <string> deg</string>
+                               </property>
+                               <property name="decimals">
+                                <number>0</number>
+                               </property>
                                <property name="maximum">
-                                <number>999999</number>
+                                <double>180.000000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.000000000000000</double>
                                </property>
                               </widget>
                              </item>
@@ -13471,6 +13633,61 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
+                      <item row="4" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>9999.989999999999782</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.100000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_125">
+                        <property name="text">
+                         <string>Uniform sampling voxel size. Set to 0 to disable.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>999999</number>
+                        </property>
+                        <property name="singleStep">
+                         <number>1</number>
+                        </property>
+                        <property name="value">
+                         <number>1</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="8" column="0">
                        <widget class="QCheckBox" name="loopClosure_icpPointToPlane">
                         <property name="text">
@@ -13572,35 +13789,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_125">
-                        <property name="text">
-                         <string>Uniform sampling voxel size. Set to 0 to disable.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="0">
-                       <widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
-                        <property name="minimum">
-                         <number>1</number>
-                        </property>
-                        <property name="maximum">
-                         <number>999999</number>
-                        </property>
-                        <property name="singleStep">
-                         <number>1</number>
-                        </property>
-                        <property name="value">
-                         <number>1</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="3" column="0">
                        <widget class="QDoubleSpinBox" name="loopClosure_icpVoxelSize">
                         <property name="suffix">
@@ -13697,29 +13885,10 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
-                       <widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.001000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>9999.989999999999782</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.010000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.100000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="8" column="1">
                        <widget class="QLabel" name="label_144">
                         <property name="text">
-                         <string>Point to plane ICP. Only for ICP 3D.</string>
+                         <string>Point to plane ICP.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -13764,6 +13933,45 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
+                      <item row="10" column="1">
+                       <widget class="QLabel" name="label_426">
+                        <property name="text">
+                         <string>Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="11" column="1">
+                       <widget class="QLabel" name="label_429">
+                        <property name="text">
+                         <string>Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="11" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsMinComplexity">
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.13.2</version>
+  <version>0.13.3</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/KittiDataset/main.cpp

@@ -60,6 +60,7 @@ void showUsage()
 			"  --scan_step #      Scan downsample step (default=10).\n"
 			"  --scan_voxel #.#   Scan voxel size (default 0.3 m).\n"
 			"  --scan_k           Scan normal K (default 20).\n"
+			"  --scan_radius      Scan normal radius (default 0).\n"
 			"  --map_update  #    Do map update each X odometry frames (default=10, which\n"
 			"                        gives 1 Hz map update assuming images are at 10 Hz).\n\n"
 			"%s\n"
@@ -104,6 +105,7 @@ int main(int argc, char * argv[])
 	int scanStep = 10;
 	float scanVoxel = 0.3f;
 	int scanNormalK = 20;
+	float scanNormalRadius = 0.0f;
 	std::string gtPath;
 	if(argc < 2)
 	{
@@ -153,6 +155,15 @@ int main(int argc, char * argv[])
 					showUsage();
 				}
 			}
+			else if(std::strcmp(argv[i], "--scan_radius") == 0)
+			{
+				scanNormalRadius = atof(argv[++i]);
+				if(scanNormalRadius < 0.0f)
+				{
+					printf("scanNormalRadius should be >= 0\n");
+					showUsage();
+				}
+			}
 			else if(std::strcmp(argv[i], "--gt") == 0)
 			{
 				gtPath = argv[++i];
@@ -233,10 +244,11 @@ int main(int argc, char * argv[])
 	if(scan)
 	{
 		pathScan = path+"/velodyne";
-		printf("   Scan:              %s\n", pathScan.c_str());
+		printf("   Scan:               %s\n", pathScan.c_str());
-		printf("   Scan step:         %d\n", scanStep);
+		printf("   Scan step:          %d\n", scanStep);
-		printf("   Scan voxel:        %fm\n", scanVoxel);
+		printf("   Scan voxel:         %fm\n", scanVoxel);
-		printf("   Scan normal k:     %d\n", scanNormalK);
+		printf("   Scan normal k:      %d\n", scanNormalK);
+		printf("   Scan normal radius: %f\n", scanNormalRadius);
 	}
 	if(!parameters.empty())
 	{
@@ -338,6 +350,7 @@ int main(int argc, char * argv[])
 						scanStep,
 						scanVoxel,
 						scanNormalK,
+						scanNormalRadius,
 						Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
 	}
 
@@ -387,8 +400,8 @@ int main(int argc, char * argv[])
 			if(odomInfo.interval>0.0)
 				speed = odomInfo.transform.x()/odomInfo.interval*3.6;
 			externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
-			externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
+			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
-			externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
+			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
 
 			bool processData = true;
 			if(iteration % mapUpdate != 0)
@@ -400,11 +413,11 @@ int main(int argc, char * argv[])
 			}
 			if(covariance.empty())
 			{
-				covariance = odomInfo.covariance;
+				covariance = odomInfo.reg.covariance;
 			}
 			else
 			{
-				covariance += odomInfo.covariance;
+				covariance += odomInfo.reg.covariance;
 			}
 
 			timer.restart();
@@ -418,7 +431,7 @@ int main(int argc, char * argv[])
 
 			++iteration;
 			printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
-					iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
 			if(processData && rtabmap.getLoopClosureId()>0)
 			{
 				printf(" *");

tools/OdometryViewer/main.cpp

@@ -191,6 +191,7 @@ int main (int argc, char * argv[])
 	float maxDepth = 4.0f;
 	float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
 	int normalsK = 0;
+	float normalsRadius = 0.0f;
 	if(regStrategy == 1 || regStrategy == 2)
 	{
 		// icp requires scans
@@ -203,8 +204,10 @@ int main (int argc, char * argv[])
 		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
 		if(pointToPlane)
 		{
-			normalsK = rtabmap::Parameters::defaultIcpPointToPlaneNormalNeighbors();
+			normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK();
-			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneNormalNeighbors(), normalsK);
+			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK);
+			normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius();
+			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius);
 		}
 
 		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
@@ -310,7 +313,7 @@ int main (int argc, char * argv[])
 		{
 			rtabmap::CameraThread cameraThread(camera, parameters);
 
-			cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK);
+			cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK, normalsRadius);
 
 			odomThread.start();
 			cameraThread.start();

tools/RgbdDataset/main.cpp

@@ -226,8 +226,8 @@ int main(int argc, char * argv[])
 			Transform pose = odom.process(data, &odomInfo);
 			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
 			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
+			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
-			externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
+			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
 
 			bool processData = true;
 			if(detectionRate>0.0f &&
@@ -251,11 +251,11 @@ int main(int argc, char * argv[])
 			}
 			if(covariance.empty())
 			{
-				covariance = odomInfo.covariance;
+				covariance = odomInfo.reg.covariance;
 			}
 			else
 			{
-				covariance += odomInfo.covariance;
+				covariance += odomInfo.reg.covariance;
 			}
 
 			timer.restart();
@@ -269,7 +269,7 @@ int main(int argc, char * argv[])
 
 			++iteration;
 			printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
-					iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
 			if(processData && rtabmap.getLoopClosureId()>0)
 			{
 				printf(" *");