Version 0.11.0: Refactored Visual/ICP transformation estimation approaches, Added Registration classes for convenience, Added Parameters migration approach, 3D laser scans can be used

CMakeLists.txt

@@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 10)
+SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 11)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/CameraRGB.h

@@ -56,6 +56,16 @@ public:
 			bool isDepth = false,
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraImages(const std::string & scanPath,
+				const Transform & scanLocalTransform,
+				int scanMaxPts,
+			    const std::string & path,
+				int startAt = 1,
+				bool refreshDir = false,
+				bool rectifyImages = false,
+				bool isDepth = false,
+				float imageRate = 0,
+				const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -79,6 +89,12 @@ private:
 	int _count;
 	UDirectory * _dir;
 	std::string _lastFileName;
+	int _countScan;
+	UDirectory * _scanDir;
+	std::string _lastScanFileName;
+	std::string _scanPath;
+	Transform _scanLocalTransform;
+	int _scanMaxPts;
 
 	std::string _cameraName;
 	CameraModel _model;

corelib/include/rtabmap/core/CameraStereo.h

@@ -119,6 +119,17 @@ public:
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoImages(
+			const std::string & scanPath,
+			const Transform & scanLocalTransform,
+			int scanMaxPts,
+			const std::string & pathLeftImages,
+			const std::string & pathRightImages,
+			bool filenamesAreTimestamps = false,
+			const std::string & timestampsPath = "", // "times.txt"
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/Features2d.h

@@ -106,11 +106,13 @@ public:
 	static void filterKeypointsByDepth(
 				std::vector<cv::KeyPoint> & keypoints,
 				const cv::Mat & depth,
+				float minDepth,
 				float maxDepth);
 	static void filterKeypointsByDepth(
 			std::vector<cv::KeyPoint> & keypoints,
 			cv::Mat & descriptors,
 			const cv::Mat & depth,
+			float minDepth,
 			float maxDepth);
 
 	static void filterKeypointsByDisparity(

corelib/include/rtabmap/core/Memory.h

@@ -52,6 +52,8 @@ class VWDictionary;
 class VisualWord;
 class Feature2D;
 class Statistics;
+class RegistrationVis;
+class RegistrationIcp;
 
 class RTABMAP_EXP Memory
 {
@@ -176,21 +178,16 @@ public:
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
-	float getBowInlierDistance() const {return _bowInlierDistance;}
+
-	int getBowIterations() const {return _bowIterations;}
+	Transform computeVisualTransform(int fromId, int toId, std::string * rejectedMsg = 0, int * inliers = 0, float * variance = 0);
-	int getBowMinInliers() const {return _bowMinInliers;}
+	Transform computeIcpTransform(int fromId, int toId, Transform guess, std::string * rejectedMsg = 0, int * correspondences = 0, float * variance = 0, float * correspondencesRatio = 0);
-	bool getBowForce2D() const {return _bowForce2D;}
-	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
-	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
 			const std::map<int, Transform> & poses,
 			std::string * rejectedMsg = 0,
 			int * inliers = 0,
-			double * variance = 0);
+			float * variance = 0);
 
 private:
 	void preUpdate();
@@ -241,8 +238,9 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	int _imageDecimation;
-	float _laserScanVoxelSize;
+	float _laserScanDownsampleStepSize;
 	bool _localSpaceLinksKeptInWM;
+	bool _reextractLoopClosureFeatures;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
@@ -268,34 +266,11 @@ private:
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
 	float _wordsMaxDepth; // 0=inf
+	float _wordsMinDepth;
 	std::vector<float> _roiRatios; // size 4
 
-	// RGBD-SLAM stuff
+	RegistrationVis * _registrationVis;
-	int _bowMinInliers;
+	RegistrationIcp * _registrationIcp;
-	float _bowInlierDistance;
-	int _bowIterations;
-	int _bowRefineIterations;
-	bool _bowForce2D;
-	float _bowEpipolarGeometryVar;
-	int _bowEstimationType;
-	double _bowPnPReprojError;
-	int _bowPnPFlags;
-	bool _bowVarianceFromInliersCount;
-	float _icpMaxTranslation;
-	float _icpMaxRotation;
-	int _icpDecimation;
-	float _icpMaxDepth;
-	float _icpVoxelSize;
-	int _icpSamples;
-	float _icpMaxCorrespondenceDistance;
-	int _icpMaxIterations;
-	float _icpCorrespondenceRatio;
-	bool _icpPointToPlane;
-	int _icpPointToPlaneNormalNeighbors;
-	float _icp2MaxCorrespondenceDistance;
-	int _icp2MaxIterations;
-	float _icp2CorrespondenceRatio;
-	float _icp2VoxelSize;
 
 	// Stereo stuff
 	int _stereoFlowWinSize;

corelib/include/rtabmap/core/Parameters.h

@@ -199,14 +199,15 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
 	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
-	RTABMAP_PARAM(Mem, LaserScanVoxelSize,      float, 0.0,      "If > 0.0, voxelize laser scans when creating a signature.");
+	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,  "If > 1, downsample the laser scans when creating a signature.");
 	RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true,      "If local space links are kept in WM.");
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM_COND(Kp, NNStrategy,       int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
 	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
-	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf)");
+	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf).");
+	RTABMAP_PARAM(Kp, MinDepth,              float, 0.0, 		"Filter extracted keypoints by depth.");
 	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.5, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM_COND(Kp, NndrRatio, 	     float, RTABMAP_NONFREE, 0.8, 0.9,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
@@ -216,10 +217,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", 		"Region of interest ratios [left, right, top, bottom].");
 	RTABMAP_PARAM_STR(Kp, DictionaryPath,    "", 				"Path of the pre-computed dictionary");
 	RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,	"When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
-
+    RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,        "See cv::cornerSubPix().");
-	RTABMAP_PARAM(Kp, SubPixWinSize,         int, 3,        "See cv::cornerSubPix().");
+	RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
-	RTABMAP_PARAM(Kp, SubPixIterations,      int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+	RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02,  "See cv::cornerSubPix().");
-	RTABMAP_PARAM(Kp, SubPixEps,             double, 0.02,  "See cv::cornerSubPix().");
 
 	//Database
 	RTABMAP_PARAM(DbSqlite3, InMemory, 	   bool, false, 		"Using database in the memory instead of a file on the hard disk.");
@@ -229,7 +229,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2, 				"0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
 
 	// Keypoints descriptors/detectors
-	RTABMAP_PARAM(SURF, Extended, 		  bool, false, 	"Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
+	RTABMAP_PARAM(SURF, Extended, 		  bool, false, 	    "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
 	RTABMAP_PARAM(SURF, HessianThreshold, float, 500.0,		"Threshold for hessian keypoint detector used in SURF.");
 	RTABMAP_PARAM(SURF, Octaves, 		  int, 4, 			"Number of pyramid octaves the keypoint detector will use.");
 	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2, 			"Number of octave layers within each octave.");
@@ -286,7 +286,6 @@ class RTABMAP_EXP Parameters
 
 	// RGB-D SLAM
 	RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
-	RTABMAP_PARAM(RGBD, PoseScanMatching,         bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
 	RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
@@ -300,18 +299,22 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
 	RTABMAP_PARAM(RGBD, LocalImmunizationRatio,   float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
-	RTABMAP_PARAM(RGBD,  ScanMatchingIdsSavedInLinks, bool, true,      "Save scan matching IDs in link's user data.");
+	RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,      "Save scan matching IDs in link's user data.");
+	RTABMAP_PARAM(RGBD, IcpOdomRefining,          bool, false,  "If the newest added node's pose is refined using ICP with the previous node to correct odometry from pose to pose (laser scans required!).");
+	RTABMAP_PARAM(RGBD, IcpLoopClosureRefining,   bool, false, 	"If the estimated loop closure transformation is refined using ICP (laser scans required!).");	
+	RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures,  bool, true,    "Extract features even if there are some already in the nodes.");
 
-	// Local loop closure detection
+	// Local/Proximity loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, true, 	"Detection over locations (in Working Memory or STM) near in space.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathScansMerged,   bool, true, 	"Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth,   int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 0.5, "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed,   bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
 
 	// Graph optimization
-	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,        "Graph optimization strategy: 0=TORO and 1=g2o.");
+	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,       "Graph optimization strategy: 0=TORO and 1=g2o.");
-	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,      "Optimization iterations.");
+	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,     "Optimization iterations.");
 	RTABMAP_PARAM(RGBD, OptimizeSlam2D,            bool, false,  "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
 	RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored,   bool, false,  "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
 	RTABMAP_PARAM(RGBD, OptimizeEpsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
@@ -319,23 +322,10 @@ class RTABMAP_EXP Parameters
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");
-	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(Odom, EstimationType,  		int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
-	RTABMAP_PARAM(Odom, MaxFeatures,            int, 1000, 		"0 no limits.");
-	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.1,     "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
-	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
-	RTABMAP_PARAM(Odom, Iterations,             int, 100, 		"Maximum iterations to compute the transform from visual words.");
-	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,         "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Odom, MaxDepth,               float, 0,       "Max depth of the words (0 means no limit).");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
-	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
-	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
 	RTABMAP_PARAM(Odom, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
 	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
 	RTABMAP_PARAM(Odom, ImageBufferSize,        unsigned int, 1, "Data buffer size (0 min inf).");
-	RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
-	RTABMAP_PARAM(Odom, PnPReprojError, 		double, 5.0,     "PnP reprojection error.");
-	RTABMAP_PARAM(Odom, PnPFlags, 				int, 1,    	     "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 	RTABMAP_PARAM(Odom, ParticleFiltering, 		bool, false,     "Particle filtering to smooth the odometry trajectory.");
 	RTABMAP_PARAM(Odom, ParticleSize, 		    unsigned int, 400,  "Number of particles of the filter.");
 	RTABMAP_PARAM(Odom, ParticleNoiseT, 		float, 0.002,     "Noise (m) of translation components (x,y,z).");
@@ -344,17 +334,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, ParticleLambdaR, 		float, 100,       "Lambda of rotational components (roll,pitch,yaw).");
 
 	// Odometry Bag-of-words
-	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,      "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
+	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,   "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
-	RTABMAP_PARAM(OdomBow, NNType,                 int, 3, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
-	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");
 	RTABMAP_PARAM_STR(OdomBow, FixedLocalMapPath,   "",         "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
 
 	// Odometry Mono
 	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");
 	RTABMAP_PARAM(OdomMono, InitMinTranslation,     float, 0.1,  "Minimum translation required for the initialization step.");
-	RTABMAP_PARAM(OdomMono, MinTranslation,         float, 0.02,  "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
+	RTABMAP_PARAM(OdomMono, MinTranslation,         float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
-	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01,  "Maximum variance to add new points to local map.");
+	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01, "Maximum variance to add new points to local map.");
-
 
 	// Odometry common stuff between BOW and Optical Flow approaches
 	RTABMAP_PARAM(OdomFlow, WinSize,               int, 16,       "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
@@ -362,49 +349,45 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomFlow, Eps,                   double, 0.01,  "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(OdomFlow, MaxLevel,              int, 3,        "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
 
-	RTABMAP_PARAM(OdomSubPix, WinSize,         int, 3,        "Can be used with BOW and optical flow approaches. See cv::cornerSubPix().");
+	// Common registration parameters
-	RTABMAP_PARAM(OdomSubPix, Iterations,      int, 0,       "Can be used with BOW and optical flow approaches. See cv::cornerSubPix(). 0 disables sub pixel refining.");
+	RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
-	RTABMAP_PARAM(OdomSubPix, Eps,             double, 0.02,  "Can be used with BOW and optical flow approaches. See cv::cornerSubPix().");
+	
+	// Visual registration parameters
+	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
+	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
+	RTABMAP_PARAM(Vis, RefineIterations,         int, 10,       "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Vis, PnPReprojError, 	         double, 5.0,   "[Vis/EstimationType = 1] PnP reprojection error.");
+	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,    	"[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
+	RTABMAP_PARAM(Vis, MinInliers,               int, 10, 		"Minimum feature correspondences to compute/accept the transformation.");
+	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
+	RTABMAP_PARAM(Vis, Force2D,                  bool, false,   "Force 2D transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
+	RTABMAP_PARAM(Vis, NNType, 	                 int, 3,        "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
+	RTABMAP_PARAM(Vis, NNDR,                     float, 0.8,    "NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM(Vis, FeatureType,              int, 6,        "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,     "0 no limits.");
+	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0.0,    "Max depth of the features (0 means no limit).");
+	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0.0,    "Min depth of the features (0 means no limit).");
+	RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
+    RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,        "See cv::cornerSubPix().");
+	RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+	RTABMAP_PARAM(Vis, SubPixEps,                double, 0.02,  "See cv::cornerSubPix().");
 
-	// Loop closure constraint
-	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
-	RTABMAP_PARAM(LccIcp, MaxTranslation,  float, 0.2,      "Maximum ICP translation correction accepted (m).");
-	RTABMAP_PARAM(LccIcp, MaxRotation,     float, 0.78,     "Maximum ICP rotation correction accepted (rad).");
 
-	RTABMAP_PARAM(LccBow, EstimationType,  int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
+	// ICP registration parameters
-	RTABMAP_PARAM(LccBow, MinInliers,      int, 10, 		"Minimum visual word correspondences to compute geometry transform.");
+	RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
-	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.1,      "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
+	RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
-	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
+	RTABMAP_PARAM(Icp, 2D,                        bool, true, 	"If 2D ICP is done (only 3Dof -> x,y,yaw).");
-	RTABMAP_PARAM(LccBow, RefineIterations,  int, 10,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.025, "Uniform sampling voxel size (0=disabled).");
-	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
+	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
-	RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
+	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
-	RTABMAP_PARAM(LccBow, PnPReprojError, 	 double, 5.0,    "PnP reprojection error.");
+	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(LccBow, PnPFlags, 	     int, 1,    	 "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.3,   "Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccBow, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
+	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
-	RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
+	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
-	RTABMAP_PARAM(LccReextract, NNType, 	int, 3, 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
-	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.8, 	"NNDR: nearest neighbor distance ratio.");
-	RTABMAP_PARAM(LccReextract, FeatureType, int, 4, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 1000, 		"0 no limits.");
-	RTABMAP_PARAM(LccReextract, MaxDepth, 	float, 0.0,     "Max depth of the words (0 means no limit).");
-
-	RTABMAP_PARAM(LccIcp3, Decimation,      int, 4, 		"Depth image decimation.");
-	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 3.0, 	"Max cloud depth.");
-	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.025, 	"Voxel size to be used for ICP computation.");
-	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
-	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.2, 	"Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"Use point to plane ICP.");
-	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"Number of neighbors to compute normals for point to plane.");
-
-	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, 	"Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"Max iterations.");
-	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, 		"Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.025, 			"Voxel size to be used for ICP computation.");
 
 	// Stereo disparity
-	RTABMAP_PARAM(Stereo, WinSize,               int, 16,        "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(Stereo, WinSize,               int, 16,       "See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "See cv::calcOpticalFlowPyrLK().");
@@ -436,6 +419,17 @@ public:
 	static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
 
 	static std::string getDefaultDatabaseName();
+	
+	/**
+	 * Get removed parameters (backward compatibility)
+	 * <OldKeyName, <isEqual, NewKeyName> >, when isEqual=true, the old value can be safely copied to new parameter
+	 */
+	static const std::map<std::string, std::pair<bool, std::string> > & getRemovedParameters();
+	
+	/**
+	 * <NewKeyName, OldKeyName>
+	 */
+	static const ParametersMap & getBackwardCompatibilityMap();
 
 private:
 	Parameters();
@@ -445,6 +439,9 @@ private:
 	static ParametersMap parameters_;
 	static ParametersMap descriptions_;
 	static Parameters instance_;
+	
+	static std::map<std::string, std::pair<bool, std::string> > removedParameters_;
+	static ParametersMap backwardCompatibilityMap_;
 };
 
 }

corelib/include/rtabmap/core/Registration.h

@@ -0,0 +1,66 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef REGISTRATION_H_
+#define REGISTRATION_H_
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+class Registration
+{
+public:
+	virtual ~Registration() {}
+	virtual void parseParameters(const ParametersMap & parameters)
+	{
+		Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _bowVarianceFromInliersCount);
+	}
+	virtual Transform computeTransformation(
+			const Signature & from,
+			const Signature & to,
+			Transform guess = Transform::getIdentity(),
+			std::string * rejectedMsg = 0,
+			int * inliersOut = 0,
+			float * varianceOut = 0,
+			float * inliersRatioOut = 0) = 0;
+protected:
+	Registration(const ParametersMap & parameters = ParametersMap()) :
+		_bowVarianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())
+	{
+		this->parseParameters(parameters);
+	}
+
+protected:
+	bool _bowVarianceFromInliersCount;
+
+};
+
+}
+
+#endif /* REGISTRATION_H_ */

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef REGISTRATIONICP_H_
+#define REGISTRATIONICP_H_
+
+#include <rtabmap/core/Registration.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+// Geometrical registration
+class RegistrationIcp : public Registration
+{
+public:
+	RegistrationIcp(const ParametersMap & parameters = ParametersMap());
+	virtual ~RegistrationIcp() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	virtual Transform computeTransformation(
+			const Signature & from,
+			const Signature & to,
+			Transform guess = Transform::getIdentity(),
+			std::string * rejectedMsg = 0,
+			int * inliersOut = 0,
+			float * varianceOut = 0,
+			float * inliersRatioOut = 0);
+
+	Transform computeTransformation(
+			const SensorData & from,
+			const SensorData & to,
+			Transform guess = Transform::getIdentity(),
+			std::string * rejectedMsg = 0,
+			int * inliersOut = 0,
+			float * varianceOut = 0,
+			float * inliersRatioOut = 0);
+
+private:
+	float _icpMaxTranslation;
+	float _icpMaxRotation;
+	bool _icp2D;
+	float _icpVoxelSize;
+	int _icpDownsamplingStep;
+	float _icpMaxCorrespondenceDistance;
+	int _icpMaxIterations;
+	float _icpCorrespondenceRatio;
+	bool _icpPointToPlane;
+	int _icpPointToPlaneNormalNeighbors;
+};
+
+}
+
+#endif /* REGISTRATIONICP_H_ */

corelib/include/rtabmap/core/RegistrationVis.h

@@ -0,0 +1,86 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef REGISTRATIONVIS_H_
+#define REGISTRATIONVIS_H_
+
+#include <rtabmap/core/Registration.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+// Visual registration
+class RegistrationVis : public Registration
+{
+public:
+	RegistrationVis(const ParametersMap & parameters = ParametersMap());
+	virtual ~RegistrationVis() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	virtual Transform computeTransformation(
+			const Signature & from,
+			const Signature & to,
+			Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
+			std::string * rejectedMsg = 0,
+			int * inliersOut = 0,
+			float * varianceOut = 0,
+			float * inliersRatioOut = 0);
+
+	float getBowInlierDistance() const {return _bowInlierDistance;}
+	int getBowIterations() const {return _bowIterations;}
+	int getBowMinInliers() const {return _bowMinInliers;}
+	bool getBowForce2D() const {return _bowForce2D;}
+
+private:
+	int _bowMinInliers;
+	float _bowInlierDistance;
+	int _bowIterations;
+	int _bowRefineIterations;
+	bool _bowForce2D;
+	float _bowEpipolarGeometryVar;
+	int _bowEstimationType;
+	double _bowPnPReprojError;
+	int _bowPnPFlags;
+
+	int _reextractNNType;
+	float _reextractNNDR;
+	int _reextractFeatureType;
+	int _reextractMaxWords;
+	float _reextractMaxDepth;
+	float _reextractMinDepth;
+	std::string _reextractRoiRatios;
+
+	int _subPixWinSize;
+	int _subPixIterations;
+	double _subPixEps;
+
+};
+
+}
+
+#endif /* REGISTRATION_H_ */

corelib/include/rtabmap/core/Rtabmap.h

@@ -184,8 +184,8 @@ private:
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
 	float _newMapOdomChangeDistance;
-	int _globalLoopClosureIcpType;
+	bool _loopClosureIcpRefining;
-	bool _poseScanMatching;
+	bool _odomIcpRefining;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	bool _scanMatchingIdsSavedInLinks;
@@ -194,15 +194,10 @@ private:
 	int _localDetectMaxGraphDepth;
 	float _localPathFilteringRadius;
 	bool _localPathOdomPosesUsed;
+	bool _localPathScansMerged;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxLinearError;
-	bool _reextractLoopClosureFeatures;
-	int _reextractNNType;
-	float _reextractNNDR;
-	int _reextractFeatureType;
-	int _reextractMaxWords;
-	float _reextractMaxDepth;
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	bool _goalsSavedInUserData;

corelib/include/rtabmap/core/Statistics.h

@@ -74,6 +74,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(OdomCorrection, Inliers,);
 	RTABMAP_STATS(OdomCorrection, Inliers_ratio,);
 	RTABMAP_STATS(OdomCorrection, Variance,);
+	RTABMAP_STATS(OdomCorrection, Pts,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
@@ -91,7 +92,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
-	RTABMAP_STATS(Timing, Scan_matching, ms);
+	RTABMAP_STATS(Timing, Odom_correction, ms);
 	RTABMAP_STATS(Timing, Local_detection_TIME, ms);
 	RTABMAP_STATS(Timing, Local_detection_SPACE, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);

corelib/include/rtabmap/core/Transform.h

@@ -124,6 +124,7 @@ public:
 	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
+	static Transform fromString(const std::string & string);
 
 private:
 	cv::Mat data_;

corelib/include/rtabmap/core/util3d.h

@@ -127,12 +127,8 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					float maxDepth = 0,
 					const Transform & localTransform = Transform::getIdentity());
 
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform = Transform::getIdentity());
 
 pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
@@ -180,6 +176,8 @@ void RTABMAP_EXP savePCDWords(
 		const std::multimap<int, pcl::PointXYZ> & words,
 		const Transform & transform = Transform::getIdentity());
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_filtering.h

@@ -41,6 +41,16 @@ namespace rtabmap
 namespace util3d
 {
 
+cv::Mat RTABMAP_EXP downsample(
+		const cv::Mat & cloud,
+		int step);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int step);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		int step);
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float voxelSize);
@@ -51,11 +61,30 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		float voxelSize);
 
+inline pcl::PointCloud<pcl::PointXYZ>::Ptr uniformSampling(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float voxelSize)
+{
+	return voxelize(cloud, voxelSize);
+}
+inline pcl::PointCloud<pcl::PointXYZRGB>::Ptr uniformSampling(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float voxelSize)
+{
+	return voxelize(cloud, voxelSize);
+}
+inline pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr uniformSampling(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float voxelSize)
+{
+	return voxelize(cloud, voxelSize);
+}
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		int samples);
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP sampling(
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int samples);
 

corelib/src/CMakeLists.txt

@@ -44,6 +44,9 @@ SET(SRC_FILES
 	Compression.cpp
 	Link.cpp
 	
+	RegistrationIcp.cpp
+	RegistrationVis.cpp
+	
 	Odometry.cpp
 	OdometryThread.cpp
 	OdometryBOW.cpp

corelib/src/CameraRGB.cpp

@@ -38,6 +38,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/imgproc/imgproc.hpp>
 
+#include <rtabmap/core/util3d.h>
+#include <pcl/io/pcd_io.h>
+
 #include <iostream>
 #include <cmath>
 
@@ -61,7 +64,36 @@ CameraImages::CameraImages(const std::string & path,
 	_rectifyImages(rectifyImages),
 	_isDepth(isDepth),
 	_count(0),
-	_dir(0)
+	_dir(0),
+	_countScan(0),
+	_scanDir(0)
+{
+
+}
+
+CameraImages::CameraImages(const std::string & scanPath,
+					const Transform & scanLocalTransform,
+					int scanMaxPts,
+					const std::string & path,
+					 int startAt,
+					 bool refreshDir,
+					 bool rectifyImages,
+					 bool isDepth,
+					 float imageRate,
+					 const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_path(path),
+	_startAt(startAt),
+	_refreshDir(refreshDir),
+	_rectifyImages(rectifyImages),
+	_isDepth(isDepth),
+	_count(0),
+	_dir(0),
+	_countScan(0),
+	_scanDir(0),
+	_scanPath(scanPath),
+	_scanLocalTransform(scanLocalTransform),
+	_scanMaxPts(scanMaxPts)
 {
 
 }
@@ -72,11 +104,19 @@ CameraImages::~CameraImages(void)
 	{
 		delete _dir;
 	}
+	if(_scanDir)
+	{
+		delete _scanDir;
+	}
 }
 
 bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	_cameraName = cameraName;
+	_lastFileName.clear();
+	_lastScanFileName.clear();
+	_count = 0;
+	_countScan = 0;
 
 	UDEBUG("");
 	if(_dir)
@@ -87,7 +127,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	{
 		_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
 	}
-	_count = 0;
 	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
 	{
 		_path.append("/");
@@ -105,6 +144,47 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
 	}
 
+	// check for scan directory
+	if(_scanDir)
+	{
+		delete _scanDir;
+		_scanDir = 0;
+	}
+	if(!_scanPath.empty())
+	{
+		_scanDir = new UDirectory(_scanPath, "pcd bin"); // "bin" is for KITTI format
+		if(_scanPath[_scanPath.size()-1] != '\\' && _scanPath[_scanPath.size()-1] != '/')
+		{
+			_scanPath.append("/");
+		}
+		if(!_scanDir->isValid())
+		{
+			UERROR("Scan directory path is not valid \"%s\"", _scanPath.c_str());
+			delete _scanDir;
+			_scanDir = 0;
+		}
+		else if(_scanDir->getFileNames().size() == 0)
+		{
+			UWARN("Scan directory is empty \"%s\"", _scanPath.c_str());
+			delete _scanDir;
+			_scanDir = 0;
+		}
+		else if(_scanDir->getFileNames().size() != _dir->getFileNames().size())
+		{
+			UERROR("Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
+					_scanPath.c_str(),
+					(int)_scanDir->getFileNames().size(),
+					_path.c_str(),
+					(int)_dir->getFileNames().size());
+			delete _scanDir;
+			_scanDir = 0;
+		}
+		else
+		{
+			UINFO("path=%s scans=%d", _scanPath.c_str(), (int)this->imagesCount());
+		}
+	}
+
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
@@ -164,12 +244,17 @@ std::vector<std::string> CameraImages::filenames() const
 SensorData CameraImages::captureImage()
 {
 	cv::Mat img;
+	cv::Mat scan;
 	UDEBUG("");
 	if(_dir->isValid())
 	{
 		if(_refreshDir)
 		{
 			_dir->update();
+			if(_scanDir)
+			{
+				_scanDir->update();
+			}
 		}
 		if(_startAt == 0)
 		{
@@ -183,6 +268,28 @@ SensorData CameraImages::captureImage()
 					img = cv::imread(fullPath.c_str());
 				}
 			}
+			if(_scanDir)
+			{
+				const std::list<std::string> & scanFileNames = _scanDir->getFileNames();
+				if(scanFileNames.size())
+				{
+					if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
+					{
+						_lastScanFileName = *scanFileNames.rbegin();
+						std::string fullPath = _scanPath + _lastScanFileName;
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+						if(UFile::getExtension(_lastScanFileName).compare("bin") == 0)
+						{
+							cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
+						}
+						else
+						{
+							pcl::io::loadPCDFile(fullPath, *cloud);
+						}
+						scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
+					}
+				}
+			}
 		}
 		else
 		{
@@ -242,6 +349,33 @@ SensorData CameraImages::captureImage()
 					}
 				}
 			}
+
+			if(_scanDir)
+			{
+				fileName = _scanDir->getNextFileName();
+				if(fileName.size())
+				{
+					fullPath = _scanPath + fileName;
+					while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
+					{
+						fullPath = _scanPath + fileName;
+					}
+					if(fileName.size())
+					{
+						UDEBUG("Loading scan : %s", fullPath.c_str());
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+						if(UFile::getExtension(fileName).compare("bin") == 0)
+						{
+							cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
+						}
+						else
+						{
+							pcl::io::loadPCDFile(fullPath, *cloud);
+						}
+						scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
+					}
+				}
+			}
 		}
 
 		if(!img.empty() && _model.isValid() && _rectifyImages)
@@ -256,9 +390,9 @@ SensorData CameraImages::captureImage()
 
 	if(_isDepth)
 	{
-		return SensorData(cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
+		return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
 	}
-	return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
+	return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, img, cv::Mat(), _model, this->getNextSeqID(), UTimer::now());
 }
 
 

corelib/src/CameraStereo.cpp

@@ -779,6 +779,28 @@ CameraStereoImages::CameraStereoImages(
 	}
 }
 
+CameraStereoImages::CameraStereoImages(
+		const std::string & scanPath,
+		const Transform & scanLocalTransform,
+		int scanMaxPts,
+		const std::string & pathLeftImages,
+		const std::string & pathRightImages,
+		bool filenamesAreTimestamps,
+		const std::string & timestampsPath,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		camera_(0),
+		camera2_(0),
+		filenamesAreTimestamps_(filenamesAreTimestamps),
+		timestampsPath_(timestampsPath),
+		rectifyImages_(rectifyImages)
+{
+	camera_ = new CameraImages(scanPath, scanLocalTransform, scanMaxPts, pathLeftImages);
+	camera2_ = new CameraImages(pathRightImages);
+}
+
 CameraStereoImages::~CameraStereoImages()
 {
 	if(camera_)
@@ -811,6 +833,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 					stereoModel_.baseline());
 		}
 	}
+
 	stereoModel_.setLocalTransform(this->getLocalTransform());
 	if(rectifyImages_ && !stereoModel_.isValid())
 	{
@@ -968,7 +991,7 @@ SensorData CameraStereoImages::captureImage()
 					leftImage = stereoModel_.left().rectifyImage(leftImage);
 					rightImage = stereoModel_.right().rectifyImage(rightImage);
 				}
-				data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
+				data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
 			}
 		}
 	}
@@ -1034,6 +1057,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 						stereoModel_.baseline());
 			}
 		}
+
 		stereoModel_.setLocalTransform(this->getLocalTransform());
 		if(rectifyImages_ && !stereoModel_.isValid())
 		{

corelib/src/Compression.cpp

@@ -231,11 +231,6 @@ cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
 		int width = *((int*)&bytes[size-2*sizeof(int)]);
 		int type = *((int*)&bytes[size-1*sizeof(int)]);
 
-		// If the size is higher, it may be a wrong data format.
-		UASSERT_MSG(height>=0 && height<10000 &&
-				    width>=0 && width<10000,
-				    uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
-
 		data = cv::Mat(height, width, type);
 		uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
 

corelib/src/Features2d.cpp

@@ -60,18 +60,22 @@ namespace rtabmap {
 void Feature2D::filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
+		float minDepth,
 		float maxDepth)
 {
 	cv::Mat descriptors;
-	filterKeypointsByDepth(keypoints, descriptors, depth, maxDepth);
+	filterKeypointsByDepth(keypoints, descriptors, depth, minDepth, maxDepth);
 }
 
 void Feature2D::filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors,
 		const cv::Mat & depth,
+		float minDepth,
 		float maxDepth)
 {
+	UASSERT(minDepth >= 0.0f);
+	UASSERT(maxDepth <= 0.0f || maxDepth > minDepth);
 	if(!depth.empty() && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
 	{
 		std::vector<cv::KeyPoint> output(keypoints.size());
@@ -85,7 +89,7 @@ void Feature2D::filterKeypointsByDepth(
 			if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
 			{
 				float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
-				if(uIsFinite(d) && d>0.0f && (maxDepth <= 0.0f || d < maxDepth))
+				if(uIsFinite(d) && d>minDepth && (maxDepth <= 0.0f || d < maxDepth))
 				{
 					output[oi++] = keypoints[i];
 					indexes[i] = 1;

corelib/src/Odometry.cpp

@@ -35,14 +35,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
-		_roiRatios(Parameters::defaultOdomRoiRatios()),
+		_roiRatios(Parameters::defaultVisRoiRatios()),
-		_minInliers(Parameters::defaultOdomMinInliers()),
+		_minInliers(Parameters::defaultVisMinInliers()),
-		_inlierDistance(Parameters::defaultOdomInlierDistance()),
+		_inlierDistance(Parameters::defaultVisInlierDistance()),
-		_iterations(Parameters::defaultOdomIterations()),
+		_iterations(Parameters::defaultVisIterations()),
-		_refineIterations(Parameters::defaultOdomRefineIterations()),
+		_refineIterations(Parameters::defaultVisRefineIterations()),
-		_maxDepth(Parameters::defaultOdomMaxDepth()),
+		_maxDepth(Parameters::defaultVisMaxDepth()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
-		_force2D(Parameters::defaultOdomForce2D()),
+		_force2D(Parameters::defaultVisForce2D()),
 		_holonomic(Parameters::defaultOdomHolonomic()),
 		_particleFiltering(Parameters::defaultOdomParticleFiltering()),
 		_particleSize(Parameters::defaultOdomParticleSize()),
@@ -51,30 +51,30 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
 		_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
 		_fillInfoData(Parameters::defaultOdomFillInfoData()),
-		_estimationType(Parameters::defaultOdomEstimationType()),
+		_estimationType(Parameters::defaultVisEstimationType()),
-		_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
+		_pnpReprojError(Parameters::defaultVisPnPReprojError()),
-		_pnpFlags(Parameters::defaultOdomPnPFlags()),
+		_pnpFlags(Parameters::defaultVisPnPFlags()),
-		_varianceFromInliersCount(Parameters::defaultOdomVarianceFromInliersCount()),
+		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
 		previousTransform_(Transform::getIdentity()),
 		distanceTravelled_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
-	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
+	Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
-	Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
+	Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
-	Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
+	Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
-	Parameters::parse(parameters, Parameters::kOdomRefineIterations(), _refineIterations);
+	Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
-	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
+	Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
-	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
+	Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
-	Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
+	Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
-	Parameters::parse(parameters, Parameters::kOdomEstimationType(), _estimationType);
+	Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
-	Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
+	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
-	Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
+	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
 	UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
-	Parameters::parse(parameters, Parameters::kOdomVarianceFromInliersCount(), _varianceFromInliersCount);
+	Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
 	Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
 	Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);

corelib/src/OdometryBOW.cpp

@@ -65,26 +65,26 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
 	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
-	int nn = Parameters::defaultOdomBowNNType();
+	int nn = Parameters::defaultVisNNType();
-	float nndr = Parameters::defaultOdomBowNNDR();
+	float nndr = Parameters::defaultVisNNDR();
-	int featureType = Parameters::defaultOdomFeatureType();
+	int featureType = Parameters::defaultVisFeatureType();
-	int maxFeatures = Parameters::defaultOdomMaxFeatures();
+	int maxFeatures = Parameters::defaultVisMaxFeatures();
-	Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
+	Parameters::parse(parameters, Parameters::kVisNNType(), nn);
-	Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
-	Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
+	Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
-	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
+	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
 	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
 	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
 	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
 	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
 
 	// Memory's stereo parameters, copy from Odometry
-	int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
+	int subPixWinSize = Parameters::defaultVisSubPixWinSize();
-	int subPixIterations = Parameters::defaultOdomSubPixIterations();
+	int subPixIterations = Parameters::defaultVisSubPixIterations();
-	double subPixEps = Parameters::defaultOdomSubPixEps();
+	double subPixEps = Parameters::defaultVisSubPixEps();
-	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
+	Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
-	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
+	Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
-	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
+	Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
 	customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
 	customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
 	customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));

corelib/src/OdometryMono.cpp

@@ -94,25 +94,25 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
 	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
-	int nn = Parameters::defaultOdomBowNNType();
+	int nn = Parameters::defaultVisNNType();
-	float nndr = Parameters::defaultOdomBowNNDR();
+	float nndr = Parameters::defaultVisNNDR();
-	int featureType = Parameters::defaultOdomFeatureType();
+	int featureType = Parameters::defaultVisFeatureType();
-	int maxFeatures = Parameters::defaultOdomMaxFeatures();
+	int maxFeatures = Parameters::defaultVisMaxFeatures();
-	Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
+	Parameters::parse(parameters, Parameters::kVisNNType(), nn);
-	Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
-	Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
+	Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
-	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
+	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
 	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
 	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
 	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
 	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
 
-	int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
+	int subPixWinSize = Parameters::defaultVisSubPixWinSize();
-	int subPixIterations = Parameters::defaultOdomSubPixIterations();
+	int subPixIterations = Parameters::defaultVisSubPixIterations();
-	double subPixEps = Parameters::defaultOdomSubPixEps();
+	double subPixEps = Parameters::defaultVisSubPixEps();
-	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
+	Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
-	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
+	Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
-	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
+	Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
 	customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
 	customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
 	customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));

corelib/src/OdometryOpticalFlow.cpp

@@ -54,9 +54,9 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
 	stereoEps_(Parameters::defaultStereoEps()),
 	stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
 	stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
-	subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
+	subPixWinSize_(Parameters::defaultVisSubPixWinSize()),
-	subPixIterations_(Parameters::defaultOdomSubPixIterations()),
+	subPixIterations_(Parameters::defaultVisSubPixIterations()),
-	subPixEps_(Parameters::defaultOdomSubPixEps()),
+	subPixEps_(Parameters::defaultVisSubPixEps()),
 	refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
 {
 	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
@@ -68,20 +68,20 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
 	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
 	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
-	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
+	Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize_);
-	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
+	Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations_);
-	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
+	Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps_);
 
 	ParametersMap::const_iterator iter;
-	Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
+	Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultVisFeatureType();
-	if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
+	if((iter=parameters.find(Parameters::kVisFeatureType())) != parameters.end())
 	{
 		detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
 	}
 
 	ParametersMap customParameters;
-	int maxFeatures = Parameters::defaultOdomMaxFeatures();
+	int maxFeatures = Parameters::defaultVisMaxFeatures();
-	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
+	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
 	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
 	// add only feature stuff
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)

corelib/src/Parameters.cpp

@@ -39,6 +39,8 @@ namespace rtabmap
 ParametersMap Parameters::parameters_;
 ParametersMap Parameters::descriptions_;
 Parameters Parameters::instance_;
+std::map<std::string, std::pair<bool, std::string> > Parameters::removedParameters_;
+ParametersMap Parameters::backwardCompatibilityMap_;
 
 Parameters::Parameters()
 {
@@ -69,6 +71,95 @@ std::string Parameters::getDefaultDatabaseName()
 	return "rtabmap.db";
 }
 
+const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemovedParameters()
+{
+	if(removedParameters_.empty())
+	{
+		// removed parameters
+		removedParameters_.insert(std::make_pair("Mem/LaserScanVoxelSize",        std::make_pair(false, Parameters::kMemLaserScanDownsampleStepSize())));
+
+		removedParameters_.insert(std::make_pair("RGBD/PoseScanMatching",         std::make_pair(true,  Parameters::kRGBDIcpOdomRefining())));
+
+		removedParameters_.insert(std::make_pair("Odom/FeatureType",              std::make_pair(true,  Parameters::kVisFeatureType())));
+		removedParameters_.insert(std::make_pair("Odom/EstimationType",           std::make_pair(true,  Parameters::kVisEstimationType())));
+		removedParameters_.insert(std::make_pair("Odom/MaxFeatures",              std::make_pair(true,  Parameters::kVisMaxFeatures())));
+		removedParameters_.insert(std::make_pair("Odom/InlierDistance",           std::make_pair(true,  Parameters::kVisInlierDistance())));
+		removedParameters_.insert(std::make_pair("Odom/MinInliers",               std::make_pair(true,  Parameters::kVisMinInliers())));
+		removedParameters_.insert(std::make_pair("Odom/Iterations",               std::make_pair(true,  Parameters::kVisIterations())));
+		removedParameters_.insert(std::make_pair("Odom/RefineIterations",         std::make_pair(true,  Parameters::kVisRefineIterations())));
+		removedParameters_.insert(std::make_pair("Odom/MaxDepth",                 std::make_pair(true,  Parameters::kVisMaxDepth())));
+		removedParameters_.insert(std::make_pair("Odom/RoiRatios",                std::make_pair(true,  Parameters::kVisRoiRatios())));
+		removedParameters_.insert(std::make_pair("Odom/Force2D",                  std::make_pair(true,  Parameters::kVisForce2D())));
+		removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(true,  Parameters::kRegVarianceFromInliersCount())));
+		removedParameters_.insert(std::make_pair("Odom/PnPReprojError",           std::make_pair(true,  Parameters::kVisPnPReprojError())));
+		removedParameters_.insert(std::make_pair("Odom/PnPFlags",                 std::make_pair(true,  Parameters::kVisPnPFlags())));
+
+		removedParameters_.insert(std::make_pair("OdomBow/NNType",                std::make_pair(true,  Parameters::kVisNNType())));
+		removedParameters_.insert(std::make_pair("OdomBow/NNDR",                  std::make_pair(true,  Parameters::kVisNNDR())));
+
+		removedParameters_.insert(std::make_pair("OdomSubPix/WinSize",            std::make_pair(true,  Parameters::kVisSubPixWinSize())));
+		removedParameters_.insert(std::make_pair("OdomSubPix/Iterations",         std::make_pair(true,  Parameters::kVisSubPixIterations())));
+		removedParameters_.insert(std::make_pair("OdomSubPix/Eps",                std::make_pair(true,  Parameters::kVisSubPixEps())));
+
+		removedParameters_.insert(std::make_pair("LccReextract/Activated",         std::make_pair(false,  Parameters::kRGBDLoopClosureReextractFeatures())));
+		removedParameters_.insert(std::make_pair("LccReextract/FeatureType",       std::make_pair(false,  Parameters::kVisFeatureType())));
+		removedParameters_.insert(std::make_pair("LccReextract/MaxWords",          std::make_pair(false,  Parameters::kVisMaxFeatures())));
+		removedParameters_.insert(std::make_pair("LccReextract/MaxDepth",          std::make_pair(false,  Parameters::kVisMaxDepth())));
+		removedParameters_.insert(std::make_pair("LccReextract/RoiRatios",         std::make_pair(false,  Parameters::kVisRoiRatios())));
+		removedParameters_.insert(std::make_pair("LccReextract/NNType",            std::make_pair(false,  Parameters::kVisNNType())));
+		removedParameters_.insert(std::make_pair("LccReextract/NNDR",              std::make_pair(false,  Parameters::kVisNNDR())));
+
+		removedParameters_.insert(std::make_pair("LccBow/EstimationType",           std::make_pair(false,  Parameters::kVisEstimationType())));
+		removedParameters_.insert(std::make_pair("LccBow/InlierDistance",           std::make_pair(false,  Parameters::kVisInlierDistance())));
+		removedParameters_.insert(std::make_pair("LccBow/MinInliers",               std::make_pair(false,  Parameters::kVisMinInliers())));
+		removedParameters_.insert(std::make_pair("LccBow/Iterations",               std::make_pair(false,  Parameters::kVisIterations())));
+		removedParameters_.insert(std::make_pair("LccBow/RefineIterations",         std::make_pair(false,  Parameters::kVisRefineIterations())));
+		removedParameters_.insert(std::make_pair("LccBow/Force2D",                  std::make_pair(false,  Parameters::kVisForce2D())));
+		removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false,  Parameters::kRegVarianceFromInliersCount())));
+		removedParameters_.insert(std::make_pair("LccBow/PnPReprojError",           std::make_pair(false,  Parameters::kVisPnPReprojError())));
+		removedParameters_.insert(std::make_pair("LccBow/PnPFlags",                 std::make_pair(false,  Parameters::kVisPnPFlags())));
+		removedParameters_.insert(std::make_pair("LccBow/EpipolarGeometryVar",      std::make_pair(true,   Parameters::kVisEpipolarGeometryVar())));
+
+		removedParameters_.insert(std::make_pair("LccIcp/Type",                         std::make_pair(true,  Parameters::kRGBDIcpLoopClosureRefining())));
+
+		removedParameters_.insert(std::make_pair("LccIcp3/Decimation",                  std::make_pair(false, "")));
+		removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth",                    std::make_pair(false, "")));
+		removedParameters_.insert(std::make_pair("LccIcp3/VoxelSize",                   std::make_pair(false, Parameters::kIcpVoxelSize())));
+		removedParameters_.insert(std::make_pair("LccIcp3/Samples",                     std::make_pair(false, Parameters::kIcpDownsamplingStep())));
+		removedParameters_.insert(std::make_pair("LccIcp3/MaxCorrespondenceDistance",   std::make_pair(false, Parameters::kIcpMaxCorrespondenceDistance())));
+		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("LccIcp2/MaxCorrespondenceDistance",   std::make_pair(true,  Parameters::kIcpMaxCorrespondenceDistance())));
+		removedParameters_.insert(std::make_pair("LccIcp2/Iterations",                  std::make_pair(true,  Parameters::kIcpIterations())));
+		removedParameters_.insert(std::make_pair("LccIcp2/CorrespondenceRatio",         std::make_pair(true,  Parameters::kIcpCorrespondenceRatio())));
+		removedParameters_.insert(std::make_pair("LccIcp2/VoxelSize",                   std::make_pair(true,  Parameters::kIcpVoxelSize())));
+	}
+	return removedParameters_;
+}
+
+const ParametersMap & Parameters::getBackwardCompatibilityMap()
+{
+	if(backwardCompatibilityMap_.empty())
+	{
+		getRemovedParameters(); // make sure removedParameters is filled
+
+		// compatibility
+		for(std::map<std::string, std::pair<bool, std::string> >::iterator iter=removedParameters_.begin();
+			iter!=removedParameters_.end();
+			++iter)
+		{
+			if(iter->second.first)
+			{
+				backwardCompatibilityMap_.insert(ParametersPair(iter->second.second, iter->first));
+			}
+		}
+	}
+	return backwardCompatibilityMap_;
+}
+
 std::string Parameters::getDescription(const std::string & paramKey)
 {
 	std::string description;

corelib/src/RegistrationIcp.cpp

@@ -0,0 +1,366 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include <rtabmap/core/RegistrationIcp.h>
+#include <rtabmap/core/util3d_registration.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>
+#include <pcl/io/pcd_io.h>
+
+namespace rtabmap {
+
+RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) :
+	_icpMaxTranslation(Parameters::defaultIcpMaxTranslation()),
+	_icpMaxRotation(Parameters::defaultIcpMaxRotation()),
+	_icp2D(Parameters::defaultIcp2D()),
+	_icpVoxelSize(Parameters::defaultIcpVoxelSize()),
+	_icpDownsamplingStep(Parameters::defaultIcpDownsamplingStep()),
+	_icpMaxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
+	_icpMaxIterations(Parameters::defaultIcpIterations()),
+	_icpCorrespondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
+	_icpPointToPlane(Parameters::defaultIcpPointToPlane()),
+	_icpPointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors())
+{
+	this->parseParameters(parameters);
+}
+
+void RegistrationIcp::parseParameters(const ParametersMap & parameters)
+{
+	Registration::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _icpMaxTranslation);
+	Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _icpMaxRotation);
+	Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D);
+	Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _icpVoxelSize);
+	Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _icpDownsamplingStep);
+	Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance);
+	Parameters::parse(parameters, Parameters::kIcpIterations(), _icpMaxIterations);
+	Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _icpCorrespondenceRatio);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _icpPointToPlane);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors);
+
+	UASSERT_MSG(_icpVoxelSize >= 0, uFormat("value=%d", _icpVoxelSize).c_str());
+	UASSERT_MSG(_icpDownsamplingStep >= 0, uFormat("value=%d", _icpDownsamplingStep).c_str());
+	UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str());
+	UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str());
+	UASSERT_MSG(_icpCorrespondenceRatio >=0.0f && _icpCorrespondenceRatio <=1.0f, uFormat("value=%f", _icpCorrespondenceRatio).c_str());
+	UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str());
+}
+
+Transform RegistrationIcp::computeTransformation(
+			const Signature & fromSignature,
+			const Signature & toSignature,
+			Transform guess,
+			std::string * rejectedMsg,
+			int * inliersOut,
+			float * varianceOut,
+			float * inliersRatioOut)
+{
+	return computeTransformation(
+			fromSignature.sensorData(),
+			toSignature.sensorData(),
+			guess,
+			rejectedMsg,
+			inliersOut,
+			varianceOut,
+			inliersRatioOut);
+}
+
+Transform RegistrationIcp::computeTransformation(
+			const SensorData & dataFrom,
+			const SensorData & dataTo,
+			Transform guess,
+			std::string * rejectedMsg,
+			int * inliersOut,
+			float * varianceOut,
+			float * inliersRatioOut)
+{
+	UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
+	UDEBUG("Voxel size=%f", _icpVoxelSize);
+	UDEBUG("2D=%d", _icp2D?1:0);
+	UDEBUG("PointToPlane=%d", _icpPointToPlane?1:0);
+	UDEBUG("Normal neighborhood=%d", _icpPointToPlaneNormalNeighbors);
+	UDEBUG("Max corrrespondence distance=%f", _icpMaxCorrespondenceDistance);
+	UDEBUG("Max Iterations=%d", _icpMaxIterations);
+	UDEBUG("Variance from inliers count=%d", _bowVarianceFromInliersCount?1:0);
+	UDEBUG("Correspondence Ratio=%f", _icpCorrespondenceRatio);
+	UDEBUG("Max translation=%f", _icpMaxTranslation);
+	UDEBUG("Max rotation=%f", _icpMaxRotation);
+	UDEBUG("Downsampling step=%d", _icpDownsamplingStep);
+
+	std::string msg;
+	Transform transform;
+
+	// ICP with guess transform
+	if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
+	{
+		int maxLaserScans = dataTo.laserScanMaxPts();
+		cv::Mat fromScan = dataFrom.laserScanRaw();
+		cv::Mat toScan = dataTo.laserScanRaw();
+		if(_icpDownsamplingStep>1)
+		{
+			fromScan = util3d::downsample(fromScan, _icpDownsamplingStep);
+			toScan = util3d::downsample(toScan, _icpDownsamplingStep);
+			maxLaserScans/=_icpDownsamplingStep;
+		}
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
+		pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
+
+		if(toCloud->size() && fromCloud->size())
+		{
+			//filtering
+			pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
+			pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
+			bool filtered = false;
+			if(_icpVoxelSize > 0.0f)
+			{
+				fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _icpVoxelSize);
+				toCloudFiltered = util3d::voxelize(toCloudFiltered, _icpVoxelSize);
+				filtered = true;
+			}
+
+			Transform icpT;
+			bool hasConverged = false;
+			float correspondencesRatio = 0.0f;
+			int correspondences = 0;
+			double variance = 1.0;
+			bool correspondencesComputed = false;
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
+			if(!_icp2D) // 3D ICP
+			{
+				if(_icpPointToPlane)
+				{
+					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _icpPointToPlaneNormalNeighbors);
+					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _icpPointToPlaneNormalNeighbors);
+
+					std::vector<int> indices;
+					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
+					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+
+					if(toCloudNormals->size() && fromCloudNormals->size())
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr newCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+						icpT = util3d::icpPointToPlane(
+								toCloudNormals,
+								fromCloudNormals,
+							   _icpMaxCorrespondenceDistance,
+							   _icpMaxIterations,
+							   hasConverged,
+							   *newCloudNormalsRegistered);
+						if(!filtered &&
+							!icpT.isNull() &&
+							hasConverged)
+						{
+							util3d::computeVarianceAndCorrespondences(
+								newCloudNormalsRegistered,
+								fromCloudNormals,
+								_icpMaxCorrespondenceDistance,
+								variance,
+								correspondences);
+							correspondencesComputed = true;
+						}
+					}
+				}
+				else
+				{
+					icpT = util3d::icp(
+							toCloudFiltered,
+							fromCloudFiltered,
+							_icpMaxCorrespondenceDistance,
+							_icpMaxIterations,
+							hasConverged,
+							*newCloudRegistered);
+				}
+			}
+			else // 2D ICP
+			{
+				icpT = util3d::icp2D(
+							toCloudFiltered,
+							fromCloudFiltered,
+						   _icpMaxCorrespondenceDistance,
+						   _icpMaxIterations,
+						   hasConverged,
+						   *newCloudRegistered);
+			}
+
+			/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
+			pcl::io::savePCDFile("toCloud.pcd", *toCloud);
+			UWARN("saved fromCloud.pcd and toCloud.pcd");
+			if(!icpT.isNull())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudTmp = util3d::transformPointCloud(toCloud, icpT);
+				pcl::io::savePCDFile("newCloudFinal.pcd", *toCloudTmp);
+				UWARN("saved toCloudFinal.pcd");
+			}*/
+
+			if(!icpT.isNull() &&
+				hasConverged)
+			{
+				float ix,iy,iz, iroll,ipitch,iyaw;
+				icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
+				if((_icpMaxTranslation>0.0f &&
+					(fabs(ix) > _icpMaxTranslation ||
+					 fabs(iy) > _icpMaxTranslation ||
+					 fabs(iz) > _icpMaxTranslation))
+					||
+					(_icpMaxRotation>0.0f &&
+					 (fabs(iroll) > _icpMaxRotation ||
+					  fabs(ipitch) > _icpMaxRotation ||
+					  fabs(iyaw) > _icpMaxRotation)))
+				{
+					msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
+							uMax3(fabs(ix), fabs(iy), fabs(iz)),
+							uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)),
+							_icpMaxTranslation,
+							_icpMaxRotation);
+					UINFO(msg.c_str());
+				}
+				else
+				{
+					if(!correspondencesComputed)
+					{
+						if(filtered)
+						{
+							fromCloud = util3d::transformPointCloud(fromCloud, icpT);
+						}
+						else
+						{
+							fromCloud = newCloudRegistered;
+						}
+
+						util3d::computeVarianceAndCorrespondences(
+								toCloud,
+								fromCloud,
+								_icpMaxCorrespondenceDistance,
+								variance,
+								correspondences);
+					}
+
+					// verify if there are enough correspondences
+					if(maxLaserScans)
+					{
+						correspondencesRatio = float(correspondences)/float(maxLaserScans);
+					}
+					else
+					{
+						UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
+								dataTo.id());
+						correspondencesRatio = float(correspondences)/float(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size());
+					}
+
+					UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
+							dataTo.id(), dataFrom.id(),
+							hasConverged?"true":"false",
+							variance,
+							correspondences,
+							maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
+							correspondencesRatio*100.0f);
+
+					if(_bowVarianceFromInliersCount)
+					{
+						variance = correspondencesRatio > 0?1.0/double(correspondencesRatio):1.0;
+					}
+
+					if(varianceOut)
+					{
+						*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
+					}
+					if(inliersOut)
+					{
+						*inliersOut = correspondences;
+					}
+					if(inliersRatioOut)
+					{
+						*inliersRatioOut = correspondencesRatio;
+					}
+
+					if(correspondencesRatio < _icpCorrespondenceRatio)
+					{
+						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
+								correspondences, correspondencesRatio, _icpCorrespondenceRatio);
+						UINFO(msg.c_str());
+					}
+					else
+					{
+						transform = guess*icpT;
+					}
+				}
+			}
+			else
+			{
+				msg = uFormat("Cannot compute transform (converged=%s var=%f)",
+						hasConverged?"true":"false", variance);
+				UINFO(msg.c_str());
+			}
+
+			// still compute the variance for information
+			/*if(variance == 1 && varianceOut)
+			{
+				util3d::computeVarianceAndCorrespondences(
+					toCloudFiltered,
+					fromCloudFiltered,
+					_icpMaxCorrespondenceDistance,
+					variance,
+					correspondences);
+				if(variance > 0)
+				{
+					*varianceOut = variance;
+				}
+			}*/
+		}
+		else
+		{
+			msg = "Laser scans empty ?!?";
+			UWARN(msg.c_str());
+		}
+	}
+	else
+	{
+		msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
+				dataTo.id(), dataTo.laserScanRaw().total(),
+				dataFrom.id(), dataFrom.laserScanRaw().total());
+		UERROR(msg.c_str());
+	}
+
+
+	if(rejectedMsg)
+	{
+		*rejectedMsg = msg;
+	}
+
+	UDEBUG("New transform = %s", transform.prettyPrint().c_str());
+	return transform;
+}
+
+}

corelib/src/RegistrationVis.cpp

@@ -0,0 +1,384 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+
+#include <rtabmap/core/RegistrationVis.h>
+#include <rtabmap/core/util3d_motion_estimation.h>
+#include <rtabmap/core/util3d_features.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+
+namespace rtabmap {
+
+RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
+		_bowMinInliers(Parameters::defaultVisMinInliers()),
+		_bowInlierDistance(Parameters::defaultVisInlierDistance()),
+		_bowIterations(Parameters::defaultVisIterations()),
+		_bowRefineIterations(Parameters::defaultVisRefineIterations()),
+		_bowForce2D(Parameters::defaultVisForce2D()),
+		_bowEpipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
+		_bowEstimationType(Parameters::defaultVisEstimationType()),
+		_bowPnPReprojError(Parameters::defaultVisPnPReprojError()),
+		_bowPnPFlags(Parameters::defaultVisPnPFlags()),
+		_reextractNNType(Parameters::defaultVisNNType()),
+		_reextractNNDR(Parameters::defaultVisNNDR()),
+		_reextractFeatureType(Parameters::defaultVisFeatureType()),
+		_reextractMaxWords(Parameters::defaultVisMaxFeatures()),
+		_reextractMaxDepth(Parameters::defaultVisMaxDepth()),
+		_reextractMinDepth(Parameters::defaultVisMinDepth()),
+		_reextractRoiRatios(Parameters::defaultVisRoiRatios()),
+		_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
+		_subPixIterations(Parameters::defaultKpSubPixIterations()),
+		_subPixEps(Parameters::defaultKpSubPixEps())
+{
+	this->parseParameters(parameters);
+}
+
+void RegistrationVis::parseParameters(const ParametersMap & parameters)
+{
+	Registration::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kVisMinInliers(), _bowMinInliers);
+	Parameters::parse(parameters, Parameters::kVisInlierDistance(), _bowInlierDistance);
+	Parameters::parse(parameters, Parameters::kVisIterations(), _bowIterations);
+	Parameters::parse(parameters, Parameters::kVisRefineIterations(), _bowRefineIterations);
+	Parameters::parse(parameters, Parameters::kVisForce2D(), _bowForce2D);
+	Parameters::parse(parameters, Parameters::kVisEstimationType(), _bowEstimationType);
+	Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _bowEpipolarGeometryVar);
+	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _bowPnPReprojError);
+	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _bowPnPFlags);
+
+	Parameters::parse(parameters, Parameters::kVisNNType(), _reextractNNType);
+	Parameters::parse(parameters, Parameters::kVisNNDR(), _reextractNNDR);
+	Parameters::parse(parameters, Parameters::kVisFeatureType(), _reextractFeatureType);
+	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _reextractMaxWords);
+	Parameters::parse(parameters, Parameters::kVisMaxDepth(), _reextractMaxDepth);
+
+	Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
+	Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
+	Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
+
+	UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
+	UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
+	UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
+}
+
+Transform RegistrationVis::computeTransformation(
+			const Signature & fromSignature,
+			const Signature & toSignature,
+			Transform guess, // guess is ignored for RegistrationVis
+			std::string * rejectedMsg,
+			int * inliersOut,
+			float * varianceOut,
+			float * inliersRatioOut)
+{
+	Transform transform;
+	std::string msg;
+	// Guess transform from visual words
+
+	int inliersCount= 0;
+	double variance = 1.0;
+
+	// Extract features?
+	const std::multimap<int, cv::KeyPoint> * wordsFrom = 0;
+	const std::multimap<int, cv::KeyPoint> * wordsTo = 0;
+	const std::multimap<int, pcl::PointXYZ> * words3From = 0;
+	const std::multimap<int, pcl::PointXYZ> * words3To = 0;
+	std::multimap<int, cv::KeyPoint> extractedWordsFrom, extractedWordsTo;
+	std::multimap<int, pcl::PointXYZ> extractedWords3From, extractedWords3To;
+	if(fromSignature.getWords().size() == 0 && toSignature.getWords().size() == 0)
+	{
+		// Use the Memory class to extract features
+		ParametersMap customParameters;
+		// override some parameters
+		uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
+		uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+		uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
+		uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
+		uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
+		uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+		uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
+		uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
+		uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
+		uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+		uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
+		uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_reextractMinDepth)));
+		uInsert(customParameters, ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(_subPixEps)));
+		uInsert(customParameters, ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(_subPixIterations)));
+		uInsert(customParameters, ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(_subPixWinSize)));
+		uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
+		uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _reextractRoiRatios));
+		uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
+
+		Memory memory(customParameters);
+
+		// Add signatures
+		SensorData dataFrom = fromSignature.sensorData();
+		SensorData dataTo = toSignature.sensorData();
+		// make sure there are no features already in the SensorData
+		dataFrom.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+		dataTo.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+
+		UTimer timeT;
+
+		memory.update(dataFrom);
+		if(memory.getLastWorkingSignature() == 0)
+		{
+			UWARN("Failed to extract features for node %d", dataFrom.id());
+		}
+		else
+		{
+			extractedWordsFrom = memory.getLastWorkingSignature()->getWords();
+			extractedWords3From = memory.getLastWorkingSignature()->getWords3();
+			UDEBUG("timeTo = %fs", timeT.ticks());
+
+			memory.update(dataTo);
+			if(memory.getLastWorkingSignature() == 0)
+			{
+				UWARN("Failed to extract features for node %d", dataTo.id());
+			}
+			else
+			{
+				extractedWordsTo = memory.getLastWorkingSignature()->getWords();
+				extractedWords3To = memory.getLastWorkingSignature()->getWords3();
+				UDEBUG("timeFrom = %fs", timeT.ticks());
+			}
+		}
+
+		wordsFrom = &extractedWordsFrom;
+		wordsTo = &extractedWordsTo;
+		words3From = &extractedWords3From;
+		words3To = &extractedWords3To;
+	}
+	else
+	{
+		wordsFrom = &fromSignature.getWords();
+		wordsTo = &toSignature.getWords();
+		words3From = &fromSignature.getWords3();
+		words3To = &toSignature.getWords3();
+	}
+
+	if(_bowEstimationType == 2) // Epipolar Geometry
+	{
+		if(!toSignature.sensorData().stereoCameraModel().isValid() &&
+		   (toSignature.sensorData().cameraModels().size() != 1 ||
+			!toSignature.sensorData().cameraModels()[0].isValid()))
+		{
+			UERROR("Calibrated camera required (multi-cameras not supported).");
+		}
+		else if((int)wordsFrom->size() >= _bowMinInliers &&
+				(int)wordsTo->size() >= _bowMinInliers)
+		{
+			UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid()));
+			const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
+
+			// we only need the camera transform, send guess words3 for scale estimation
+			Transform cameraTransform;
+			std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
+					*wordsFrom,
+					*wordsTo,
+					cameraModel,
+					cameraTransform,
+					_bowIterations,
+					_bowPnPReprojError,
+					_bowPnPFlags, // cv::SOLVEPNP_ITERATIVE
+					1.0f,
+					0.99f,
+					*words3From, // for scale estimation
+					&variance);
+
+			inliersCount = (int)inliers3D.size();
+
+			if(!cameraTransform.isNull())
+			{
+				if((int)inliers3D.size() >= _bowMinInliers)
+				{
+					if(variance <= _bowEpipolarGeometryVar)
+					{
+						transform = cameraTransform;
+					}
+					else
+					{
+						msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _bowEpipolarGeometryVar, variance);
+						UINFO(msg.c_str());
+					}
+				}
+				else
+				{
+					msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _bowMinInliers);
+					UINFO(msg.c_str());
+				}
+			}
+			else
+			{
+				msg = uFormat("No camera transform found");
+				UINFO(msg.c_str());
+			}
+		}
+		else if(words3From->size() == 0)
+		{
+			msg = uFormat("No 3D guess words found");
+			UWARN(msg.c_str());
+		}
+		else
+		{
+			msg = uFormat("No camera model");
+			UWARN(msg.c_str());
+		}
+	}
+	else if(_bowEstimationType == 1) // PnP
+	{
+		if(!toSignature.sensorData().stereoCameraModel().isValid() &&
+		   (toSignature.sensorData().cameraModels().size() != 1 ||
+			!toSignature.sensorData().cameraModels()[0].isValid()))
+		{
+			UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
+					toSignature.id(),
+					(int)toSignature.sensorData().cameraModels().size(),
+					toSignature.sensorData().stereoCameraModel().isValid()?1:0,
+					toSignature.getWeight());
+		}
+		else
+		{
+			// 3D to 2D
+			if((int)words3From->size() >= _bowMinInliers &&
+			   (int)wordsTo->size() >= _bowMinInliers)
+			{
+				UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid()));
+				const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
+
+				std::vector<int> inliersV;
+				transform = util3d::estimateMotion3DTo2D(
+						uMultimapToMap(*words3From),
+						uMultimapToMap(*wordsTo),
+						cameraModel,
+						_bowMinInliers,
+						_bowIterations,
+						_bowPnPReprojError,
+						_bowPnPFlags,
+						Transform::getIdentity(),
+						uMultimapToMap(*words3To),
+						&variance,
+						0,
+						&inliersV);
+				inliersCount = (int)inliersV.size();
+				if(transform.isNull())
+				{
+					msg = uFormat("Not enough inliers %d/%d between %d and %d",
+							inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
+					UINFO(msg.c_str());
+				}
+			}
+			else
+			{
+				msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
+						(int)words3From->size(), (int)wordsTo->size(), _bowMinInliers);
+				UINFO(msg.c_str());
+			}
+		}
+
+	}
+	else
+	{
+		// 3D -> 3D
+		if((int)words3From->size() >= _bowMinInliers &&
+		   (int)words3To->size() >= _bowMinInliers)
+		{
+			std::vector<int> inliersV;
+			transform = util3d::estimateMotion3DTo3D(
+					uMultimapToMap(*words3From),
+					uMultimapToMap(*words3To),
+					_bowMinInliers,
+					_bowInlierDistance,
+					_bowIterations,
+					_bowRefineIterations,
+					&variance,
+					0,
+					&inliersV);
+			inliersCount = (int)inliersV.size();
+			if(transform.isNull())
+			{
+				msg = uFormat("Not enough inliers %d/%d between %d and %d",
+						inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
+				UINFO(msg.c_str());
+			}
+		}
+		else
+		{
+			msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
+					(int)words3From->size(), (int)words3To->size(), _bowMinInliers);
+			UINFO(msg.c_str());
+		}
+	}
+
+	if(!transform.isNull())
+	{
+		// verify if it is a 180 degree transform, well verify > 90
+		float x,y,z, roll,pitch,yaw;
+		transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+		if(fabs(roll) > CV_PI/2 ||
+		   fabs(pitch) > CV_PI/2 ||
+		   fabs(yaw) > CV_PI/2)
+		{
+			transform.setNull();
+			msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
+					roll, pitch, yaw);
+			UWARN(msg.c_str());
+		}
+		else if(_bowForce2D)
+		{
+			UDEBUG("Forcing 2D...");
+			transform = Transform(x,y,0, 0, 0, yaw);
+		}
+	}
+
+	if(_bowVarianceFromInliersCount)
+	{
+		variance = inliersCount > 0?1.0/double(inliersCount):1.0;
+	}
+
+	if(rejectedMsg)
+	{
+		*rejectedMsg = msg;
+	}
+	if(inliersOut)
+	{
+		*inliersOut = inliersCount;
+	}
+	if(varianceOut)
+	{
+		*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
+	}
+	UDEBUG("transform=%s", transform.prettyPrint().c_str());
+	return transform;
+}
+
+}

corelib/src/Rtabmap.cpp

@@ -90,8 +90,8 @@ Rtabmap::Rtabmap() :
 	_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
 	_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
 	_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
-	_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
+	_loopClosureIcpRefining(Parameters::defaultRGBDIcpLoopClosureRefining()),
-	_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
+	_odomIcpRefining(Parameters::defaultRGBDIcpOdomRefining()),
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
 	_scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
@@ -100,15 +100,10 @@ Rtabmap::Rtabmap() :
 	_localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()),
 	_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
 	_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
+	_localPathScansMerged(Parameters::defaultRGBDLocalLoopDetectionPathScansMerged()),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
 	_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
-	_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
-	_reextractNNType(Parameters::defaultLccReextractNNType()),
-	_reextractNNDR(Parameters::defaultLccReextractNNDR()),
-	_reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
-	_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
-	_reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
@@ -402,7 +397,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
-	Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching);
+	Parameters::parse(parameters, Parameters::kRGBDIcpOdomRefining(), _odomIcpRefining);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
 	Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
@@ -411,38 +406,20 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
+	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathScansMerged(), _localPathScansMerged);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
-	Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
-	Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
-	Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
-	Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
-	Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
-	Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
 	Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
 	Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
 	Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
 	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
+	Parameters::parse(parameters, Parameters::kRGBDIcpLoopClosureRefining(), _loopClosureIcpRefining);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
 
-	// RGB-D SLAM stuff
-	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
-	{
-		int icpType = std::atoi((*iter).second.c_str());
-		if(icpType >= 0 && icpType <= 2)
-		{
-			_globalLoopClosureIcpType = icpType;
-		}
-		else
-		{
-			UERROR("Icp type must be 0, 1 or 2 (value=%d)", icpType);
-		}
-	}
-
 	// By default, we create our strategies if they are not already created.
 	// If they already exists, we check the parameters if a change is requested
 
@@ -1023,17 +1000,17 @@ bool Rtabmap::process(
 			//============================================================
 			// Scan matching
 			//============================================================
-			if(_poseScanMatching &&
+			if(_odomIcpRefining &&
 				!signature->sensorData().laserScanCompressed().empty() &&
 				rehearsedId == 0) // don't do it if rehearsal happened
 			{
 				UINFO("Odometry correction by scan matching");
-				Transform guess = signature->getLinks().begin()->second.transform();
+				Transform guess = signature->getLinks().begin()->second.transform().inverse();
-				double variance = 1.0;
+				float variance = 1.0f;
 				int inliers = 0;
 				float inliersRatio = 0;
 				std::string rejectedMsg;
-				Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
+				Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &rejectedMsg, &inliers, &variance, &inliersRatio);
 				if(!t.isNull())
 				{
 					UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
@@ -1043,14 +1020,14 @@ bool Rtabmap::process(
 							signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
 							t.prettyPrint().c_str());
 					UASSERT(variance > 0.0);
-					_memory->updateLink(signature->id(), oldId, t, variance, variance);
+					_memory->updateLink(oldId, signature->id(), t, variance, variance);
 
 					if(_optimizeFromGraphEnd)
 					{
 						// update all previous nodes
 						// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
 						// parameters just changed state, we should put back all poses without map correction.
-						Transform u = guess.inverse() * t;
+						Transform u = guess * t.inverse();
 						std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
 						UASSERT(jter!=_optimizedPoses.end());
 						Transform up = jter->second * u * jter->second.inverse();
@@ -1074,6 +1051,7 @@ bool Rtabmap::process(
 				statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
 				statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
 				statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
+				statistics_.addStatistic(Statistics::kOdomCorrectionPts(), signature->sensorData().laserScanRaw().cols);
 			}
 			timeScanMatching = timer.ticks();
 			ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
@@ -1179,12 +1157,12 @@ bool Rtabmap::process(
 				{
 					std::string rejectedMsg;
 					UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
-					double variance = 1.0;
+					float variance = 1.0f;
 					int inliers = -1;
-					Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
+					Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance);
-					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
+					if(!transform.isNull() && _loopClosureIcpRefining)
 					{
-						transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
+						transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance);
 					}
 					if(!transform.isNull())
 					{
@@ -1733,72 +1711,16 @@ bool Rtabmap::process(
 	{
 		//Compute transform if metric data are present
 		Transform transform;
-		double variance = 1;
+		float variance = 1.0f;
 		if(_rgbdSlamMode)
 		{
 			std::string rejectedMsg;
-			if(_reextractLoopClosureFeatures)
+
+			transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance);
+
+			if(!transform.isNull() && _loopClosureIcpRefining)
 			{
-				ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
+				transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance);
-				// override some parameters
-				uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
-				uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-				uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
-				uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
-				uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
-				uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
-				uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
-				uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
-				uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
-				uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
-				uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
-				uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
-				uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
-				uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
-
-				//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
-				//{
-				//	UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
-				//}
-
-				Memory memory(customParameters);
-
-				UTimer timeT;
-
-				// Add signatures
-				SensorData dataFrom = data;
-				dataFrom.setId(signature->id());
-				SensorData dataTo = _memory->getNodeData(_loopClosureHypothesis.first, true);
-				UDEBUG("timeTo = %fs", timeT.ticks());
-
-				if(!dataFrom.depthOrRightRaw().empty() &&
-				   !dataTo.depthOrRightRaw().empty() &&
-				   dataFrom.id() != Memory::kIdInvalid &&
-				   dataTo.id() != Memory::kIdInvalid)
-				{
-					memory.update(dataTo);
-					UDEBUG("timeUpTo = %fs", timeT.ticks());
-					memory.update(dataFrom);
-					UDEBUG("timeUpFrom = %fs", timeT.ticks());
-
-					transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
-					UDEBUG("timeTransform = %fs", timeT.ticks());
-				}
-				else
-				{
-					// Fallback to normal way (raw data not kept in database...)
-					UWARN("Loop closure: Some images not found in memory for re-extracting "
-						  "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
-					transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
-				}
-			}
-			else
-			{
-				transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
-			}
-			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
-			{
-				transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
 			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
@@ -1896,70 +1818,11 @@ bool Rtabmap::process(
 						(_localPathFilteringRadius <= 0.0f ||
 						 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
 					{
-						double variance = 1.0;
+						float variance = 1.0f;
-						Transform transform;
+						Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance);
-						if(_reextractLoopClosureFeatures)
+						if(!transform.isNull() && _loopClosureIcpRefining)
 						{
-							ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
+							transform  = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance);
-							// override some parameters
-							uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
-							uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-							uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
-							uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
-							uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
-							uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
-							uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
-							uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
-							uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
-							uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
-							uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
-							uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
-							uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
-							uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
-
-							//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
-							//{
-							//	UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
-							//}
-
-							Memory memory(customParameters);
-
-							UTimer timeT;
-
-							// Add signatures
-							SensorData dataFrom = data;
-							dataFrom.setId(signature->id());
-							SensorData dataTo = _memory->getNodeData(nearestId, true);
-							UDEBUG("timeTo = %fs", timeT.ticks());
-
-							if(!dataFrom.depthOrRightRaw().empty() &&
-							   !dataTo.depthOrRightRaw().empty() &&
-							   dataFrom.id() != Memory::kIdInvalid &&
-							   dataTo.id() != Memory::kIdInvalid)
-							{
-								memory.update(dataTo);
-								UDEBUG("timeUpTo = %fs", timeT.ticks());
-								memory.update(dataFrom);
-								UDEBUG("timeUpFrom = %fs", timeT.ticks());
-
-								transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), 0, 0, &variance);
-								UDEBUG("timeTransform = %fs", timeT.ticks());
-							}
-							else
-							{
-								// Fallback to normal way (raw data not kept in database...)
-								UWARN("Loop closure: Some images not found in memory for re-extracting "
-									  "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
-								transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
-							}
-						}
-						else
-						{
-							transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
-						}
-						if(!transform.isNull() && _globalLoopClosureIcpType > 0)
-						{
-							transform  = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
 						}
 						if(!transform.isNull())
 						{
@@ -2019,38 +1882,48 @@ bool Rtabmap::process(
 						   (_localPathFilteringRadius <= 0.0f ||
 							_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
 						{
-							// Assemble scans in the path and do ICP only
+							if(!_localPathScansMerged)
-							if(_localPathOdomPosesUsed)
 							{
-								//optimize the path's poses locally
+								//only keep the nearest node
-								path = optimizeGraph(nearestId, uKeysSet(path), false);
+								std::map<int, Transform> tmp;
-								// transform local poses in optimized graph referential
+								tmp.insert(*path.find(nearestId));
-								UASSERT(uContains(path, nearestId));
+								path = tmp;
-								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
+							}
-								for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
+							else
+							{
+								// Assemble scans in the path and do ICP only
+								if(_localPathOdomPosesUsed)
 								{
-									jter->second = t * jter->second;
+									//optimize the path's poses locally
+									path = optimizeGraph(nearestId, uKeysSet(path), false);
+									// transform local poses in optimized graph referential
+									UASSERT(uContains(path, nearestId));
+									Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
+									for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
+									{
+										jter->second = t * jter->second;
+									}
+								}
+								if(path.size() > 2 && _localPathFilteringRadius > 0.0f)
+								{
+									// path filtering
+									std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
+									// make sure the nearest and farthest poses are still here
+									filteredPath.insert(*path.find(nearestId));
+									filteredPath.insert(*path.begin());
+									filteredPath.insert(*path.rbegin());
+									path = filteredPath;
 								}
 							}
-							if(_localPathFilteringRadius > 0.0f)
-							{
-								// path filtering
-								std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
-								// make sure the nearest and farthest poses are still here
-								filteredPath.insert(*path.find(nearestId));
-								filteredPath.insert(*path.begin());
-								filteredPath.insert(*path.rbegin());
-								path = filteredPath;
-							}
 
-							if(path.size() > 2) // more than current+nearest
+							if(path.size() > 0)
 							{
 								// add current node to poses
 								path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
 								//The nearest will be the reference for a loop closure transform
 								if(signature->getLinks().find(nearestId) == signature->getLinks().end())
 								{
-									double variance = 1.0;
+									float variance = 1.0f;
 									Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
 									if(!transform.isNull())
 									{
@@ -2340,7 +2213,7 @@ bool Rtabmap::process(
 
 			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
-			statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
+			statistics_.addStatistic(Statistics::kTimingOdom_correction(), timeScanMatching*1000);
 			statistics_.addStatistic(Statistics::kTimingLocal_detection_TIME(), timeLocalTimeDetection*1000);
 			statistics_.addStatistic(Statistics::kTimingLocal_detection_SPACE(), timeLocalSpaceDetection*1000);
 			statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
@@ -3836,12 +3709,41 @@ void Rtabmap::readParameters(const std::string & configFile, ParametersMap & par
 			else
 			{
 				key = uReplaceChar(key, '\\', '/'); // Ini files use \ by default for separators, so replace them
+
+				// look for old parameter name
+				bool addParameter = true;
+				std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
+				if(oldIter!=Parameters::getRemovedParameters().end())
+				{
+					addParameter = oldIter->second.first;
+					if(addParameter)
+					{
+						key = oldIter->second.second;
+						UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
+								oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second);
+					}
+					else if(oldIter->second.second.empty())
+					{
+						UWARN("Parameter \"%s\" doesn't exist anymore.",
+									oldIter->first.c_str());
+					}
+					else
+					{
+						UWARN("Parameter \"%s\" doesn't exist anymore, you may want to use this similar parameter \"%s\":\"%s\".",
+									oldIter->first.c_str(), oldIter->second.second.c_str(), Parameters::getDescription(oldIter->second.second).c_str());
+					}
+
+				}
+
 				ParametersMap::iterator jter = parameters.find(key);
 				if(jter != parameters.end())
 				{
 					parameters.erase(jter);
 				}
-				parameters.insert(ParametersPair(key, (*iter).second));
+				if(addParameter)
+				{
+					parameters.insert(ParametersPair(key, iter->second));
+				}
 			}
 		}
 	}

corelib/src/RtabmapThread.cpp

@@ -553,12 +553,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			}
 			if(ignoreFrame)
 			{
-				// remove data from the frame, keeping only constraints
+				// set negative id so rtabmap will detect it as an intermediate node
-				SensorData tmp(
+				SensorData tmp = odomEvent.data();
-						cv::Mat(),
+				tmp.setId(-1);
-						odomEvent.data().id(),
+				tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
-						odomEvent.data().stamp(),
-						odomEvent.data().userDataRaw());
 				_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
 			}
 			else

corelib/src/SensorData.cpp

@@ -199,7 +199,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2)
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -306,7 +306,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2)
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -412,7 +412,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = right;
 	}
 
-	if(laserScan.type() == CV_32FC2)
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
 	{
 		_laserScanRaw = laserScan;
 	}

corelib/src/Transform.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
 #include <iomanip>
 
 namespace rtabmap {
@@ -315,4 +316,45 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
 					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
 }
 
+/**
+ * Format (6 values): x y z roll pitch yaw.
+ * Format (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz].
+ */
+Transform Transform::fromString(const std::string & string)
+{
+	Transform t;
+	std::list<std::string> list = uSplit(string, ' ');
+	if(list.size() == 6 || list.size() == 9 || list.size() == 12)
+	{
+		std::vector<float> numbers(list.size());
+		int i = 0;
+		for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
+		{
+			numbers[i++] = uStr2Float(*iter);
+		}
+
+		if(numbers.size() == 6)
+		{
+			t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
+		}
+		else if(numbers.size() == 9)
+		{
+			t = Transform(numbers[0], numbers[1], numbers[2], 0,
+						  numbers[3], numbers[4], numbers[5], 0,
+						  numbers[6], numbers[7], numbers[8], 0);
+		}
+		else if(numbers.size() == 12)
+		{
+			t = Transform(numbers[0], numbers[1], numbers[2], numbers[9],
+						  numbers[3], numbers[4], numbers[5], numbers[10],
+						  numbers[6], numbers[7], numbers[8], numbers[11]);
+		}
+	}
+	else
+	{
+		UERROR("Local transform is wrong! must have 6 or 9 items (%s)", string.c_str());
+	}
+	return t;
+}
+
 }

corelib/src/util3d.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
@@ -550,7 +551,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 
 					if(tmp->size() && samples)
 					{
-						tmp = util3d::sampling(tmp, samples);
+						tmp = util3d::randomSampling(tmp, samples);
 						filtered = true;
 					}
 
@@ -685,7 +686,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 
 					if(tmp->size() && samples)
 					{
-						tmp = util3d::sampling(tmp, samples);
+						tmp = util3d::randomSampling(tmp, samples);
 						filtered = true;
 					}
 
@@ -789,64 +790,60 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 	return scan;
 }
 
-cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
 {
-	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
+	bool nullTransform = transform.isNull();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
+		if(!nullTransform)
-		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
+		{
+			pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
+			laserScan.at<cv::Vec3f>(i)[0] = pt.x;
+			laserScan.at<cv::Vec3f>(i)[1] = pt.y;
+			laserScan.at<cv::Vec3f>(i)[2] = pt.z;
+		}
+		else
+		{
+			laserScan.at<cv::Vec3f>(i)[0] = cloud.at(i).x;
+			laserScan.at<cv::Vec3f>(i)[1] = cloud.at(i).y;
+			laserScan.at<cv::Vec3f>(i)[2] = cloud.at(i).z;
+		}
+
 	}
 	return laserScan;
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
+pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	output->resize(laserScan.cols);
+	bool nullTransform = transform.isNull();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
-		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
+		if(laserScan.type() == CV_32FC2)
-		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
+		{
+			output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
+			output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
+		}
+		else
+		{
+			output->at(i).x = laserScan.at<cv::Vec3f>(i)[0];
+			output->at(i).y = laserScan.at<cv::Vec3f>(i)[1];
+			output->at(i).z = laserScan.at<cv::Vec3f>(i)[2];
+		}
+		if(!nullTransform)
+		{
+			output->at(i) = pcl::transformPoint(output->at(i), transform3f);
+		}
 	}
 	return output;
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform)
-{
-	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
-	UASSERT(matrix.rows == 1);
-
-	Eigen::Affine3f t = tranform.toEigen3f();
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(matrix.cols);
-	if(matrix.channels() == 2)
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
-			cloud->at(i).z = 0.0f;
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	else // channels=3
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
-			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	return cloud;
-}
-
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,
@@ -950,6 +947,42 @@ void savePCDWords(
 	}
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
+{
+	UASSERT(dim > 0);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	long bytes = UFile::length(fileName);
+	if(bytes)
+	{
+		UASSERT(bytes % sizeof(float) == 0);
+		int32_t num = bytes/sizeof(float);
+		UASSERT(num % dim == 0);
+		float *data = (float*)malloc(num*sizeof(float));
+
+		// pointers
+		float *px = data+0;
+		float *py = data+1;
+		float *pz = data+2;
+		float *pr = data+3;
+
+		// load point cloud
+		FILE *stream;
+		stream = fopen (fileName.c_str(),"rb");
+		num = fread(data,sizeof(float),num,stream)/4;
+		cloud->resize(num);
+		for (int32_t i=0; i<num; i++) {
+			(*cloud)[i].x = *px;
+			(*cloud)[i].y = *py;
+			(*cloud)[i].z = *pz;
+			px+=4; py+=4; pz+=4; pr+=4;
+		}
+		fclose(stream);
+	}
+
+	return cloud;
+}
+
 }
 
 }

corelib/src/util3d_filtering.cpp

@@ -49,6 +49,101 @@ namespace rtabmap
 namespace util3d
 {
 
+cv::Mat downsample(
+		const cv::Mat & cloud,
+		int step)
+{
+	// 2D or 3D point clouds (laser scans)
+	UASSERT(cloud.type() == CV_32FC2 || cloud.type() == CV_32FC3);
+	UASSERT(step > 0);
+	cv::Mat output;
+	if(step == 1)
+	{
+		// no sampling
+		output = cloud.clone();
+	}
+	else
+	{
+		if(cloud.cols > step)
+		{
+			int finalSize = cloud.cols/step;
+			output = cv::Mat(1, finalSize, cloud.type());
+			int oi = 0;
+			for(unsigned int i=0; i<cloud.cols-step+1; i+=step)
+			{
+				cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
+				++oi;
+			}
+		}
+		else if(cloud.cols)
+		{
+			output = cv::Mat(1, 1, cloud.type());
+			cv::Mat(cloud, cv::Range::all(), cv::Range(0,1)).copyTo(output); // first point
+		}
+	}
+	return output;
+}
+pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int step)
+{
+	UASSERT(step > 0);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	if(step == 1)
+	{
+		// no sampling
+		*output = *cloud;
+	}
+	else
+	{
+		if(cloud->size() > step)
+		{
+			int finalSize = cloud->size()/step;
+			output->resize(finalSize);
+			int oi = 0;
+			for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
+			{
+				(*output)[oi++] = cloud->at(i);
+			}
+		}
+		else if(cloud->size())
+		{
+			output->push_back(cloud->at(0));
+		}
+	}
+	return output;
+}
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		int step)
+{
+	UASSERT(step > 0);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+	if(step == 1)
+	{
+		// no sampling
+		*output = *cloud;
+	}
+	else
+	{
+		if(cloud->size() > step)
+		{
+			int finalSize = cloud->size()/step;
+			output->resize(finalSize);
+			int oi = 0;
+			for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
+			{
+				(*output)[oi++] = cloud->at(i);
+			}
+		}
+		else if(cloud->size())
+		{
+			output->push_back(cloud->at(0));
+		}
+	}
+	return output;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float voxelSize)
@@ -87,7 +182,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
 }
 
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
+pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
 {
 	UASSERT(samples > 0);
@@ -98,7 +193,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
 	filter.filter(*output);
 	return output;
 }
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
 {
 	UASSERT(samples > 0);

corelib/src/util3d_registration.cpp

@@ -306,7 +306,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	// Set the maximum number of iterations (criterion 1)
 	icp.setMaximumIterations (maximumIterations);
 	// Set the transformation epsilon (criterion 2)
-	//icp.setTransformationEpsilon (transformationEpsilon);
+	//icp.setTransformationEpsilon (1e-8);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -340,7 +340,7 @@ Transform icpPointToPlane(
 	// Set the maximum number of iterations (criterion 1)
 	icp.setMaximumIterations (maximumIterations);
 	// Set the transformation epsilon (criterion 2)
-	//icp.setTransformationEpsilon (transformationEpsilon);
+	//icp.setTransformationEpsilon (1e-8);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -373,7 +373,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	// Set the maximum number of iterations (criterion 1)
 	icp.setMaximumIterations (maximumIterations);
 	// Set the transformation epsilon (criterion 2)
-	//icp.setTransformationEpsilon (transformationEpsilon);
+	//icp.setTransformationEpsilon (1e-8);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -423,7 +423,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 			}
 			else if(samples>0 && (int)cloud->size() > samples)
 			{
-				cloud = sampling(cloud, samples);
+				cloud = randomSampling(cloud, samples);
 			}
 
 			if(cloud->size())
@@ -439,7 +439,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 	return cloud;
 }
 
-
 }
 
 }

guilib/include/rtabmap/gui/CloudViewer.h

@@ -129,6 +129,12 @@ public:
 			const Transform & pose = Transform::getIdentity(),
 			const QColor & color = QColor());
 
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
 	bool addCloud(
 			const std::string & id,
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -84,7 +84,9 @@ private slots:
 	void generateTOROGraph();
 	void generateG2OGraph();
 	void view3DMap();
+	void view3DLaserScans();
 	void generate3DMap();
+	void generate3DLaserScans();
 	void detectMoreLoopClosures();
 	void refineAllNeighborLinks();
 	void refineAllLoopClosureLinks();
@@ -106,6 +108,7 @@ private slots:
 	void resetConstraint();
 	void rejectConstraint();
 	void updateConstraintView();
+	void updateLoggerLevel();
 	void updateStereo();
 
 private:

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -151,6 +151,8 @@ public:
 	int getCloudPointSize(int index) const;    // 0=map, 1=odom
 
 	bool isScansShown(int index) const;       // 0=map, 1=odom
+	int getDownsamplingStepScan(int index) const; // 0=map, 1=odom
+	double getCloudVoxelSizeScan(int index) const; // 0=map, 1=odom
 	double getScanOpacity(int index) const;    // 0=map, 1=odom
 	int getScanPointSize(int index) const;     // 0=map, 1=odom
 
@@ -177,7 +179,6 @@ public:
 	// source panel
 	double getGeneralInputRate() const;
 	bool isSourceMirroring() const;
-	QString getCalibrationName() const;
 	PreferencesDialog::Src getSourceType() const;
 	PreferencesDialog::Src getSourceDriver() const;
 	QString getSourceDriverStr() const;
@@ -192,6 +193,7 @@ public:
 	bool isSourceRGBDColorOnly() const;
 	bool isSourceStereoDepthGenerated() const;
 	Transform getSourceLocalTransform() const;    //Openni group
+	Transform getStereoLaserLocalTransform() const; // stereo images
 	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
@@ -209,7 +211,7 @@ public:
 	double getSimThr() const;
 	int getOdomStrategy() const;
 	int getOdomBufferSize() const;
-	bool getLccBowVarianceFromInliersCount() const;
+	bool getRegVarianceFromInliersCount() const;
 	QString getCameraInfoDir() const; // "workinfDir/camera_info"
 
 	//
@@ -254,12 +256,14 @@ private slots:
 	void updateBasicParameter();
 	void openDatabaseViewer();
 	void selectSourceDatabase();
+	void selectCalibrationPath();
 	void selectSourceRGBDImagesStamps();
 	void selectSourceRGBDImagesPathRGB();
 	void selectSourceRGBDImagesPathDepth();
 	void selectSourceStereoImagesStamps();
 	void selectSourceStereoImagesPathLeft();
 	void selectSourceStereoImagesPathRight();
+	void selectSourceStereoImagesPathScans();
 	void selectSourceImagesPath();
 	void selectSourceVideoPath();
 	void selectSourceStereoVideoPath();
@@ -307,7 +311,6 @@ private:
 	void addParameters(const QGroupBox * box);
 	QList<QGroupBox*> getGroupBoxes();
 	void readSettingsBegin();
-	void testOdometry(int type);
 
 protected:
 	rtabmap::ParametersMap _parameters;
@@ -331,6 +334,8 @@ private:
 	QVector<QDoubleSpinBox*> _3dRenderingOpacity;
 	QVector<QSpinBox*> _3dRenderingPtSize;
 	QVector<QCheckBox*> _3dRenderingShowScans;
+	QVector<QSpinBox*> _3dRenderingDownsamplingScan;
+	QVector<QDoubleSpinBox*> _3dRenderingVoxelSizeScan;
 	QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
 	QVector<QSpinBox*> _3dRenderingPtSizeScan;
 };

guilib/src/CloudViewer.cpp

@@ -518,6 +518,23 @@ bool CloudViewer::addCloud(
 	return false;
 }
 
+bool CloudViewer::addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	if(!_addedClouds.contains(id))
+	{
+		UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
+
+		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
+		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
+		return addCloud(id, binaryCloud, pose, false, true, color);
+	}
+	return false;
+}
+
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -102,6 +102,7 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
 		if(!name.isEmpty())
 		{
+			cameraName_ = name;
 			std::string base = (dir+QDir::separator()+name).toStdString();
 			std::string leftPath = base+"_left.yaml";
 			std::string rightPath = base+"_right.yaml";

guilib/src/DatabaseViewer.cpp

@@ -60,12 +60,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/core/Graph.h"
+#include "rtabmap/core/RegistrationVis.h"
+#include "rtabmap/core/RegistrationIcp.h"
 #include "rtabmap/gui/DataRecorder.h"
 #include "rtabmap/core/SensorData.h"
 #include "ExportDialog.h"
 #include "rtabmap/gui/ProgressDialog.h"
 
 #include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
@@ -90,6 +93,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	ui_->buttonBox->setVisible(false);
 	connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
 
+	ui_->comboBox_logger_level->setVisible(parent==0);
+	ui_->label_logger_level->setVisible(parent==0);
+	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
+
 	QString title("RTAB-Map Database Viewer[*]");
 	this->setWindowTitle(title);
 
@@ -164,7 +171,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->actionGenerate_g2o_graph_g2o, SIGNAL(triggered()), this, SLOT(generateG2OGraph()));
 	ui_->actionGenerate_g2o_graph_g2o->setEnabled(graph::G2OOptimizer::available());
 	connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
+	connect(ui_->actionView_3D_laser_scans, SIGNAL(triggered()), this, SLOT(view3DLaserScans()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
+	connect(ui_->actionExport_3D_laser_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(generate3DLaserScans()));
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
 	connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
 	connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
@@ -221,6 +230,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
 	connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -260,6 +270,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
 	//connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
 	//connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
+	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	// Graph view
 	connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
@@ -288,11 +299,13 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_voxel, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
+	connect(ui_->spinBox_icp_downsamplingStepSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->spinBox_icp_iteration, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_icp_p2plane, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->spinBox_icp_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_icp_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_icp_laserScan, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_minCorrespondenceRatio, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	// Visual parameters
 	connect(ui_->groupBox_visual_recomputeFeatures, SIGNAL(clicked(bool)), this, SLOT(configModified()));
@@ -382,6 +395,8 @@ void DatabaseViewer::readSettings()
 	}
 	savedMaximized_ = settings.value("maximized", false).toBool();
 
+	ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
+
 	// GraphViewer settings
 	ui_->graphViewer->loadSettings(settings, "GraphView");
 
@@ -423,11 +438,13 @@ void DatabaseViewer::readSettings()
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
 	ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
 	ui_->doubleSpinBox_icp_voxel->setValue(settings.value("voxel", ui_->doubleSpinBox_icp_voxel->value()).toDouble());
+	ui_->spinBox_icp_downsamplingStepSize->setValue(settings.value("samplingStep", ui_->spinBox_icp_downsamplingStepSize->value()).toInt());
 	ui_->doubleSpinBox_icp_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value()).toDouble());
 	ui_->spinBox_icp_iteration->setValue(settings.value("iterations", ui_->spinBox_icp_iteration->value()).toInt());
 	ui_->checkBox_icp_p2plane->setChecked(settings.value("point2place", ui_->checkBox_icp_p2plane->isChecked()).toBool());
 	ui_->spinBox_icp_normalKSearch->setValue(settings.value("normalKSearch", ui_->spinBox_icp_normalKSearch->value()).toInt());
 	ui_->checkBox_icp_2d->setChecked(settings.value("icp2d", ui_->checkBox_icp_2d->isChecked()).toBool());
+	ui_->checkBox_icp_laserScan->setChecked(settings.value("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked()).toBool());
 	ui_->doubleSpinBox_icp_minCorrespondenceRatio->setValue(settings.value("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()).toDouble());
 	settings.endGroup();
 
@@ -481,6 +498,8 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("maximized", this->isMaximized());
 	savedMaximized_ = this->isMaximized();
 
+	settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
+
 	// save GraphViewer settings
 	ui_->graphViewer->saveSettings(settings, "GraphView");
 
@@ -524,11 +543,13 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
 	settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
 	settings.setValue("voxel", ui_->doubleSpinBox_icp_voxel->value());
+	settings.setValue("samplingStep", ui_->spinBox_icp_downsamplingStepSize->value());
 	settings.setValue("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value());
 	settings.setValue("iterations", ui_->spinBox_icp_iteration->value());
 	settings.setValue("point2place", ui_->checkBox_icp_p2plane->isChecked());
 	settings.setValue("normalKSearch", ui_->spinBox_icp_normalKSearch->value());
 	settings.setValue("icp2d", ui_->checkBox_icp_2d->isChecked());
+	settings.setValue("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked());
 	settings.setValue("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value());
 	settings.endGroup();
 
@@ -1541,6 +1562,112 @@ void DatabaseViewer::view3DMap()
 	}
 }
 
+void DatabaseViewer::view3DLaserScans()
+{
+	if(!ids_.size() || !dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot view 3D laser scans"), tr("The database is empty..."));
+		return;
+	}
+
+	if(graphes_.empty())
+	{
+		this->updateGraphView();
+		if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
+		{
+			QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
+			return;
+		}
+	}
+	bool ok = false;
+	int downsamplingStepSize = QInputDialog::getInt(this, tr("Downsampling?"), tr("Downsample step size (1 = no filtering)"), 1, 1, 99999, 1, &ok);
+	if(ok)
+	{
+		std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+		if(ui_->groupBox_posefiltering->isChecked())
+		{
+			optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
+					ui_->doubleSpinBox_posefilteringRadius->value(),
+					ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
+		}
+		if(optimizedPoses.size() > 0)
+		{
+			rtabmap::ProgressDialog progressDialog(this);
+			progressDialog.setMaximumSteps((int)optimizedPoses.size());
+			progressDialog.show();
+
+			// create a window
+			QDialog * window = new QDialog(this, Qt::Window);
+			window->setModal(this->isModal());
+			window->setWindowTitle(tr("3D Laser Scans"));
+			window->setMinimumWidth(800);
+			window->setMinimumHeight(600);
+
+			rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
+
+			QVBoxLayout *layout = new QVBoxLayout();
+			layout->addWidget(viewer);
+			viewer->setCameraLockZ(false);
+			window->setLayout(layout);
+			connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
+
+			window->show();
+
+			for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+			{
+				rtabmap::Transform pose = iter->second;
+				if(!pose.isNull())
+				{
+					SensorData data;
+					dbDriver_->getNodeData(iter->first, data);
+					cv::Mat scan;
+					data.uncompressDataConst(0, 0, &scan);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+					UASSERT(scan.empty() || scan.type()==CV_32FC2 || scan.type() == CV_32FC3);
+
+					if(downsamplingStepSize>1)
+					{
+						scan = util3d::downsample(scan, downsamplingStepSize);
+					}
+					cloud = util3d::laserScanToPointCloud(scan);
+
+					if(cloud->size())
+					{
+						QColor color = Qt::red;
+						int mapId, weight;
+						Transform odomPose;
+						std::string label;
+						double stamp;
+						if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp))
+						{
+							color = (Qt::GlobalColor)(mapId % 12 + 7 );
+						}
+
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, ui_->spinBox_icp_normalKSearch->value());
+
+						viewer->addCloud(uFormat("cloud%d", iter->first), cloudNormals, pose, color);
+
+						UINFO("Generated %d (%d points)", iter->first, cloud->size());
+						progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
+					}
+					else
+					{
+						UINFO("Empty cloud %d", iter->first);
+						progressDialog.appendText(QString("Empty cloud %1").arg(iter->first));
+					}
+					progressDialog.incrementStep();
+					QApplication::processEvents();
+				}
+			}
+			progressDialog.setValue(progressDialog.maximumSteps());
+		}
+		else
+		{
+			QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
+		}
+	}
+}
+
 void DatabaseViewer::generate3DMap()
 {
 	if(!ids_.size() || !dbDriver_)
@@ -1559,10 +1686,28 @@ void DatabaseViewer::generate3DMap()
 	if(ok)
 	{
 		int decimation = item.toInt();
-		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
+		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
 		if(ok)
 		{
-			QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
+			QMessageBox::StandardButton b = QMessageBox::question(
+					this,
+					tr("Assembling?"),
+					tr("Do you want to assemble all the point clouds (creating only one file with a density of 1pt/cm)?"),
+					QMessageBox::Yes|QMessageBox::No,
+					QMessageBox::Yes);
+
+			bool assemble = b == QMessageBox::Yes;
+			QString path;
+			if(assemble)
+			{
+				path = QFileDialog::getSaveFileName(this, tr("Save point cloud"),
+						pathDatabase_+QDir::separator()+"cloud.ply",
+						tr("Point Cloud (*.ply *.pcd)"));
+			}
+			else
+			{
+				path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
+			}
 			if(!path.isEmpty())
 			{
 				std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
@@ -1578,6 +1723,7 @@ void DatabaseViewer::generate3DMap()
 					progressDialog.setMaximumSteps((int)optimizedPoses.size());
 					progressDialog.show();
 
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 					{
 						const rtabmap::Transform & pose = iter->second;
@@ -1589,27 +1735,66 @@ void DatabaseViewer::generate3DMap()
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
 							UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
-							cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
+							cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, assemble?0.01:0);
-							std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
+
-							if(cloud->size())
+							if(assemble)
 							{
-								cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
+								if(cloud->size())
-								pcl::io::savePCDFile(name, *cloud);
+								{
-								UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
+									cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
-								progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
+									if(assembledCloud->size() == 0)
+									{
+										*assembledCloud = *cloud;
+									}
+									else
+									{
+										*assembledCloud += *cloud;
+									}
+								}
+								UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
+								progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(cloud->size()));
 							}
 							else
 							{
-								UINFO("Ignored empty cloud %s", name.c_str());
+								std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
-								progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
+								if(cloud->size())
+								{
+									cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
+									pcl::io::savePCDFile(name, *cloud);
+									UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
+									progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
+								}
+								else
+								{
+									UINFO("Ignored empty cloud %s", name.c_str());
+									progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
+								}
 							}
 							progressDialog.incrementStep();
 							QApplication::processEvents();
 						}
 					}
-					progressDialog.setValue(progressDialog.maximumSteps());
 
+					if(assemble && assembledCloud->size())
+					{
+						//voxelize by default to 1 cm
+						progressDialog.appendText(QString("Voxelize assembled cloud (%1 points)").arg(assembledCloud->size()));
+						QApplication::processEvents();
+						assembledCloud = util3d::voxelize(assembledCloud, 0.01);
+						if(QFileInfo(path).suffix() == "ply")
+						{
+							pcl::io::savePLYFile(path.toStdString(), *assembledCloud);
+						}
+						else
+						{
+							pcl::io::savePCDFile(path.toStdString(), *assembledCloud);
+						}
+						progressDialog.appendText(QString("Saved %1 (%2 points)").arg(path).arg(assembledCloud->size()));
+						QApplication::processEvents();
+					}
 					QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
+
+					progressDialog.setValue(progressDialog.maximumSteps());
 				}
 				else
 				{
@@ -1620,6 +1805,103 @@ void DatabaseViewer::generate3DMap()
 	}
 }
 
+void DatabaseViewer::generate3DLaserScans()
+{
+	if(!ids_.size() || !dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
+		return;
+	}
+	bool ok = false;
+	int downsamplingStepSize = QInputDialog::getInt(this, tr("Downsampling?"), tr("Downsample step size (1 = no filtering)"), 1, 1, 99999, 1, &ok);
+	if(ok)
+	{
+		QString path = QFileDialog::getSaveFileName(this, tr("Save point cloud"),
+					pathDatabase_+QDir::separator()+"cloud.ply",
+					tr("Point Cloud (*.ply *.pcd)"));
+		if(!path.isEmpty())
+		{
+			std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+			if(ui_->groupBox_posefiltering->isChecked())
+			{
+				optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
+						ui_->doubleSpinBox_posefilteringRadius->value(),
+						ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
+			}
+			if(optimizedPoses.size() > 0)
+			{
+				rtabmap::ProgressDialog progressDialog;
+				progressDialog.setMaximumSteps((int)optimizedPoses.size());
+				progressDialog.show();
+
+				pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
+				for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+				{
+					const rtabmap::Transform & pose = iter->second;
+					if(!pose.isNull())
+					{
+						SensorData data;
+						dbDriver_->getNodeData(iter->first, data);
+						cv::Mat scan;
+						data.uncompressDataConst(0, 0, &scan);
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+						UASSERT(scan.empty() || scan.type()==CV_32FC2 || scan.type() == CV_32FC3);
+
+						if(downsamplingStepSize > 1)
+						{
+							scan = util3d::downsample(scan, downsamplingStepSize);
+						}
+						cloud = util3d::laserScanToPointCloud(scan);
+
+						if(cloud->size())
+						{
+							cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
+							if(assembledCloud->size() == 0)
+							{
+								*assembledCloud = *cloud;
+							}
+							else
+							{
+								*assembledCloud += *cloud;
+							}
+						}
+						UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
+						progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(cloud->size()));
+
+						progressDialog.incrementStep();
+						QApplication::processEvents();
+					}
+				}
+
+				if(assembledCloud->size())
+				{
+					//voxelize by default to 1 cm
+					progressDialog.appendText(QString("Voxelize assembled cloud (%1 points)").arg(assembledCloud->size()));
+					QApplication::processEvents();
+					assembledCloud = util3d::voxelize(assembledCloud, 0.01);
+					if(QFileInfo(path).suffix() == "ply")
+					{
+						pcl::io::savePLYFile(path.toStdString(), *assembledCloud);
+					}
+					else
+					{
+						pcl::io::savePCDFile(path.toStdString(), *assembledCloud);
+					}
+					progressDialog.appendText(QString("Saved %1 (%2 points)").arg(path).arg(assembledCloud->size()));
+					QApplication::processEvents();
+				}
+				QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
+
+				progressDialog.setValue(progressDialog.maximumSteps());
+			}
+			else
+			{
+				QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
+			}
+		}
+	}
+}
+
 void DatabaseViewer::detectMoreLoopClosures()
 {
 	const std::map<int, Transform> & optimizedPoses = graphes_.back();
@@ -2079,6 +2361,14 @@ void DatabaseViewer::update(int value,
 		view->setSceneRect(rect);
 	}
 }
+
+void DatabaseViewer::updateLoggerLevel()
+{
+	if(this->parent() == 0)
+	{
+		ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
+	}
+}
 
 void DatabaseViewer::updateStereo()
 {
@@ -2417,6 +2707,19 @@ void DatabaseViewer::updateConstraintView(
 	{
 		link = iterLink->second;
 	}
+	else if(ui_->checkBox_ignorePoseCorrection->isChecked())
+	{
+		if(link.type() == Link::kNeighbor ||
+		   link.type() == Link::kNeighborMerged)
+		{
+			Transform poseFrom = uValue(poses_, link.from(), Transform());
+			Transform poseTo = uValue(poses_, link.to(), Transform());
+			if(!poseFrom.isNull() && !poseTo.isNull())
+			{
+				link.setTransform(poseFrom.inverse() * poseTo); // recompute raw odom transformation
+			}
+		}
+	}
 	rtabmap::Transform t = link.transform();
 
 	ui_->label_constraint->clear();
@@ -2426,7 +2729,7 @@ void DatabaseViewer::updateConstraintView(
 
 	ui_->label_type->setText(tr("%1 (%2)")
 			.arg(link.type())
-			.arg(link.type()==Link::kNeighbor?"Neigbor":
+			.arg(link.type()==Link::kNeighbor?"Neighbor":
 				 link.type()==Link::kNeighbor?"Merged neighbor":
 				 link.type()==Link::kGlobalClosure?"Loop closure":
 				 link.type()==Link::kLocalSpaceClosure?"Space proximity link":
@@ -2964,15 +3267,18 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 							cv::Mat laserScan;
 							data.uncompressDataConst(0, 0, &laserScan);
 							cv::Mat ground, obstacles;
-							util3d::occupancy2DFromLaserScan(
+							if(laserScan.type() == CV_32FC2)
-									laserScan,
+							{
-									ground,
+								util3d::occupancy2DFromLaserScan(
-									obstacles,
+										laserScan,
-									ui_->doubleSpinBox_gridCellSize->value(),
+										ground,
-									ui_->checkBox_gridFillUnkownSpace->isChecked(),
+										obstacles,
-									data.laserScanMaxRange());
+										ui_->doubleSpinBox_gridCellSize->value(),
+										ui_->checkBox_gridFillUnkownSpace->isChecked(),
+										data.laserScanMaxRange());
+								added = true;
+							}
 							localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
-							added = true;
 						}
 					}
 					if(added)
@@ -3345,207 +3651,104 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 			}
 		}
 	}
+	else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
+			graph::findLink(linksRefined_, from, to) == linksRefined_.end())
+	{
+		if(currentLink.type() == Link::kNeighbor ||
+		   currentLink.type() == Link::kNeighborMerged)
+		{
+			Transform poseFrom = uValue(poses_, currentLink.from(), Transform());
+			Transform poseTo = uValue(poses_, currentLink.to(), Transform());
+			if(!poseFrom.isNull() && !poseTo.isNull())
+			{
+				t  = poseFrom.inverse() * poseTo; // recompute raw odom transformation
+			}
+		}
+	}
 
 
-	bool hasConverged = false;
+	float variance = -1.0f;
-	double variance = -1.0;
-	int correspondences = 0;
 	Transform transform;
 
 	SensorData dataFrom, dataTo;
 	dbDriver_->getNodeData(currentLink.from(), dataFrom);
 	dbDriver_->getNodeData(currentLink.to(), dataTo);
 
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
+
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
+	UTimer timer;
-	pcl::PointCloud<pcl::PointXYZ>::Ptr scanAVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
+	if(!ui_->checkBox_icp_laserScan->isChecked())
-	pcl::PointCloud<pcl::PointXYZ>::Ptr scanBVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
-	float correspondenceRatio = 0.0f;
-	if(ui_->checkBox_icp_2d->isChecked())
 	{
-		//2D
+		// generate laser scans from depth image
-		cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.laserScanCompressed());
+		cv::Mat tmpA, tmpB, tmpC, tmpD;
-		cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.laserScanCompressed());
+		dataFrom.uncompressData(&tmpA, &tmpB, 0);
-
+		dataTo.uncompressData(&tmpC, &tmpD, 0);
-		if(!oldLaserScan.empty() && !newLaserScan.empty())
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
-		{
+				dataFrom,
-			// 2D
+				ui_->spinBox_icp_decimation->value(),
-			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
+				ui_->doubleSpinBox_icp_maxDepth->value());
-			pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
-			scanA = util3d::cvMat2Cloud(oldLaserScan);
+				dataTo,
-			scanB = util3d::cvMat2Cloud(newLaserScan, t);
+				ui_->spinBox_icp_decimation->value(),
-
+				ui_->doubleSpinBox_icp_maxDepth->value());
-			//voxelize
+		int maxLaserScans = cloudFrom->size();
-			if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
+		dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
-			{
+		dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);
-				scanA = util3d::voxelize(scanA, ui_->doubleSpinBox_icp_voxel->value());
-				scanB = util3d::voxelize(scanB, ui_->doubleSpinBox_icp_voxel->value());
-			}
-			else
-			{
-				scanAVoxelized = scanA;
-				scanBVoxelized = scanB;
-			}
-
-			if(scanB->size() && scanA->size())
-			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr scanBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
-				transform = util3d::icp2D(
-						scanB,
-						scanA,
-						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-						ui_->spinBox_icp_iteration->value(),
-						hasConverged,
-						*scanBRegistered);
-
-				if(!transform.isNull())
-				{
-					if(dataTo.laserScanMaxPts())
-					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr scanBTransformed = scanBRegistered;
-						if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
-						{
-							scanBTransformed = util3d::transformPointCloud(scanB, transform);
-						}
-
-						util3d::computeVarianceAndCorrespondences(
-								scanBTransformed,
-								scanA,
-								ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-								variance,
-								correspondences);
-
-						correspondenceRatio =  float(correspondences)/float(dataTo.laserScanMaxPts());
-					}
-					else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
-					{
-						UWARN("Laser scan max pts not set, but correspondence ratio is set!");
-					}
-				}
-			}
-		}
 	}
 	else
 	{
-		//3D
+		cv::Mat tmpA, tmpB;
-		cv::Mat im,de;
+		dataFrom.uncompressData(0, 0, &tmpA);
-		dataFrom.uncompressData(&im, &de, 0);
+		dataTo.uncompressData(0, 0, &tmpB);
-		dataTo.uncompressData(&im, &de, 0);
-		cloudA = util3d::cloudFromSensorData(dataFrom,
-				ui_->spinBox_icp_decimation->value(),
-				ui_->doubleSpinBox_icp_maxDepth->value(),
-				ui_->doubleSpinBox_icp_voxel->value());
-		cloudB = util3d::cloudFromSensorData(dataTo,
-				ui_->spinBox_icp_decimation->value(),
-				ui_->doubleSpinBox_icp_maxDepth->value(),
-				ui_->doubleSpinBox_icp_voxel->value());
-		if(cloudA->size() && cloudB->size())
-		{
-			cloudB = util3d::transformPointCloud(cloudB, t);
-			if(ui_->checkBox_icp_p2plane->isChecked())
-			{
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
-
-				cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
-				if(cloudA->size() != cloudANormals->size())
-				{
-					UWARN("removed nan normals...");
-				}
-
-				cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
-				if(cloudB->size() != cloudBNormals->size())
-				{
-					UWARN("removed nan normals...");
-				}
-
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
-				transform = util3d::icpPointToPlane(
-						cloudBNormals,
-						cloudANormals,
-						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-						ui_->spinBox_icp_iteration->value(),
-						hasConverged,
-						*cloudBRegistered);
-				util3d::computeVarianceAndCorrespondences(
-						cloudBRegistered,
-						cloudANormals,
-						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-						variance,
-						correspondences);
-			}
-			else
-			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
-				transform = util3d::icp(cloudB,
-						cloudA,
-						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-						ui_->spinBox_icp_iteration->value(),
-						hasConverged,
-						*cloudBRegistered);
-				util3d::computeVarianceAndCorrespondences(
-						cloudBRegistered,
-						cloudA,
-						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-						variance,
-						correspondences);
-			}
-			correspondenceRatio = float(correspondences)/float(cloudA->size()>cloudB->size()?cloudA->size():cloudB->size());
-		}
-		else
-		{
-			UWARN("No cloud generated!");
-		}
 	}
+	UINFO("Uncompress time: %f s", timer.ticks());
 
-	if(hasConverged && !transform.isNull())
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kIcpDownsamplingStep(), uNumber2Str(ui_->spinBox_icp_downsamplingStepSize->value())));
+	parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), uNumber2Str(ui_->doubleSpinBox_icp_voxel->value())));
+	parameters.insert(ParametersPair(Parameters::kIcp2D(), uBool2Str(ui_->checkBox_icp_2d->isChecked())));
+	parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), uBool2Str(ui_->checkBox_icp_p2plane->isChecked())));
+	parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneNormalNeighbors(), uNumber2Str(ui_->spinBox_icp_normalKSearch->value())));
+	parameters.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(ui_->doubleSpinBox_icp_maxCorrespDistance->value())));
+	parameters.insert(ParametersPair(Parameters::kIcpIterations(), uNumber2Str(ui_->spinBox_icp_iteration->value())));
+	parameters.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())));
+
+	RegistrationIcp registration(parameters);
+	transform = registration.computeTransformation(dataFrom, dataTo, t, 0, 0, &variance);
+	UINFO("Icp time: %f s", timer.ticks());
+
+	if(!transform.isNull())
 	{
-		if(correspondenceRatio < ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, variance, variance);
+
+		bool updated = false;
+		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
+		while(iter != linksRefined_.end() && iter->first == currentLink.from())
 		{
-			if(!silent)
+			if(iter->second.to() == currentLink.to() &&
+			   iter->second.type() == currentLink.type())
 			{
-				QMessageBox::warning(this,
+				iter->second = newLink;
-								tr("Refine link"),
+				updated = true;
-								tr("Cannot find a transformation between nodes %1 and %2, correspondence ratio too low (%3).")
+				break;
-								.arg(from).arg(to).arg(correspondenceRatio));
+			}
+			++iter;
+		}
+		if(!updated)
+		{
+			linksRefined_.insert(std::make_pair(newLink.from(), newLink));
+
+			if(updateGraph)
+			{
+				this->updateGraphView();
 			}
 		}
-		else
+
+		if(ui_->dockWidget_constraints->isVisible())
 		{
-
+			this->updateConstraintView(newLink, true);
-			Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance, variance);
-
-			bool updated = false;
-			std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
-			while(iter != linksRefined_.end() && iter->first == currentLink.from())
-			{
-				if(iter->second.to() == currentLink.to() &&
-				   iter->second.type() == currentLink.type())
-				{
-					iter->second = newLink;
-					updated = true;
-					break;
-				}
-				++iter;
-			}
-			if(!updated)
-			{
-				linksRefined_.insert(std::make_pair(newLink.from(), newLink));
-
-				if(updateGraph)
-				{
-					this->updateGraphView();
-				}
-			}
-
-			if(ui_->dockWidget_constraints->isVisible())
-			{
-				cloudB = util3d::transformPointCloud(cloudB, transform);
-				scanBVoxelized = util3d::transformPointCloud(scanBVoxelized, transform);
-				this->updateConstraintView(newLink, true, cloudA, cloudB, scanAVoxelized, scanBVoxelized);
-			}
 		}
 	}
+
 	else if(!silent)
 	{
 		QMessageBox::warning(this,
@@ -3576,30 +3779,31 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 		return;
 	}
 
-	// create a fake memory to compute transform
 	ParametersMap parameters;
 	parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
 	parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
-	parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
+	parameters.insert(ParametersPair(Parameters::kVisInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
 	parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
 	parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
-	parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
+	parameters.insert(ParametersPair(Parameters::kVisIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
-	parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
+	parameters.insert(ParametersPair(Parameters::kVisMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
-	parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
+	parameters.insert(ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
-	parameters.insert(ParametersPair(Parameters::kLccBowPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
+	parameters.insert(ParametersPair(Parameters::kVisPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
-	parameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
+	parameters.insert(ParametersPair(Parameters::kVisForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
-	parameters.insert(ParametersPair(Parameters::kLccBowVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
+	parameters.insert(ParametersPair(Parameters::kRegVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
 	parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
 	parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
 	parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
-	Memory tmpMemory(parameters);
 
 	Transform t;
 	std::string rejectedMsg;
-	double variance = -1.0;
+	float variance = -1.0f;
 	int inliers = -1;
 	if(ui_->groupBox_visual_recomputeFeatures->isChecked())
 	{
+		// create a fake memory to compute transform
+		Memory tmpMemory(parameters);
+
 		// Add sensor data to generate features
 		SensorData dataFrom;
 		dbDriver_->getNodeData(from, dataFrom);
@@ -3620,7 +3824,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 		}
 
 
-		t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
+		t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
 	}
 	else
 	{
@@ -3632,7 +3836,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 
 		if(signatures.size() == 2)
 		{
-			t = tmpMemory.computeVisualTransform(*signatures.front(), *signatures.back(), &rejectedMsg, &inliers, &variance);
+			RegistrationVis registration(parameters);
+			t = registration.computeTransformation(*signatures.back(), *signatures.front(), Transform(), &rejectedMsg, &inliers, &variance);
 		}
 		//cleanup
 		for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
@@ -3702,30 +3907,31 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		UASSERT(!containsLink(linksRemoved_, from, to));
 		UASSERT(!containsLink(linksRefined_, from, to));
 
-		// create a fake memory to compute the transform
 		ParametersMap parameters;
 		parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
 		parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
-		parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
+		parameters.insert(ParametersPair(Parameters::kVisInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
 		parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
 		parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
-		parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
+		parameters.insert(ParametersPair(Parameters::kVisIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
-		parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
+		parameters.insert(ParametersPair(Parameters::kVisMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
-		parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
+		parameters.insert(ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
-		parameters.insert(ParametersPair(Parameters::kLccBowPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
+		parameters.insert(ParametersPair(Parameters::kVisPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
-		parameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
+		parameters.insert(ParametersPair(Parameters::kVisForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
-		parameters.insert(ParametersPair(Parameters::kLccBowVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
+		parameters.insert(ParametersPair(Parameters::kRegVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
 		parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
 		parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
 		parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
-		Memory tmpMemory(parameters);
 
 		Transform t;
 		std::string rejectedMsg;
-		double variance = -1.0;
+		float variance = -1.0f;
 		int inliers = -1;
 		if(ui_->groupBox_visual_recomputeFeatures->isChecked())
 		{
+			// create a fake memory to compute the transform
+			Memory tmpMemory(parameters);
+
 			// Add sensor data to generate features
 			SensorData dataFrom;
 			dbDriver_->getNodeData(from, dataFrom);
@@ -3746,7 +3952,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			}
 
 
-			t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
+			t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
 
 			if(!silent)
 			{
@@ -3765,7 +3971,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 
 			if(signatures.size() == 2)
 			{
-				t = tmpMemory.computeVisualTransform(*signatures.front(), *signatures.back(), &rejectedMsg, &inliers, &variance);
+				RegistrationVis registration(parameters);
+
+				t = registration.computeTransformation(*signatures.back(), *signatures.front(), Transform(), &rejectedMsg, &inliers, &variance);
 			}
 			//cleanup
 			for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)

guilib/src/MainWindow.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/DBDriver.h"
+#include "rtabmap/core/RegistrationVis.h"
 
 #include "rtabmap/gui/ImageView.h"
 #include "rtabmap/gui/KeypointItem.h"
@@ -94,6 +95,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/Graph.h"
+#include "rtabmap/core/RegistrationIcp.h"
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
@@ -481,9 +483,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 
 	// update loop closure viewer parameters
 	ParametersMap parameters = _preferencesDialog->getAllParameters();
-	_ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
+	_ui->widget_loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
-	_ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
+	_ui->widget_loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
-	_ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
 
 	//update ui
 	_ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
@@ -810,21 +811,22 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 					_preferencesDialog->isScansShown(1))
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-					cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
+					cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw(), pose);
-					cloud = util3d::transformPointCloud(cloud, pose);
+					if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
+					{
+						cloud = util3d::downsample(cloud, _preferencesDialog->getDownsamplingStepScan(1));
+					}
+					if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
+					{
+						cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
+					}
 					if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
 					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+						UERROR("Adding scanOdom to viewer failed!");
-						cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
-						cloud = util3d::transformPointCloud(cloud, pose);
-						if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
-						{
-							UERROR("Adding scanOdom to viewer failed!");
-						}
-						_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
-						_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
-						_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 					}
+					_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
+					_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
+					_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 				}
 			}
 		}
@@ -1929,6 +1931,14 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 		cloud = util3d::laserScanToPointCloud(depth2D);
+		if(_preferencesDialog->getDownsamplingStepScan(0) > 0)
+		{
+			cloud = util3d::downsample(cloud, _preferencesDialog->getDownsamplingStepScan(0));
+		}
+		if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+		{
+			cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
+		}
 		QColor color = Qt::gray;
 		if(mapId >= 0)
 		{
@@ -1942,9 +1952,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		{
 			_createdScans.insert(std::make_pair(nodeId, cloud));
 
-			cv::Mat ground, obstacles;
+			if(depth2D.channels() == 2)
-			util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
+			{
-			_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+				cv::Mat ground, obstacles;
+				util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
+				_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+			}
 		}
 		_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 		_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
@@ -2376,19 +2389,9 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bo
 			_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 		}
 
-		// update loop closure viewer parameters
+		// update loop closure viewer parameters (Use Map parameters)
-		if(uContains(parameters, Parameters::kLccIcp3Decimation()))
+		_ui->widget_loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
-		{
+		_ui->widget_loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
-			_ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
-		}
-		if(uContains(parameters, Parameters::kLccIcp3MaxDepth()))
-		{
-			_ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
-		}
-		if(uContains(parameters, Parameters::kLccIcp3Samples()))
-		{
-			_ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
-		}
 
 		// update graph view parameters
 		if(uContains(parameters, Parameters::kRGBDLocalRadius()))
@@ -3370,31 +3373,6 @@ void MainWindow::postProcessing()
 			{
 				odomPoses.insert(*iter); // fill raw poses
 			}
-			/*
-			if(jter->sensorData().cameraModels().size() == 0 && !jter->sensorData().stereoCameraModel().isValid())
-			{
-				UWARN("Calibration of %d is null.", iter->first);
-				allDataAvailable = false;
-			}
-			if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
-			{
-				// depth data required
-				if(jter->sensorData().depthOrRightCompressed().empty())
-				{
-					UWARN("Depth data of %d missing.", iter->first);
-					allDataAvailable = false;
-				}
-
-				if(reextractFeatures)
-				{
-					// rgb required
-					if(jter->sensorData().imageCompressed().empty())
-					{
-						UWARN("Rgb of %d missing.", iter->first);
-						allDataAvailable = false;
-					}
-				}
-			}*/
 		}
 		else
 		{
@@ -3442,22 +3420,6 @@ void MainWindow::postProcessing()
 	if(detectMoreLoopClosures)
 	{
 		UDEBUG("");
-		Memory memory(parameters);
-		if(reextractFeatures)
-		{
-			ParametersMap customParameters;
-			// override some parameters
-			customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-			customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
-			customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
-			customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
-			customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kLccReextractNNType())));
-			customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kLccReextractNNDR())));
-			customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kLccReextractFeatureType())));
-			customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kLccReextractMaxWords())));
-			customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
-			memory.parseParameters(customParameters);
-		}
 
 		UASSERT(detectLoopClosureIterations>0);
 		for(int n=0; n<detectLoopClosureIterations; ++n)
@@ -3502,52 +3464,24 @@ void MainWindow::postProcessing()
 						_initProgressDialog->incrementStep();
 						QApplication::processEvents();
 
-						Signature & signatureFrom = _cachedSignatures[from];
+						Signature signatureFrom = _cachedSignatures[from];
-						Signature & signatureTo = _cachedSignatures[to];
+						Signature signatureTo = _cachedSignatures[to];
+
+						if(reextractFeatures)
+						{
+							signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
+							signatureFrom.setWords3(std::multimap<int, pcl::PointXYZ>());
+							signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
+							signatureTo.setWords3(std::multimap<int, pcl::PointXYZ>());
+						}
 
 						Transform transform;
 						std::string rejectedMsg;
 						int inliers = -1;
-						double variance = -1.0;
+						float variance = -1.0f;
-						if(reextractFeatures)
+						RegistrationVis registration(parameters);
-						{
+						transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &rejectedMsg, &inliers, &variance);
-							memory.init("", true); // clear previously added signatures
 
-							// Add signatures
-							SensorData dataFrom = signatureFrom.sensorData();
-							SensorData dataTo = signatureTo.sensorData();
-
-							cv::Mat image, depth;
-							dataFrom.uncompressData(&image, &depth, 0);
-							dataTo.uncompressData(&image, &depth, 0);
-
-							if(dataFrom.isValid() &&
-							   dataTo.isValid() &&
-							   dataFrom.id() != Memory::kIdInvalid &&
-							   signatureFrom.id() != Memory::kIdInvalid)
-							{
-								if(from > to)
-								{
-									memory.update(dataTo);
-									memory.update(dataFrom);
-								}
-								else
-								{
-									memory.update(dataFrom);
-									memory.update(dataTo);
-								}
-
-								transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers, &variance);
-							}
-							else
-							{
-								UERROR("not supposed to be here!");
-							}
-						}
-						else
-						{
-							transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers, &variance);
-						}
 						if(!transform.isNull())
 						{
 							UINFO("Added new loop closure between %d and %d.", from, to);
@@ -3597,26 +3531,10 @@ void MainWindow::postProcessing()
 		{
 			_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
 		}
+		// TODO: support ICP from laser scans?
 		_initProgressDialog->appendText(tr("Refining links..."));
 
-		int decimation=Parameters::defaultLccIcp3Decimation();
+		RegistrationIcp regIcp(parameters);
-		float maxDepth=Parameters::defaultLccIcp3MaxDepth();
-		float voxelSize=Parameters::defaultLccIcp3VoxelSize();
-		int samples = Parameters::defaultLccIcp3Samples();
-		float maxCorrespondenceDistance = Parameters::defaultLccIcp3MaxCorrespondenceDistance();
-		float correspondenceRatio = Parameters::defaultLccIcp3CorrespondenceRatio();
-		float icpIterations = Parameters::defaultLccIcp3Iterations();
-		Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation);
-		Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth);
-		Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize);
-		Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples);
-		Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio);
-		Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondenceDistance);
-		Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
-		bool pointToPlane = Parameters::defaultLccIcp3PointToPlane();
-		int pointToPlaneNormalNeighbors = Parameters::defaultLccIcp3PointToPlaneNormalNeighbors();
-		Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane);
-		Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors);
 
 		int i=0;
 		for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter, ++i)
@@ -3646,102 +3564,21 @@ void MainWindow::postProcessing()
 					Signature & signatureFrom = _cachedSignatures[from];
 					Signature & signatureTo = _cachedSignatures[to];
 
-					//3D
+					if(!signatureFrom.sensorData().laserScanRaw().empty() &&
-					UDEBUG("");
+					   !signatureTo.sensorData().laserScanRaw().empty())
-					cv::Mat depthA, depthB;
-					if(signatureFrom.sensorData().stereoCameraModel().isValid())
 					{
-						cv::Mat leftA, leftB;
+						std::string rejectedMsg;
-						signatureFrom.sensorData().uncompressData(&leftA, &depthA, 0);
+						float variance = -1.0f;
-						signatureTo.sensorData().uncompressData(&leftB, &depthB, 0);
+						Transform transform = regIcp.computeTransformation(signatureFrom, signatureTo, iter->second.transform(), &rejectedMsg, 0, &variance);
-					}
-					else
-					{
-						signatureFrom.sensorData().uncompressData(0, &depthA, 0);
-						signatureTo.sensorData().uncompressData(0, &depthB, 0);
-					}
 
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::cloudFromSensorData(
+						if(!transform.isNull())
-							signatureFrom.sensorData(),
-							decimation,
-							maxDepth,
-							voxelSize,
-							samples);
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::cloudFromSensorData(
-							signatureTo.sensorData(),
-							decimation,
-							maxDepth,
-							voxelSize,
-							samples);
-					if(cloudA->size() && cloudB->size())
-					{
-						cloudB = util3d::transformPointCloud(cloudB, iter->second.transform());
-
-						bool hasConverged = false;
-						double variance = -1;
-						int correspondences = 0;
-						Transform transform;
-						if(pointToPlane)
-						{
-							UDEBUG("");
-							pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
-							pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
-
-							cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
-							if(cloudA->size() != cloudANormals->size())
-							{
-								UWARN("removed nan normals...");
-							}
-
-							cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
-							if(cloudB->size() != cloudBNormals->size())
-							{
-								UWARN("removed nan normals...");
-							}
-
-							pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
-							transform = util3d::icpPointToPlane(cloudBNormals,
-									cloudANormals,
-									maxCorrespondenceDistance,
-									icpIterations,
-									hasConverged,
-									*cloudBRegistered);
-							util3d::computeVarianceAndCorrespondences(
-									cloudBRegistered,
-									cloudANormals,
-									maxCorrespondenceDistance,
-									variance,
-									correspondences);
-						}
-						else
-						{
-							UDEBUG("");
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
-							transform = util3d::icp(cloudB,
-									cloudA,
-									maxCorrespondenceDistance,
-									icpIterations,
-									hasConverged,
-									*cloudBRegistered);
-							util3d::computeVarianceAndCorrespondences(
-									cloudBRegistered,
-									cloudA,
-									maxCorrespondenceDistance,
-									variance,
-									correspondences);
-						}
-
-						float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
-
-						if(!transform.isNull() && hasConverged &&
-						   correspondencesRatio >= correspondenceRatio)
 						{
 							Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
 							iter->second = newLink;
 						}
 						else
 						{
-							QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
+							QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(rejectedMsg.c_str());
 							_initProgressDialog->appendText(str, Qt::darkYellow);
 							UWARN("%s", str.toStdString().c_str());
 							warn = true;

guilib/src/PreferencesDialog.cpp

@@ -129,7 +129,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 		// remove BruteForceGPU option
 		_ui->comboBox_dictionary_strategy->removeItem(4);
-		_ui->odom_bin_nn->removeItem(4);
 		_ui->reextract_nn->removeItem(4);
 	}
 
@@ -145,12 +144,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(0, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(1, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(0, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(1, 0, Qt::UserRole - 1);
 
 		_ui->comboBox_dictionary_strategy->setItemData(1, 0, Qt::UserRole - 1);
 		_ui->reextract_nn->setItemData(1, 0, Qt::UserRole - 1);
-		_ui->odom_bin_nn->setItemData(1, 0, Qt::UserRole - 1);
 
 #if CV_MAJOR_VERSION == 3
 		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
@@ -165,12 +161,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->reextract_type->setItemData(4, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(5, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(6, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(0, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(1, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(3, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(4, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(5, 0, Qt::UserRole - 1);
-		_ui->odom_type->setItemData(6, 0, Qt::UserRole - 1);
 #endif
 	}
 	if(!graph::G2OOptimizer::available())
@@ -279,6 +269,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingShowScans[0] = _ui->checkBox_showScans;
 	_3dRenderingShowScans[1] = _ui->checkBox_showOdomScans;
 
+	_3dRenderingDownsamplingScan.resize(2);
+	_3dRenderingDownsamplingScan[0] = _ui->spinBox_downsamplingScan;
+	_3dRenderingDownsamplingScan[1] = _ui->spinBox_downsamplingScan_odom;
+
+	_3dRenderingVoxelSizeScan.resize(2);
+	_3dRenderingVoxelSizeScan[0] = _ui->doubleSpinBox_voxelSizeScan;
+	_3dRenderingVoxelSizeScan[1] = _ui->doubleSpinBox_voxelSizeScan_odom;
+
 	_3dRenderingOpacityScan.resize(2);
 	_3dRenderingOpacityScan[0] = _ui->doubleSpinBox_opacity_scan;
 	_3dRenderingOpacityScan[1] = _ui->doubleSpinBox_opacity_odom_scan;
@@ -295,6 +293,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingMaxDepth[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowScans[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
+		connect(_3dRenderingDownsamplingScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingVoxelSizeScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingOpacity[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingPtSize[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingOpacityScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -333,7 +333,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	//Source panel
 	connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_calibrationName, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->toolButton_source_path_calibration, SIGNAL(clicked()), this, SLOT(selectCalibrationPath()));
+	connect(_ui->lineEdit_calibrationFile, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	_ui->stackedWidget_src->setCurrentIndex(_ui->comboBox_sourceType->currentIndex());
 	connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_src, SLOT(setCurrentIndex(int)));
 	connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -398,8 +399,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_cameraStereoImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_cameraStereoImages_path_left, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathLeft()));
 	connect(_ui->toolButton_cameraStereoImages_path_right, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathRight()));
+	connect(_ui->toolButton_cameraStereoImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathScans()));
 	connect(_ui->lineEdit_cameraStereoImages_path_left, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraStereoImages_path_right, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraStereoImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraStereoImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraStereoImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereoImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereoImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -476,7 +481,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_localSpaceLinksKeptInWM->setObjectName(Parameters::kMemLocalSpaceLinksKeptInWM().c_str());
 	_ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str());
 	_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
-	_ui->general_doubleSpinBox_laserScanVoxel->setObjectName(Parameters::kMemLaserScanVoxelSize().c_str());
+	_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
 
 
 	// Database
@@ -503,6 +508,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
 	_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
 	_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
+	_ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str());
 	_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpWordsPerImage().c_str());
 	_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
 	_ui->checkBox_kp_tfIdfLikelihoodUsed->setObjectName(Parameters::kKpTfIdfLikelihoodUsed().c_str());
@@ -511,6 +517,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->lineEdit_dictionaryPath->setObjectName(Parameters::kKpDictionaryPath().c_str());
 	connect(_ui->toolButton_dictionaryPath, SIGNAL(clicked()), this, SLOT(changeDictionaryPath()));
 	_ui->checkBox_kp_newWordsComparedTogether->setObjectName(Parameters::kKpNewWordsComparedTogether().c_str());
+	_ui->subpix_winSize_kp->setObjectName(Parameters::kKpSubPixWinSize().c_str());
+	_ui->subpix_iterations_kp->setObjectName(Parameters::kKpSubPixIterations().c_str());
+	_ui->subpix_eps_kp->setObjectName(Parameters::kKpSubPixEps().c_str());
 
 	_ui->subpix_winSize->setObjectName(Parameters::kKpSubPixWinSize().c_str());
 	_ui->subpix_iterations->setObjectName(Parameters::kKpSubPixIterations().c_str());
@@ -581,7 +590,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
 	_ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str());
 	_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
-	_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
+	_ui->odomScanHistory->setObjectName(Parameters::kRGBDIcpOdomRefining().c_str());
 	_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
 
 	_ui->graphOptimization_type->setObjectName(Parameters::kRGBDOptimizeStrategy().c_str());
@@ -605,76 +614,57 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str());
 	_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str());
 	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed().c_str());
+	_ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDLocalLoopDetectionPathScansMerged().c_str());
 	_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
 
-	_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
+	_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kVisMinInliers().c_str());
-	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
+	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kVisInlierDistance().c_str());
-	_ui->loopClosure_bowIterations->setObjectName(Parameters::kLccBowIterations().c_str());
+	_ui->loopClosure_bowIterations->setObjectName(Parameters::kVisIterations().c_str());
-	_ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kLccBowRefineIterations().c_str());
+	_ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kVisRefineIterations().c_str());
-	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kLccBowForce2D().c_str());
+	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kVisForce2D().c_str());
-	_ui->loopClosure_estimationType->setObjectName(Parameters::kLccBowEstimationType().c_str());
+	_ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str());
 	connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int)));
-	_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultLccBowEstimationType());
+	_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType());
-	_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kLccBowEpipolarGeometryVar().c_str());
+	_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
-	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kLccBowPnPReprojError().c_str());
+	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
-	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kLccBowPnPFlags().c_str());
+	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
-	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kLccBowVarianceFromInliersCount().c_str());
+	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
 
-	_ui->groupBox_reextract->setObjectName(Parameters::kLccReextractActivated().c_str());
+	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
-	_ui->reextract_nn->setObjectName(Parameters::kLccReextractNNType().c_str());
+	_ui->reextract_nn->setObjectName(Parameters::kVisNNType().c_str());
-	_ui->reextract_nndrRatio->setObjectName(Parameters::kLccReextractNNDR().c_str());
+	_ui->reextract_nndrRatio->setObjectName(Parameters::kVisNNDR().c_str());
-	_ui->reextract_type->setObjectName(Parameters::kLccReextractFeatureType().c_str());
+	_ui->reextract_type->setObjectName(Parameters::kVisFeatureType().c_str());
-	_ui->reextract_maxFeatures->setObjectName(Parameters::kLccReextractMaxWords().c_str());
+	_ui->reextract_maxFeatures->setObjectName(Parameters::kVisMaxFeatures().c_str());
-	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kLccReextractMaxDepth().c_str());
+	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
+	_ui->loopClosure_bowMinDepth->setObjectName(Parameters::kVisMinDepth().c_str());
+	_ui->loopClosure_roi->setObjectName(Parameters::kVisRoiRatios().c_str());
+	_ui->subpix_winSize->setObjectName(Parameters::kVisSubPixWinSize().c_str());
+	_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
+	_ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str());
 
-	_ui->globalDetection_icpType->setObjectName(Parameters::kLccIcpType().c_str());
+	_ui->loopClosure_icp->setObjectName(Parameters::kRGBDIcpLoopClosureRefining().c_str());
-	_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kLccIcpMaxTranslation().c_str());
+	_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str());
-	_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kLccIcpMaxRotation().c_str());
+	_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str());
+	_ui->loopClosure_icp2D->setObjectName(Parameters::kIcp2D().c_str());
 
-	_ui->loopClosure_icpDecimation->setObjectName(Parameters::kLccIcp3Decimation().c_str());
+	_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kIcpVoxelSize().c_str());
-	_ui->loopClosure_icpMaxDepth->setObjectName(Parameters::kLccIcp3MaxDepth().c_str());
+	_ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str());
-	_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kLccIcp3VoxelSize().c_str());
+	_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str());
-	_ui->loopClosure_icpSamples->setObjectName(Parameters::kLccIcp3Samples().c_str());
+	_ui->loopClosure_icpIterations->setObjectName(Parameters::kIcpIterations().c_str());
-	_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp3MaxCorrespondenceDistance().c_str());
+	_ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str());
-	_ui->loopClosure_icpIterations->setObjectName(Parameters::kLccIcp3Iterations().c_str());
+	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
-	_ui->loopClosure_icpRatio->setObjectName(Parameters::kLccIcp3CorrespondenceRatio().c_str());
+	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str());
-	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kLccIcp3PointToPlane().c_str());
-	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kLccIcp3PointToPlaneNormalNeighbors().c_str());
-
-	_ui->loopClosure_icp2MaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp2MaxCorrespondenceDistance().c_str());
-	_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
-	_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
-	_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
 
 
 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
-	_ui->odom_type->setObjectName(Parameters::kOdomFeatureType().c_str());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
-	_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxFeatures().c_str());
-	_ui->odom_inlierDistance->setObjectName(Parameters::kOdomInlierDistance().c_str());
-	_ui->odom_iterations->setObjectName(Parameters::kOdomIterations().c_str());
-	_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
-	_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
-	_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
-	_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
 	_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
 	_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
-	_ui->odom_varianceFromInliersCount->setObjectName(Parameters::kOdomVarianceFromInliersCount().c_str());
-	connect(_ui->odom_varianceFromInliersCount, SIGNAL(clicked(bool)), _ui->loopClosure_bowVarianceFromInliersCount, SLOT(setChecked(bool)));
-	connect(_ui->loopClosure_bowVarianceFromInliersCount, SIGNAL(clicked(bool)), _ui->odom_varianceFromInliersCount, SLOT(setChecked(bool)));
-	_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
-	_ui->odom_estimationType->setObjectName(Parameters::kOdomEstimationType().c_str());
-	connect(_ui->odom_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odomEstimation, SLOT(setCurrentIndex(int)));
-	_ui->stackedWidget_odomEstimation->setCurrentIndex(Parameters::defaultOdomEstimationType());
-	_ui->odom_pnpReprojError->setObjectName(Parameters::kOdomPnPReprojError().c_str());
-	_ui->odom_pnpFlags->setObjectName(Parameters::kOdomPnPFlags().c_str());
 
 	//Odometry BOW
 	_ui->odom_localHistory->setObjectName(Parameters::kOdomBowLocalHistorySize().c_str());
-	_ui->odom_bin_nn->setObjectName(Parameters::kOdomBowNNType().c_str());
-	_ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomBowNNDR().c_str());
 	_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomBowFixedLocalMapPath().c_str());
 	connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
 
@@ -683,9 +673,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_flow_maxLevel->setObjectName(Parameters::kOdomFlowMaxLevel().c_str());
 	_ui->odom_flow_iterations->setObjectName(Parameters::kOdomFlowIterations().c_str());
 	_ui->odom_flow_eps->setObjectName(Parameters::kOdomFlowEps().c_str());
-	_ui->odom_subpix_winSize->setObjectName(Parameters::kOdomSubPixWinSize().c_str());
-	_ui->odom_subpix_iterations->setObjectName(Parameters::kOdomSubPixIterations().c_str());
-	_ui->odom_subpix_eps->setObjectName(Parameters::kOdomSubPixEps().c_str());
 
 	//Odometry Mono
 	_ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str());
@@ -737,6 +724,7 @@ PreferencesDialog::~PreferencesDialog() {
 
 void PreferencesDialog::init()
 {
+	UDEBUG("");
 	//First set all default values
 	const ParametersMap & defaults = Parameters::getDefaultParameters();
 	for(ParametersMap::const_iterator iter=defaults.begin(); iter!=defaults.end(); ++iter)
@@ -1048,6 +1036,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingMaxDepth[i]->setValue(0.0);
 			_3dRenderingShowScans[i]->setChecked(true);
 
+			_3dRenderingDownsamplingScan[i]->setValue(0);
+			_3dRenderingVoxelSizeScan[i]->setValue(0.0);
 			_3dRenderingOpacity[i]->setValue(i==0?1.0:0.5);
 			_3dRenderingPtSize[i]->setValue(2);
 			_3dRenderingOpacityScan[i]->setValue(i==0?1.0:0.5);
@@ -1088,10 +1078,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 	{
 		_ui->general_doubleSpinBox_imgRate->setValue(0.0);
 		_ui->source_mirroring->setChecked(false);
-		_ui->lineEdit_calibrationName->clear();
+		_ui->lineEdit_calibrationFile->clear();
 		_ui->comboBox_sourceType->setCurrentIndex(kSrcRGBD);
 		_ui->lineEdit_sourceDevice->setText("");
-		_ui->lineEdit_sourceLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
+		_ui->lineEdit_sourceLocalTransform->setText("0 0 1 -1 0 0 0 -1 0");
 
 		_ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice);
 		_ui->source_images_spinBox_startPos->setValue(1);
@@ -1160,6 +1150,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_cameraStereoImages_timestamps->setText("");
 		_ui->lineEdit_cameraStereoImages_path_left->setText("");
 		_ui->lineEdit_cameraStereoImages_path_right->setText("");
+		_ui->lineEdit_cameraStereoImages_path_scans->setText("");
+		_ui->lineEdit_cameraStereoImages_laser_transform->setText("0 0 0 0 0 0");
+		_ui->spinBox_cameraStereoImages_max_scan_pts->setValue(0);
 		_ui->checkBox_stereoImages_timestamps->setChecked(false);
 		_ui->checkBox_stereoImages_rectify->setChecked(false);
 		_ui->lineEdit_cameraStereoVideo_path->setText("");
@@ -1280,7 +1273,7 @@ void PreferencesDialog::loadConfigFrom()
 
 void PreferencesDialog::readSettings(const QString & filePath)
 {
-	ULOGGER_DEBUG("");
+	ULOGGER_DEBUG("%s", filePath.toStdString().c_str());
 	readGuiSettings(filePath);
 	readCameraSettings(filePath);
 	if(!readCoreSettings(filePath))
@@ -1348,6 +1341,8 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingMaxDepth[i]->setValue(settings.value(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value()).toDouble());
 		_3dRenderingShowScans[i]->setChecked(settings.value(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked()).toBool());
 
+		_3dRenderingDownsamplingScan[i]->setValue(settings.value(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value()).toInt());
+		_3dRenderingVoxelSizeScan[i]->setValue(settings.value(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value()).toDouble());
 		_3dRenderingOpacity[i]->setValue(settings.value(QString("opacity%1").arg(i), _3dRenderingOpacity[i]->value()).toDouble());
 		_3dRenderingPtSize[i]->setValue(settings.value(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value()).toInt());
 		_3dRenderingOpacityScan[i]->setValue(settings.value(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value()).toDouble());
@@ -1392,7 +1387,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("Camera");
 	_ui->general_doubleSpinBox_imgRate->setValue(settings.value("imgRate", _ui->general_doubleSpinBox_imgRate->value()).toDouble());
 	_ui->source_mirroring->setChecked(settings.value("mirroring", _ui->source_mirroring->isChecked()).toBool());
-	_ui->lineEdit_calibrationName->setText(settings.value("calibrationName", _ui->lineEdit_calibrationName->text()).toString());
+	_ui->lineEdit_calibrationFile->setText(settings.value("calibrationName", _ui->lineEdit_calibrationFile->text()).toString());
 	_ui->comboBox_sourceType->setCurrentIndex(settings.value("type", _ui->comboBox_sourceType->currentIndex()).toInt());
 	_ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString());
 	_ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString());
@@ -1446,6 +1441,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString());
 	_ui->lineEdit_cameraStereoImages_path_left->setText(settings.value("path_left", _ui->lineEdit_cameraStereoImages_path_left->text()).toString());
 	_ui->lineEdit_cameraStereoImages_path_right->setText(settings.value("path_right", _ui->lineEdit_cameraStereoImages_path_right->text()).toString());
+	_ui->lineEdit_cameraStereoImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraStereoImages_path_scans->text()).toString());
+	_ui->lineEdit_cameraStereoImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraStereoImages_laser_transform->text()).toString());
+	_ui->spinBox_cameraStereoImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraStereoImages_max_scan_pts->value()).toInt());
 	_ui->checkBox_stereoImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_stereoImages_timestamps->isChecked()).toBool());
 	_ui->checkBox_stereoImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoImages_rectify->isChecked()).toBool());
 	settings.endGroup(); // StereoImages
@@ -1484,13 +1482,14 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 
 bool PreferencesDialog::readCoreSettings(const QString & filePath)
 {
-	UDEBUG("");
 	QString path = getIniFilePath();
 	if(!filePath.isEmpty())
 	{
 		path = filePath;
 	}
 
+	UDEBUG("%s", path.toStdString().c_str());
+
 	if(!QFile::exists(path))
 	{
 		QMessageBox::information(this, tr("INI file doesn't exist..."), tr("The configuration file \"%1\" does not exist, it will be created with default parameters.").arg(path));
@@ -1533,8 +1532,23 @@ bool PreferencesDialog::readCoreSettings(const QString & filePath)
 	const rtabmap::ParametersMap & parameters = Parameters::getDefaultParameters();
 	for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		QString key((*iter).first.c_str());
+		QString key(iter->first.c_str());
 		QString value = settings.value(key, "").toString();
+		if(value.isEmpty())
+		{
+			// look for old parameter name
+			rtabmap::ParametersMap::const_iterator oldIter = Parameters::getBackwardCompatibilityMap().find(iter->first);
+			if(oldIter!=Parameters::getBackwardCompatibilityMap().end())
+			{
+				value = settings.value(QString(oldIter->second.c_str()), "").toString();
+				if(!value.isEmpty())
+				{
+					UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
+							oldIter->second.c_str(), oldIter->first.c_str(), value.toStdString().c_str());
+				}
+			}
+		}
+
 		if(!value.isEmpty())
 		{
 			if(key.toStdString().compare(Parameters::kRtabmapWorkingDirectory()) == 0)
@@ -1563,6 +1577,7 @@ bool PreferencesDialog::readCoreSettings(const QString & filePath)
 		else
 		{
 			UDEBUG("key.toStdString()=%s", key.toStdString().c_str());
+
 			// Use the default value if the key doesn't exist yet
 			this->setParameter(key.toStdString(), (*iter).second);
 
@@ -1668,6 +1683,8 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value());
 		settings.setValue(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked());
 
+		settings.setValue(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value());
+		settings.setValue(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value());
 		settings.setValue(QString("opacity%1").arg(i), _3dRenderingOpacity[i]->value());
 		settings.setValue(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value());
 		settings.setValue(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value());
@@ -1711,7 +1728,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.beginGroup("Camera");
 	settings.setValue("imgRate", 		 _ui->general_doubleSpinBox_imgRate->value());
 	settings.setValue("mirroring",       _ui->source_mirroring->isChecked());
-	settings.setValue("calibrationName", _ui->lineEdit_calibrationName->text());
+	settings.setValue("calibrationName", _ui->lineEdit_calibrationFile->text());
 	settings.setValue("type",            _ui->comboBox_sourceType->currentIndex());
 	settings.setValue("device", 		 _ui->lineEdit_sourceDevice->text());
 	settings.setValue("localTransform",  _ui->lineEdit_sourceLocalTransform->text());
@@ -1765,6 +1782,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("stamps",         _ui->lineEdit_cameraStereoImages_timestamps->text());
 	settings.setValue("path_left",      _ui->lineEdit_cameraStereoImages_path_left->text());
 	settings.setValue("path_right",     _ui->lineEdit_cameraStereoImages_path_right->text());
+	settings.setValue("path_scans",     _ui->lineEdit_cameraStereoImages_path_scans->text());
+	settings.setValue("scan_transform", _ui->lineEdit_cameraStereoImages_laser_transform->text());
+	settings.setValue("scan_max_pts", _ui->spinBox_cameraStereoImages_max_scan_pts->value());
 	settings.setValue("filenames_as_stamps", _ui->checkBox_stereoImages_timestamps->isChecked());
 	settings.setValue("rectify", 	    _ui->checkBox_stereoImages_rectify->isChecked());
 	settings.endGroup(); // StereoImages
@@ -1889,14 +1909,6 @@ bool PreferencesDialog::validateForm()
 					   "of features on loop closure."));
 			_ui->reextract_type->setCurrentIndex(Feature2D::kFeatureFastBrief);
 		}
-		// odom type
-		if(_ui->odom_type->currentIndex() <= 1)
-		{
-			QMessageBox::warning(this, tr("Parameter warning"),
-					tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
-					   "with the nonfree module from OpenCV. GFTT/Brief is set instead for odometry."));
-			_ui->odom_type->setCurrentIndex(Feature2D::kFeatureGfttBrief);
-		}
 	}
 
 	// optimization strategy
@@ -1956,32 +1968,6 @@ bool PreferencesDialog::validateForm()
 		_ui->reextract_nn->setCurrentIndex(VWDictionary::kNNBruteForce);
 	}
 
-	// odom type
-	if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannLSH && _ui->odom_type->currentIndex() <= 1)
-	{
-		QMessageBox::warning(this, tr("Parameter warning"),
-				tr("With the selected feature type (SURF or SIFT), parameter \"Odometry->Nearest Neighbor\" "
-				   "cannot be LSH (used for binary descriptor). KD-tree is set instead for odometry."));
-		_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannKdTree);
-	}
-	else if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannKdTree && _ui->odom_type->currentIndex() >1)
-	{
-		QMessageBox::warning(this, tr("Parameter warning"),
-				tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Odometry->Nearest Neighbor\" "
-				   "cannot be KD-Tree (used for float descriptor). BruteForce matching is set instead for odometry."));
-		_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNBruteForce);
-	}
-
-	if(_ui->loopClosure_bowVarianceFromInliersCount->isChecked() != _ui->odom_varianceFromInliersCount->isChecked())
-	{
-		QMessageBox::warning(this, tr("Parameter warning"),
-				tr("Odometry %1 variance from inliers count but Loop Closure constraint %2. "
-				   "Applying the same parameter to Loop Closure Constraint.")
-				   .arg(_ui->odom_varianceFromInliersCount->isChecked()?tr("uses"):tr("does not use"))
-				   .arg(_ui->odom_varianceFromInliersCount->isChecked()?tr("does not"):tr("does")));
-		_ui->loopClosure_bowVarianceFromInliersCount->setChecked(_ui->odom_varianceFromInliersCount->isChecked());
-	}
-
 	if(_ui->doubleSpinBox_freenect2MinDepth->value() >= _ui->doubleSpinBox_freenect2MaxDepth->value())
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
@@ -1992,6 +1978,28 @@ bool PreferencesDialog::validateForm()
 		_ui->doubleSpinBox_freenect2MaxDepth->setValue(_ui->doubleSpinBox_freenect2MaxDepth->value()+1);
 	}
 
+	if(_ui->surf_doubleSpinBox_maxDepth->value() > 0.0 &&
+	   _ui->surf_doubleSpinBox_minDepth->value() >= _ui->surf_doubleSpinBox_maxDepth->value())
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Visual word minimum depth (%1 m) should be lower than maximum depth (%2 m). Setting maximum depth to %3 m.")
+				   .arg(_ui->surf_doubleSpinBox_minDepth->value())
+				   .arg(_ui->surf_doubleSpinBox_maxDepth->value())
+				   .arg(_ui->surf_doubleSpinBox_maxDepth->value()+1));
+		_ui->doubleSpinBox_freenect2MinDepth->setValue(0);
+	}
+
+	if(_ui->loopClosure_bowMaxDepth->value() > 0.0 &&
+	   _ui->loopClosure_bowMinDepth->value() >= _ui->loopClosure_bowMaxDepth->value())
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Visual registration word minimum depth (%1 m) should be lower than maximum depth (%2 m). Setting maximum depth to %3 m.")
+				   .arg(_ui->loopClosure_bowMinDepth->value())
+				   .arg(_ui->loopClosure_bowMaxDepth->value())
+				   .arg(_ui->loopClosure_bowMaxDepth->value()+1));
+		_ui->loopClosure_bowMinDepth->setValue(0);
+	}
+
 	return true;
 }
 
@@ -2014,7 +2022,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->label_dictionaryPath->setEnabled(false);
 
 		_ui->groupBox_source0->setEnabled(false);
-		_ui->groupBox_odometry1->setEnabled(false);
+		_ui->groupBox_odometry2->setEnabled(false);
 
 		this->setWindowTitle(tr("Preferences [Monitoring mode]"));
 	}
@@ -2029,7 +2037,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->label_dictionaryPath->setEnabled(true);
 
 		_ui->groupBox_source0->setEnabled(true);
-		_ui->groupBox_odometry1->setEnabled(true);
+		_ui->groupBox_odometry2->setEnabled(true);
 
 		this->setWindowTitle(tr("Preferences"));
 	}
@@ -2336,6 +2344,20 @@ void PreferencesDialog::openDatabaseViewer()
 	}
 }
 
+void PreferencesDialog::selectCalibrationPath()
+{
+	QString dir = _ui->lineEdit_calibrationFile->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory()+"/camera_info";
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml)"));
+	if(path.size())
+	{
+		_ui->lineEdit_calibrationFile->setText(path);
+	}
+}
+
 void PreferencesDialog::selectSourceRGBDImagesStamps()
 {
 	QString dir = _ui->lineEdit_cameraRGBDImages_timestamps->text();
@@ -2420,6 +2442,20 @@ void PreferencesDialog::selectSourceStereoImagesPathRight()
 	}
 }
 
+void PreferencesDialog::selectSourceStereoImagesPathScans()
+{
+	QString dir = _ui->lineEdit_cameraStereoImages_path_scans->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getExistingDirectory(this, tr("Select scans directory"), dir);
+	if(path.size())
+	{
+		_ui->lineEdit_cameraStereoImages_path_scans->setText(path);
+	}
+}
+
 void PreferencesDialog::selectSourceImagesPath()
 {
 	QString dir = _ui->source_images_lineEdit_path->text();
@@ -2534,8 +2570,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 				{
 					if(valueInt <= 1 &&
 							(combo->objectName().toStdString().compare(Parameters::kKpDetectorStrategy()) == 0 ||
-							combo->objectName().toStdString().compare(Parameters::kLccReextractFeatureType()) == 0 ||
+							 combo->objectName().toStdString().compare(Parameters::kVisFeatureType()) == 0))
-							combo->objectName().toStdString().compare(Parameters::kOdomFeatureType()) == 0))
 					{
 						UWARN("Trying to set \"%s\" to SIFT/SURF but RTAB-Map isn't built "
 							  "with the nonfree module from OpenCV. Keeping default combo value: %s.",
@@ -2545,8 +2580,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 					}
 					else if(valueInt==1 &&
 							(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
-							combo->objectName().toStdString().compare(Parameters::kLccReextractNNType()) == 0 ||
+							 combo->objectName().toStdString().compare(Parameters::kVisNNType()) == 0))
-							combo->objectName().toStdString().compare(Parameters::kOdomBowNNType()) == 0))
 
 					{
 						UWARN("Trying to set \"%s\" to KdTree but RTAB-Map isn't built "
@@ -2691,7 +2725,6 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 					}
 				}
 				else if(comboBox == _ui->comboBox_detector_strategy ||
-						comboBox == _ui->odom_type ||
 						comboBox == _ui->reextract_type)
 				{
 					if(value == 0) //  surf
@@ -2731,25 +2764,10 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 						this->addParameters(_ui->groupBox_detector_brisk2);
 					}
 				}
-				else if(comboBox == _ui->globalDetection_icpType)
-				{
-					if(value == 1) // 1 icp3
-					{
-						this->addParameters(_ui->groupBox_loopClosure_icp3);
-					}
-					else if(value == 2) // 2 icp2
-					{
-						this->addParameters(_ui->groupBox_loopClosure_icp2);
-					}
-				}
 				else if(comboBox == _ui->loopClosure_estimationType)
 				{
 					this->addParameters(_ui->stackedWidget_loopClosureEstimation, _ui->loopClosure_estimationType->currentIndex());
 				}
-				else if(comboBox == _ui->odom_estimationType)
-				{
-					this->addParameters(_ui->stackedWidget_odomEstimation, _ui->stackedWidget_odomEstimation->currentIndex());
-				}
 				else if(comboBox == _ui->graphOptimization_type)
 				{
 					this->addParameter(_ui->graphOptimization_iterations,        _ui->graphOptimization_iterations->value());
@@ -2802,6 +2820,11 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 				this->addParameters(_ui->groupBox_localDetection_time);
 				this->addParameters(_ui->groupBox_localDetection_space);
 				this->addParameters(_ui->groupBox_visualTransform2);
+				this->addParameters(_ui->groupBox_icp2);
+			}
+			else if(value && checkbox == _ui->loopClosure_icp)
+			{
+				this->addParameters(_ui->groupBox_icp2);
 			}
 
 			if(groupBox)
@@ -2813,7 +2836,7 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 				}
 				if(value && groupBox == _ui->groupBox_localDetection_space)
 				{
-					this->addParameters(_ui->groupBox_loopClosure_icp2);
+					this->addParameters(_ui->groupBox_icp2);
 				}
 
 				this->addParameters(groupBox);
@@ -3345,6 +3368,16 @@ bool PreferencesDialog::isScansShown(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingShowScans[index]->isChecked();
 }
+int PreferencesDialog::getDownsamplingStepScan(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingDownsamplingScan[index]->value();
+}
+double PreferencesDialog::getCloudVoxelSizeScan(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingVoxelSizeScan[index]->value();
+}
 double PreferencesDialog::getScanOpacity(int index) const
 {
 	UASSERT(index >= 0 && index <= 1);
@@ -3426,10 +3459,6 @@ bool PreferencesDialog::isSourceMirroring() const
 {
 	return _ui->source_mirroring->isChecked();
 }
-QString PreferencesDialog::getCalibrationName() const
-{
-	return _ui->lineEdit_calibrationName->text();
-}
 PreferencesDialog::Src PreferencesDialog::getSourceType() const
 {
 	int index = _ui->comboBox_sourceType->currentIndex();
@@ -3498,49 +3527,23 @@ QString PreferencesDialog::getSourceDevice() const
 {
 	return _ui->lineEdit_sourceDevice->text();
 }
+
 Transform PreferencesDialog::getSourceLocalTransform() const
 {
-	Transform t = Transform::getIdentity();
+	Transform t = Transform::fromString(_ui->lineEdit_sourceLocalTransform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
-	QString str = _ui->lineEdit_sourceLocalTransform->text();
+	if(t.isNull())
-	str.replace("PI_2", QString::number(3.141592/2.0));
-	QStringList list = str.split(' ');
-	if(list.size() == 6 || list.size() == 9 || list.size() == 12)
 	{
-		std::vector<float> numbers(list.size());
+		return Transform::getIdentity();
-		bool ok = false;
-		for(int i=0; i<list.size(); ++i)
-		{
-			numbers[i] = list.at(i).toDouble(&ok);
-			if(!ok)
-			{
-				UERROR("Parsing local transform failed! \"%s\" not recognized (%s)",
-						list.at(i).toStdString().c_str(), str.toStdString().c_str());
-				break;
-			}
-		}
-		if(ok)
-		{
-			if(numbers.size() == 6)
-			{
-				t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
-			}
-			else if(numbers.size() == 9)
-			{
-				t = Transform(numbers[0], numbers[1], numbers[2], 0,
-						      numbers[3], numbers[4], numbers[5], 0,
-						      numbers[6], numbers[7], numbers[8], 0);
-			}
-			else if(numbers.size() == 12)
-			{
-				t = Transform(numbers[0], numbers[1], numbers[2], numbers[9],
-							  numbers[3], numbers[4], numbers[5], numbers[10],
-							  numbers[6], numbers[7], numbers[8], numbers[11]);
-			}
-		}
 	}
-	else
+	return t;
+}
+
+Transform PreferencesDialog::getStereoLaserLocalTransform() const
+{
+	Transform t = Transform::fromString(_ui->lineEdit_cameraStereoImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
+	if(t.isNull())
 	{
-		UERROR("Local transform is wrong! must have 6 or 9 items (%s)", str.toStdString().c_str());
+		return Transform::getIdentity();
 	}
 	return t;
 }
@@ -3700,6 +3703,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	else if(driver == kSrcStereoImages)
 	{
 		camera = new CameraStereoImages(
+			_ui->lineEdit_cameraStereoImages_path_scans->text().append(QDir::separator()).toStdString(),
+			this->getStereoLaserLocalTransform(),
+			_ui->spinBox_cameraStereoImages_max_scan_pts->value(),
 			_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
 			_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
 			_ui->checkBox_stereoImages_timestamps->isChecked(),
@@ -3755,7 +3761,20 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	if(camera)
 	{
 		// don't set calibration folder if we want raw images
-		if(!camera->init(useRawImages?"":this->getCameraInfoDir().toStdString(), this->getCalibrationName().toStdString()))
+		QString dir = this->getCameraInfoDir();
+		QString name = QFileInfo(_ui->lineEdit_calibrationFile->text()
+				.remove("_left.yaml")
+				.remove("_right.yaml")
+				.remove("_pose.yaml")).baseName();
+		if(!_ui->lineEdit_calibrationFile->text().isEmpty())
+		{
+			QDir d = QFileInfo(_ui->lineEdit_calibrationFile->text()).dir();
+			if(!d.path().isEmpty())
+			{
+				dir = d.absolutePath();
+			}
+		}
+		if(!camera->init(useRawImages?"":dir.toStdString(), name.toStdString()))
 		{
 			UWARN("init camera failed... ");
 			QMessageBox::warning(this,
@@ -3809,7 +3828,7 @@ int PreferencesDialog::getOdomBufferSize() const
 {
 	return _ui->odom_dataBufferSize->value();
 }
-bool PreferencesDialog::getLccBowVarianceFromInliersCount() const
+bool PreferencesDialog::getRegVarianceFromInliersCount() const
 {
 	return _ui->loopClosure_bowVarianceFromInliersCount->isChecked();
 }
@@ -3906,11 +3925,6 @@ void PreferencesDialog::setSLAMMode(bool enabled)
 }
 
 void PreferencesDialog::testOdometry()
-{
-	testOdometry(_ui->odom_type->currentIndex());
-}
-
-void PreferencesDialog::testOdometry(int type)
 {
 	DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
 					 _ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
@@ -4098,7 +4112,7 @@ void PreferencesDialog::calibrate()
 
 void PreferencesDialog::calibrateSimple()
 {
-	CreateSimpleCalibrationDialog dialog(this->getCameraInfoDir(), _ui->lineEdit_calibrationName->text(), this);
+	CreateSimpleCalibrationDialog dialog(this->getCameraInfoDir(), "", this);
 	dialog.exec();
 }
 

guilib/src/ui/DatabaseViewer.ui

@@ -50,8 +50,8 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>166</width>
+             <width>181</width>
-             <height>173</height>
+             <height>184</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -236,8 +236,8 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>165</width>
+             <width>181</width>
-             <height>173</height>
+             <height>184</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -418,7 +418,7 @@
      <x>0</x>
      <y>0</y>
      <width>1285</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -430,6 +430,7 @@
     <addaction name="actionSave_config"/>
     <addaction name="separator"/>
     <addaction name="actionGenerate_3D_map_pcd"/>
+    <addaction name="actionExport_3D_laser_scans_ply_pcd"/>
     <addaction name="actionExport"/>
     <addaction name="actionExtract_images"/>
     <addaction name="separator"/>
@@ -453,6 +454,7 @@
     <addaction name="actionReset_all_changes"/>
     <addaction name="separator"/>
     <addaction name="actionView_3D_map"/>
+    <addaction name="actionView_3D_laser_scans"/>
    </widget>
    <widget class="QMenu" name="menuView">
     <property name="title">
@@ -826,25 +828,103 @@
    </attribute>
    <widget class="QWidget" name="dockWidgetContents_3">
     <layout class="QVBoxLayout" name="verticalLayout_10">
+     <item>
+      <layout class="QHBoxLayout" name="horizontalLayout_11">
+       <item>
+        <widget class="QComboBox" name="comboBox_logger_level">
+         <property name="currentIndex">
+          <number>1</number>
+         </property>
+         <item>
+          <property name="text">
+           <string>Debug</string>
+          </property>
+         </item>
+         <item>
+          <property name="text">
+           <string>Info</string>
+          </property>
+         </item>
+         <item>
+          <property name="text">
+           <string>Warning</string>
+          </property>
+         </item>
+         <item>
+          <property name="text">
+           <string>Error</string>
+          </property>
+         </item>
+        </widget>
+       </item>
+       <item>
+        <widget class="QLabel" name="label_logger_level">
+         <property name="text">
+          <string>Logger level</string>
+         </property>
+        </widget>
+       </item>
+      </layout>
+     </item>
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>1</number>
+        <number>0</number>
        </property>
        <widget class="QWidget" name="page">
         <property name="geometry">
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>314</width>
+          <width>312</width>
-          <height>303</height>
+          <height>374</height>
          </rect>
         </property>
         <attribute name="label">
          <string>ICP</string>
         </attribute>
         <layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
-         <item row="0" column="0">
+         <item row="8" column="0">
+          <widget class="QSpinBox" name="spinBox_icp_iteration">
+           <property name="minimum">
+            <number>1</number>
+           </property>
+           <property name="maximum">
+            <number>1000</number>
+           </property>
+           <property name="value">
+            <number>30</number>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="1">
+          <widget class="QLabel" name="label_15">
+           <property name="text">
+            <string>Iteration</string>
+           </property>
+          </widget>
+         </item>
+         <item row="9" column="1">
+          <widget class="QLabel" name="label_11">
+           <property name="text">
+            <string>Point to plane</string>
+           </property>
+          </widget>
+         </item>
+         <item row="10" column="0">
+          <widget class="QSpinBox" name="spinBox_icp_normalKSearch">
+           <property name="minimum">
+            <number>1</number>
+           </property>
+           <property name="maximum">
+            <number>1000</number>
+           </property>
+           <property name="value">
+            <number>20</number>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="0">
           <widget class="QSpinBox" name="spinBox_icp_decimation">
            <property name="minimum">
             <number>1</number>
@@ -857,14 +937,14 @@
            </property>
           </widget>
          </item>
-         <item row="0" column="1">
+         <item row="1" column="1">
           <widget class="QLabel" name="label_14">
            <property name="text">
             <string>Decimation</string>
            </property>
           </widget>
          </item>
-         <item row="1" column="0">
+         <item row="2" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_icp_maxDepth">
            <property name="suffix">
             <string> m</string>
@@ -880,14 +960,7 @@
            </property>
           </widget>
          </item>
-         <item row="1" column="1">
+         <item row="4" column="0">
-          <widget class="QLabel" name="label_17">
-           <property name="text">
-            <string>Max depth</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_icp_voxel">
            <property name="suffix">
             <string> m</string>
@@ -903,14 +976,14 @@
            </property>
           </widget>
          </item>
-         <item row="2" column="1">
+         <item row="4" column="1">
           <widget class="QLabel" name="label_13">
            <property name="text">
-            <string>Voxel</string>
+            <string>Voxel size</string>
            </property>
           </widget>
          </item>
-         <item row="4" column="0">
+         <item row="6" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_icp_maxCorrespDistance">
            <property name="suffix">
             <string> m</string>
@@ -929,14 +1002,31 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="1">
+         <item row="6" column="1">
           <widget class="QLabel" name="label_12">
            <property name="text">
             <string>Max correspondence distance</string>
            </property>
           </widget>
          </item>
-         <item row="5" column="0">
+         <item row="11" column="0">
+          <widget class="QCheckBox" name="checkBox_icp_2d">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>false</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="2" column="1">
+          <widget class="QLabel" name="label_17">
+           <property name="text">
+            <string>Max depth</string>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_icp_minCorrespondenceRatio">
            <property name="suffix">
             <string/>
@@ -955,34 +1045,14 @@
            </property>
           </widget>
          </item>
-         <item row="5" column="1">
+         <item row="7" column="1">
           <widget class="QLabel" name="label_46">
            <property name="text">
             <string>Min correspondence ratio</string>
            </property>
           </widget>
          </item>
-         <item row="6" column="0">
+         <item row="9" column="0">
-          <widget class="QSpinBox" name="spinBox_icp_iteration">
-           <property name="minimum">
-            <number>1</number>
-           </property>
-           <property name="maximum">
-            <number>1000</number>
-           </property>
-           <property name="value">
-            <number>30</number>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="1">
-          <widget class="QLabel" name="label_15">
-           <property name="text">
-            <string>Iteration</string>
-           </property>
-          </widget>
-         </item>
-         <item row="7" column="0">
           <widget class="QCheckBox" name="checkBox_icp_p2plane">
            <property name="text">
             <string/>
@@ -992,51 +1062,21 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="1">
+         <item row="10" column="1">
-          <widget class="QLabel" name="label_11">
-           <property name="text">
-            <string>Point to plane</string>
-           </property>
-          </widget>
-         </item>
-         <item row="8" column="0">
-          <widget class="QSpinBox" name="spinBox_icp_normalKSearch">
-           <property name="minimum">
-            <number>1</number>
-           </property>
-           <property name="maximum">
-            <number>1000</number>
-           </property>
-           <property name="value">
-            <number>20</number>
-           </property>
-          </widget>
-         </item>
-         <item row="8" column="1">
           <widget class="QLabel" name="label_20">
            <property name="text">
             <string>Normal K neighbors</string>
            </property>
           </widget>
          </item>
-         <item row="9" column="0">
+         <item row="11" column="1">
-          <widget class="QCheckBox" name="checkBox_icp_2d">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="9" column="1">
           <widget class="QLabel" name="label_27">
            <property name="text">
-            <string>2D icp (laser scans required)</string>
+            <string>2D icp</string>
            </property>
           </widget>
          </item>
-         <item row="10" column="0">
+         <item row="12" column="0">
           <spacer name="verticalSpacer">
            <property name="orientation">
             <enum>Qt::Vertical</enum>
@@ -1049,6 +1089,43 @@
            </property>
           </spacer>
          </item>
+         <item row="0" column="1">
+          <widget class="QLabel" name="label_68">
+           <property name="text">
+            <string>Use laser scans for ICP</string>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="0">
+          <widget class="QCheckBox" name="checkBox_icp_laserScan">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="1">
+          <widget class="QLabel" name="label_69">
+           <property name="text">
+            <string>Downsample step size</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="0">
+          <widget class="QSpinBox" name="spinBox_icp_downsamplingStepSize">
+           <property name="minimum">
+            <number>1</number>
+           </property>
+           <property name="maximum">
+            <number>999999</number>
+           </property>
+           <property name="singleStep">
+            <number>1000</number>
+           </property>
+           <property name="value">
+            <number>1</number>
+           </property>
+          </widget>
+         </item>
         </layout>
        </widget>
        <widget class="QWidget" name="page_2">
@@ -1056,8 +1133,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>351</width>
+          <width>366</width>
-          <height>407</height>
+          <height>420</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1389,8 +1466,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>333</width>
+          <width>338</width>
-          <height>333</height>
+          <height>330</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1628,8 +1705,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>255</width>
+          <width>261</width>
-          <height>377</height>
+          <height>411</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1886,7 +1963,7 @@
           <x>0</x>
           <y>0</y>
           <width>201</width>
-          <height>117</height>
+          <height>126</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1985,8 +2062,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>285</width>
+          <width>283</width>
-          <height>309</height>
+          <height>322</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2394,7 +2471,7 @@
   </action>
   <action name="actionGenerate_3D_map_pcd">
    <property name="text">
-    <string>Export 3D map (*.pcd) ...</string>
+    <string>Export 3D map (*.ply *.pcd) ...</string>
    </property>
   </action>
   <action name="actionExport">
@@ -2457,6 +2534,16 @@
     <string>Generate g2o graph (*.g2o)...</string>
    </property>
   </action>
+  <action name="actionView_3D_laser_scans">
+   <property name="text">
+    <string>View 3D laser scans...</string>
+   </property>
+  </action>
+  <action name="actionExport_3D_laser_scans_ply_pcd">
+   <property name="text">
+    <string>Export 3D laser scans (*.ply *.pcd) ...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

tools/ConsoleApp/main.cpp

@@ -286,6 +286,54 @@ int main(int argc, char * argv[])
 			continue;
 		}
 
+		//backward compatibility
+		// look for old parameter name
+		std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
+		if(oldIter!=Parameters::getRemovedParameters().end())
+		{
+			++i;
+			if(i < argc)
+			{
+				std::string value = argv[i];
+				if(value.empty())
+				{
+					showUsage();
+				}
+				else
+				{
+					value = uReplaceChar(value, ',', ' ');
+				}
+
+				if(oldIter->second.first)
+				{
+					key = oldIter->second.second;
+					UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
+							oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
+				}
+				else if(oldIter->second.second.empty())
+				{
+					UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
+				}
+				else
+				{
+					UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
+				}
+				if(oldIter->second.first)
+				{
+					std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
+					if(inserted.second == false)
+					{
+						inserted.first->second = value;
+					}
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
 		printf("Unrecognized option : %s\n", argv[i]);
 		showUsage();
 	}

tools/OdometryViewer/main.cpp

@@ -100,17 +100,17 @@ int main (int argc, char * argv[])
 	float rate = 0.0;
 	std::string inputDatabase;
 	int driver = 0;
-	int odomType = rtabmap::Parameters::defaultOdomFeatureType();
+	int odomType = rtabmap::Parameters::defaultVisFeatureType();
 	bool icp = false;
 	bool flow = false;
 	bool mono = false;
-	int nnType = rtabmap::Parameters::defaultOdomBowNNType();
+	int nnType = rtabmap::Parameters::defaultVisNNType();
-	float nndr = rtabmap::Parameters::defaultOdomBowNNDR();
+	float nndr = rtabmap::Parameters::defaultVisNNDR();
-	float distance = rtabmap::Parameters::defaultOdomInlierDistance();
+	float distance = rtabmap::Parameters::defaultVisInlierDistance();
-	int maxWords = rtabmap::Parameters::defaultOdomMaxFeatures();
+	int maxWords = rtabmap::Parameters::defaultVisMaxFeatures();
-	int minInliers = rtabmap::Parameters::defaultOdomMinInliers();
+	int minInliers = rtabmap::Parameters::defaultVisMinInliers();
-	float maxDepth = rtabmap::Parameters::defaultOdomMaxDepth();
+	float maxDepth = rtabmap::Parameters::defaultVisMaxDepth();
-	int iterations = rtabmap::Parameters::defaultOdomIterations();
+	int iterations = rtabmap::Parameters::defaultVisIterations();
 	int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
 	int decimation = 4;
 	float voxel = 0.005;
@@ -620,7 +620,7 @@ int main (int argc, char * argv[])
 
 	rtabmap::ParametersMap parameters;
 
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMaxDepth(), uNumber2Str(maxDepth)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
 
 	if(!icp)
@@ -630,11 +630,11 @@ int main (int argc, char * argv[])
 		UINFO("RANSAC iterations =       %d", iterations);
 		UINFO("Max features =            %d", maxWords);
 		UINFO("GPU =                     %s", gpu?"true":"false");
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisInlierDistance(), uNumber2Str(distance)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), uNumber2Str(minInliers)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisIterations(), uNumber2Str(iterations)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMaxFeatures(), uNumber2Str(maxWords)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisFeatureType(), uNumber2Str(odomType)));
 		if(odomType == 0)
 		{
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
@@ -666,15 +666,15 @@ int main (int argc, char * argv[])
 			UINFO("Nearest neighbor =         %s", nnName.c_str());
 			UINFO("Nearest neighbor ratio =  %f", nndr);
 			UINFO("Local history =           %d", localHistory);
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisNNType(), uNumber2Str(nnType)));
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisNNDR(), uNumber2Str(nndr)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
 
 			if(mono)
 			{
-				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPFlags(), uNumber2Str(0))); //CV_ITERATIVE
+				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPFlags(), uNumber2Str(0))); //CV_ITERATIVE
-				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPReprojError(), "4.0"));
+				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPReprojError(), "4.0"));
-				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), "100"));
+				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisIterations(), "100"));
 				odom = new rtabmap::OdometryMono(parameters);
 			}
 			else