Refactoring Registration: added RegistrationInfo class, pipeline registration. Camera source: create laser scan from depth image. Removed OdometryICP as the same behavior can be achieved with OdometryF2F and "Reg/Strategy" = 1 (ICP) or 2 (Vis+ICP).

corelib/include/rtabmap/core/CameraEvent.h

@@ -48,7 +48,7 @@ public:
 		UEvent(kCodeData),
 		data_(image, seq, stamp)
 	{
-		cameraInfo_.cameraName_ = cameraName;
+		cameraInfo_.cameraName = cameraName;
 	}
 
 	CameraEvent() :
@@ -66,7 +66,7 @@ public:
 		UEvent(kCodeData),
 		data_(data)
 	{
-		cameraInfo_.cameraName_ = cameraName;
+		cameraInfo_.cameraName = cameraName;
 	}
 	CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) :
 		UEvent(kCodeData),
@@ -77,7 +77,7 @@ public:
 
 	// Image or descriptors
 	const SensorData & data() const {return data_;}
-	const std::string & cameraName() const {return cameraInfo_.cameraName_;}
+	const std::string & cameraName() const {return cameraInfo_.cameraName;}
 	const CameraInfo & info() const {return cameraInfo_;}
 
 	virtual ~CameraEvent() {}

corelib/include/rtabmap/core/CameraInfo.h

@@ -37,20 +37,22 @@ class CameraInfo
 
 public:
 	CameraInfo() :
-		cameraName_(""),
+		cameraName(""),
-		id_(0),
+		id(0),
-		timeCapture_(0.0),
+		timeCapture(0.0f),
-		timeDisparity_(0.0),
+		timeDisparity(0.0f),
-		timeMirroring_(0.0)
+		timeMirroring(0.0f),
+		timeScanFromDepth(0.0f)
 	{
 	}
 	virtual ~CameraInfo() {}
 
-	std::string cameraName_;
+	std::string cameraName;
-	int id_;
+	int id;
-	float timeCapture_;
+	float timeCapture;
-	float timeDisparity_;
+	float timeDisparity;
-	float timeMirroring_;
+	float timeMirroring;
+	float timeScanFromDepth;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -55,6 +55,8 @@ public:
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
+	void setScanFromDepth(bool enabled, int decimation=4, float maxDepth=4.0f)
+	{_scanFromDepth = enabled; _scanDecimation=decimation; _scanMaxDepth = maxDepth;}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -72,6 +74,9 @@ private:
 	bool _mirroring;
 	bool _colorOnly;
 	bool _stereoToDepth;
+	bool _scanFromDepth;
+	int _scanDecimation;
+	float _scanMaxDepth;
 	StereoDense * _stereoDense;
 };
 

corelib/include/rtabmap/core/Memory.h

@@ -51,7 +51,8 @@ class VWDictionary;
 class VisualWord;
 class Feature2D;
 class Statistics;
-class RegistrationVis;
+class Registration;
+class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
 
@@ -178,15 +179,13 @@ public:
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 
-	Transform computeVisualTransform(int fromId, int toId, std::string * rejectedMsg = 0, int * inliers = 0, float * variance = 0);
+	Transform computeTransform(int fromId, int toId, RegistrationInfo * info = 0);
-	Transform computeIcpTransform(int fromId, int toId, Transform guess, std::string * rejectedMsg = 0, int * correspondences = 0, float * variance = 0, float * correspondencesRatio = 0);
+	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(
 			int newId,
 			int oldId,
 			const std::map<int, Transform> & poses,
-			std::string * rejectedMsg = 0,
+			RegistrationInfo * info = 0);
-			int * inliers = 0,
-			float * variance = 0);
 
 private:
 	void preUpdate();
@@ -264,7 +263,7 @@ private:
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
 
-	RegistrationVis * _registrationVis;
+	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcp;
 };
 

corelib/include/rtabmap/core/Odometry.h

@@ -50,7 +50,7 @@ public:
 
 public:
 	static Odometry * create(const ParametersMap & parameters);
-	static Odometry * create(Type & type, const ParametersMap & parameters);
+	static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
 
 public:
 	virtual ~Odometry();

corelib/include/rtabmap/core/OdometryF2F.h

@@ -29,10 +29,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYF2F_H_
 
 #include <rtabmap/core/Odometry.h>
-#include <rtabmap/core/RegistrationVis.h>
+#include <rtabmap/core/Signature.h>
 
 namespace rtabmap {
 
+class Registration;
+
 class RTABMAP_EXP OdometryF2F : public Odometry
 {
 public:
@@ -51,7 +53,7 @@ private:
 	int keyFrameThr_;
 	bool guessFromMotion_;
 
-	RegistrationVis registration_;
+	Registration * registrationPipeline_;
 	Signature refFrame_;
 	Transform motionSinceLastKeyFrame_;
 };

corelib/include/rtabmap/core/OdometryICP.h

@@ -1,68 +0,0 @@
-/*
-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 ODOMETRYICP_H_
-#define ODOMETRYICP_H_
-
-#include <rtabmap/core/Odometry.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-namespace rtabmap {
-
-class RTABMAP_EXP OdometryICP : public Odometry
-{
-public:
-	OdometryICP(int decimation = 4,
-			float voxelSize = 0.005f,
-			int samples = 0,
-			float maxCorrespondenceDistance = 0.05f,
-			int maxIterations = 30,
-			float correspondenceRatio = 0.7f,
-			bool pointToPlane = true,
-			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
-	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-
-private:
-	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
-
-private:
-	int _decimation;
-	float _voxelSize;
-	float _samples;
-	float _maxCorrespondenceDistance;
-	int	_maxIterations;
-	float _correspondenceRatio;
-	bool _pointToPlane;
-
-	pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane
-	pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
-};
-
-}
-
-#endif /* ODOMETRYICP_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -311,7 +311,6 @@ class RTABMAP_EXP Parameters
 	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, NeighborLinkRefining,          bool, false,  "When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using ICP (laser scans required!).");
-	RTABMAP_PARAM(RGBD, LoopClosureLinkRefining,      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/Proximity loop closure detection
@@ -361,6 +360,8 @@ class RTABMAP_EXP Parameters
 
 	// Common registration parameters
 	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(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
+	RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
 	
 	// Visual registration parameters
 	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
@@ -373,7 +374,6 @@ class RTABMAP_EXP Parameters
 	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, 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).");
@@ -394,7 +394,6 @@ class RTABMAP_EXP Parameters
 	// ICP registration parameters
 	RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
 	RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
-	RTABMAP_PARAM(Icp, 2D,                        bool, true, 	"If 2D ICP is done (only 3Dof -> x,y,yaw).");
 	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.025, "Uniform sampling voxel size (0=disabled).");
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");

corelib/include/rtabmap/core/Registration.h

@@ -32,50 +32,75 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Signature.h>
+#include <rtabmap/core/RegistrationInfo.h>
 
 namespace rtabmap {
 
 class RTABMAP_EXP Registration
 {
 public:
-	virtual ~Registration() {}
+	enum Type {
-	virtual void parseParameters(const ParametersMap & parameters)
+		kTypeUndef = -1,
-	{
+		kTypeVis = 0,
-		Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
+		kTypeIcp = 1,
-	}
+		kTypeVisIcp = 2
+	};
+
+public:
+	static Registration * create(const ParametersMap & parameters);
+	static Registration * create(Type & type, const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual ~Registration();
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	bool isImageRequired() const;
+	bool isScanRequired() const;
+	bool isUserDataRequired() const;
+
+	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
+	bool force3DoF() const {return force3DoF_;}
+
+	// take ownership!
+	void setChildRegistration(Registration * child);
 
 	Transform computeTransformation(
 			const Signature & from,
 			const Signature & to,
 			Transform guess = Transform::getIdentity(),
-			std::string * rejectedMsg = 0,
+			RegistrationInfo * info = 0) const;
-			std::vector<int> * inliersOut = 0,
+	Transform computeTransformation(
-			float * varianceOut = 0,
+			const SensorData & from,
-			float * inliersRatioOut = 0) const
+			const SensorData & to,
-	{
+			Transform SensorData = Transform::getIdentity(),
-		Signature fromCopy(from);
+			RegistrationInfo * info = 0) const;
-		Signature toCopy(to);
-		return computeTransformationMod(fromCopy, toCopy, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
-	}
 
-	virtual Transform computeTransformationMod(
+	Transform computeTransformationMod(
 			Signature & from,
 			Signature & to,
 			Transform guess = Transform::getIdentity(),
-			std::string * rejectedMsg = 0,
+			RegistrationInfo * info = 0) const;
-			std::vector<int> * inliersOut = 0,
-			float * varianceOut = 0,
-			float * inliersRatioOut = 0) const = 0;
 
 protected:
-	Registration(const ParametersMap & parameters = ParametersMap()) :
+	// take ownership of child
-		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())
+	Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
-	{
-		this->parseParameters(parameters);
-	}
 
-protected:
+	// It is safe to modify the signatures in the implementation, if so, the
-	bool _varianceFromInliersCount;
+	// child registration will use these modifications.
+	virtual Transform computeTransformationImpl(
+			Signature & from,
+			Signature & to,
+			Transform guess,
+			RegistrationInfo & info) const = 0;
+
+	virtual bool isImageRequiredImpl() const = 0;
+	virtual bool isScanRequiredImpl() const = 0;
+	virtual bool isUserDataRequiredImpl() const = 0;
+
+private:
+	bool varianceFromInliersCount_;
+	bool force3DoF_;
+	Registration * child_;
 
 };
 

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -39,33 +39,25 @@ namespace rtabmap {
 class RTABMAP_EXP RegistrationIcp : public Registration
 {
 public:
-	RegistrationIcp(const ParametersMap & parameters = ParametersMap());
+	// take ownership of child
+	RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 	virtual ~RegistrationIcp() {}
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	virtual Transform computeTransformationMod(
+protected:
+	virtual Transform computeTransformationImpl(
 			Signature & from,
 			Signature & to,
-			Transform guess = Transform::getIdentity(),
+			Transform guess,
-			std::string * rejectedMsg = 0,
+			RegistrationInfo & info) const;
-			std::vector<int> * inliersOut = 0,
+	virtual bool isImageRequiredImpl() const {return false;}
-			float * varianceOut = 0,
+	virtual bool isScanRequiredImpl() const {return true;}
-			float * inliersRatioOut = 0) const;
+	virtual bool isUserDataRequiredImpl() const {return false;}
-
-	Transform computeTransformation(
-			const SensorData & from,
-			const SensorData & to,
-			Transform guess = Transform::getIdentity(),
-			std::string * rejectedMsg = 0,
-			std::vector<int> * inliersOut = 0,
-			float * varianceOut = 0,
-			float * inliersRatioOut = 0) const;
 
 private:
 	float _maxTranslation;
 	float _maxRotation;
-	bool _icp2D;
 	float _voxelSize;
 	int _downsamplingStep;
 	float _maxCorrespondenceDistance;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -0,0 +1,33 @@
+/*
+ * RegistrationInfo.h
+ *
+ *  Created on: Jan 5, 2016
+ *      Author: mathieu
+ */
+
+#ifndef REGISTRATIONINFO_H_
+#define REGISTRATIONINFO_H_
+
+
+namespace rtabmap {
+
+class RegistrationInfo
+{
+public:
+	RegistrationInfo() :
+		variance(0),
+		inliers(0),
+		inliersRatio(0)
+	{
+	}
+
+	float variance;
+	int inliers;
+	float inliersRatio;
+	std::vector<int> inliersIndexes_;
+	std::string rejectedMsg_;
+};
+
+}
+
+#endif /* REGISTRATIONINFO_H_ */

corelib/include/rtabmap/core/RegistrationVis.h

@@ -39,31 +39,32 @@ namespace rtabmap {
 class RTABMAP_EXP RegistrationVis : public Registration
 {
 public:
-	RegistrationVis(const ParametersMap & parameters = ParametersMap());
+	// take ownership of child
+	RegistrationVis(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
 	virtual ~RegistrationVis();
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
-	virtual Transform computeTransformationMod(
-			Signature & from,
-			Signature & to,
-			Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
-			std::string * rejectedMsg = 0,
-			std::vector<int> * inliersOut = 0,
-			float * varianceOut = 0,
-			float * inliersRatioOut = 0) const;
-
 	float getBowInlierDistance() const {return _inlierDistance;}
 	int getBowIterations() const {return _iterations;}
 	int getBowMinInliers() const {return _minInliers;}
-	bool getBowForce2D() const {return _force2D;}
+
+protected:
+	virtual Transform computeTransformationImpl(
+			Signature & from,
+			Signature & to,
+			Transform guess,
+			RegistrationInfo & info) const;
+
+	virtual bool isImageRequiredImpl() const {return true;}
+	virtual bool isScanRequiredImpl() const {return false;}
+	virtual bool isUserDataRequiredImpl() const {return false;}
 
 private:
 	int _minInliers;
 	float _inlierDistance;
 	int _iterations;
 	int _refineIterations;
-	bool _force2D;
 	float _epipolarGeometryVar;
 	int _estimationType;
 	bool _forwardEstimateOnly;

corelib/include/rtabmap/core/Rtabmap.h

@@ -184,7 +184,6 @@ private:
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
 	float _newMapOdomChangeDistance;
-	bool _loopClosureRefining;
 	bool _neighborLinkRefining;
 	bool _proximityByTime;
 	bool _proximityBySpace;

corelib/include/rtabmap/core/Transform.h

@@ -97,6 +97,7 @@ public:
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;
+	Transform to3DoF() const;
 
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
 	void getEulerAngles(float & roll, float & pitch, float & yaw) const;

corelib/src/CMakeLists.txt

@@ -50,6 +50,7 @@ SET(SRC_FILES
 	OptimizerGTSAM.cpp
 	OptimizerCVSBA.cpp
 	
+	Registration.cpp
 	RegistrationIcp.cpp
 	RegistrationVis.cpp
 	
@@ -57,7 +58,6 @@ SET(SRC_FILES
 	OdometryThread.cpp
 	OdometryLocalMap.cpp
 	OdometryMono.cpp
-	OdometryICP.cpp
 	OdometryF2F.cpp
 	
 	Stereo.cpp

corelib/src/Camera.cpp

@@ -102,8 +102,8 @@ SensorData Camera::takeImage(CameraInfo * info)
 	}
 	if(info)
 	{
-		info->id_ = data.id();
+		info->id = data.id();
-		info->timeCapture_ = captureTime;
+		info->timeCapture = captureTime;
 	}
 	return data;
 }

corelib/src/CameraThread.cpp

@@ -45,6 +45,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_mirroring(false),
 		_colorOnly(false),
 		_stereoToDepth(false),
+		_scanFromDepth(false),
+		_scanDecimation(4),
+		_scanMaxDepth(4.0f),
 		_stereoDense(new StereoBM(parameters))
 {
 	UASSERT(_camera != 0);
@@ -103,7 +106,7 @@ void CameraThread::mainLoop()
 				cv::flip(data.depthRaw(), tmpDepth, 1);
 				data.setDepthOrRightRaw(tmpDepth);
 			}
-			info.timeMirroring_ = timer.ticks();
+			info.timeMirroring = timer.ticks();
 		}
 		if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
 		{
@@ -115,10 +118,31 @@ void CameraThread::mainLoop()
 			data.setCameraModel(data.stereoCameraModel().left());
 			data.setDepthOrRightRaw(depth);
 			data.setStereoCameraModel(StereoCameraModel());
-			info.timeDisparity_ = timer.ticks();
+			info.timeDisparity = timer.ticks();
-			UINFO("Computing disparity = %f s", info.timeDisparity_);
+			UINFO("Computing disparity = %f s", info.timeDisparity);
 		}
-		info.cameraName_ = _camera->getSerial();
+		if(_scanFromDepth &&
+			data.cameraModels().size() &&
+			data.cameraModels().at(0).isValid() &&
+			!data.depthRaw().empty())
+		{
+			if(data.laserScanRaw().empty())
+			{
+				UASSERT(_scanDecimation >= 1);
+				UTimer timer;
+				cv::Mat scan = util3d::laserScanFromPointCloud(*util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth));
+				data.setLaserScanRaw(scan, (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation), _scanMaxDepth);
+				info.timeScanFromDepth = timer.ticks();
+				UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
+			}
+			else
+			{
+				UWARN("Option to create laser scan from depth image is enabled, but "
+					  "there is already a laser scan in the captured sensor data. Scan from "
+					  "depth will not be created.");
+			}
+		}
+		info.cameraName = _camera->getSerial();
 		this->post(new CameraEvent(data, info));
 	}
 	else if(!this->isKilled())

corelib/src/Memory.cpp

@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/VisualWord.h"
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/RegistrationIcp.h"
+#include "rtabmap/core/Registration.h"
 #include "rtabmap/core/RegistrationVis.h"
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/core/util3d_features.h"
@@ -102,7 +103,7 @@ Memory::Memory(const ParametersMap & parameters) :
 {
 	_feature2D = Feature2D::create(parameters);
 	_vwd = new VWDictionary(parameters);
-	_registrationVis = new RegistrationVis(parameters);
+	_registrationPipeline = Registration::create(parameters);
 	_registrationIcp = new RegistrationIcp(parameters);
 	this->parseParameters(parameters);
 }
@@ -365,9 +366,9 @@ Memory::~Memory()
 	{
 		delete _vwd;
 	}
-	if(_registrationVis)
+	if(_registrationPipeline)
 	{
-		delete _registrationVis;
+		delete _registrationPipeline;
 	}
 	if(_registrationIcp)
 	{
@@ -447,10 +448,27 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		_feature2D->parseParameters(parameters);
 	}
 
-	if(_registrationVis)
+	Registration::Type regStrategy = Registration::kTypeUndef;
+	if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
 	{
-		_registrationVis->parseParameters(parameters);
+		regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
 	}
+	if(regStrategy!=Registration::kTypeUndef)
+	{
+		UDEBUG("new registration strategy %d", int(regStrategy));
+		if(_registrationPipeline)
+		{
+			delete _registrationPipeline;
+			_registrationPipeline = 0;
+		}
+
+		_registrationPipeline = Registration::create(regStrategy, parameters_);
+	}
+	else if(_registrationPipeline)
+	{
+		_registrationPipeline->parseParameters(parameters);
+	}
+
 	if(_registrationIcp)
 	{
 		_registrationIcp->parseParameters(parameters);
@@ -2012,12 +2030,10 @@ void Memory::removeLink(int oldId, int newId)
 }
 
 // compute transform fromId -> toId
-Transform Memory::computeVisualTransform(
+Transform Memory::computeTransform(
 		int fromId,
 		int toId,
-		std::string * rejectedMsg,
+		RegistrationInfo * info)
-		int * inliers,
-		float * variance)
 {
 	const Signature * fromS = this->getSignature(fromId);
 	const Signature * toS = this->getSignature(toId);
@@ -2026,37 +2042,49 @@ Transform Memory::computeVisualTransform(
 
 	if(fromS && toS)
 	{
-		// compute transform fromId -> toId
+		// make sure we have all data needed
-		std::vector<int> inliersV;
+		if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
-		if(_reextractLoopClosureFeatures)
+		   _registrationPipeline->isScanRequired() ||
+		   _registrationPipeline->isUserDataRequired())
 		{
 			getNodeData(fromS->id(), true);
 			getNodeData(toS->id(), true);
+		}
 
+		// compute transform fromId -> toId
+		std::vector<int> inliersV;
+		if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size()))
+		{
 			Signature tmpFrom = *fromS;
 			Signature tmpTo = *toS;
 
-			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
+			if(_reextractLoopClosureFeatures)
-			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
+			{
-			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
+				tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
-			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
+				tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
-			transform = _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
+				tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
-		}
+				tmpTo.setWords3(std::multimap<int, cv::Point3f>());
-		else if(fromS->getWords().size() && toS->getWords().size())
+			}
-		{
+
-			transform = _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
+			Transform guess = Transform::getIdentity();
-		}
+			if(!_registrationPipeline->isImageRequired())
-		if(inliers)
+			{
-		{
+				// no visual in the pipeline, make visual registration for guess
-			*inliers = (int)inliersV.size();
+				RegistrationVis regVis(parameters_);
+				guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			}
+			if(!guess.isNull())
+			{
+				transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+			}
 		}
 	}
 	else
 	{
 		std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
-		if(rejectedMsg)
+		if(info)
 		{
-			*rejectedMsg = msg;
+			info->rejectedMsg_ = msg;
 		}
 		UWARN(msg.c_str());
 	}
@@ -2068,10 +2096,7 @@ Transform Memory::computeIcpTransform(
 		int fromId,
 		int toId,
 		Transform guess,
-		std::string * rejectedMsg,
+		RegistrationInfo * info)
-		int * inliers,
-		float * variance,
-		float * inliersRatio)
 {
 	Signature * fromS = this->_getSignature(fromId);
 	Signature * toS = this->_getSignature(toId);
@@ -2107,18 +2132,14 @@ Transform Memory::computeIcpTransform(
 
 		// compute transform fromId -> toId
 		std::vector<int> inliersV;
-		t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, rejectedMsg, &inliersV, variance, inliersRatio);
+		t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
-		if(inliers)
-		{
-			*inliers = (int)inliersV.size();
-		}
 	}
 	else
 	{
 		std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
-		if(rejectedMsg)
+		if(info)
 		{
-			*rejectedMsg = msg;
+			info->rejectedMsg_ = msg;
 		}
 		UWARN(msg.c_str());
 	}
@@ -2130,9 +2151,7 @@ Transform Memory::computeIcpTransformMulti(
 		int fromId,
 		int toId,
 		const std::map<int, Transform> & poses,
-		std::string * rejectedMsg,
+		RegistrationInfo * info)
-		int * inliers,
-		float * variance)
 {
 	UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
 	UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
@@ -2192,11 +2211,7 @@ Transform Memory::computeIcpTransformMulti(
 
 		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
 		std::vector<int> inliersV;
-		t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, rejectedMsg, &inliersV, variance);
+		t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);
-		if(inliers)
-		{
-			*inliers = (int)inliersV.size();
-		}
 	}
 
 	return t;

corelib/src/Odometry.cpp

@@ -70,7 +70,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_minDepth(Parameters::defaultVisMinDepth()),
 		_maxDepth(Parameters::defaultVisMaxDepth()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
-		_force2D(Parameters::defaultVisForce2D()),
+		_force2D(Parameters::defaultRegForce3DoF()),
 		_holonomic(Parameters::defaultOdomHolonomic()),
 		_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
 		_particleSize(Parameters::defaultOdomParticleSize()),
@@ -100,7 +100,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kVisMinDepth(), _minDepth);
 	Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
-	Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
+	Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force2D);
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
 	Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);

corelib/src/OdometryF2F.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/OdometryF2F.h"
 #include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/Registration.h"
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
@@ -37,15 +38,16 @@ OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
 	guessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
-	registration_(parameters),
 	motionSinceLastKeyFrame_(Transform::getIdentity())
 {
+	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomFlowGuessMotion(), guessFromMotion_);
 }
 
 OdometryF2F::~OdometryF2F()
 {
+	delete registrationPipeline_;
 }
 
 void OdometryF2F::reset(const Transform & initialPose)
@@ -74,20 +76,16 @@ Transform OdometryF2F::computeTransform(
 		return output;
 	}
 
-	float variance = 0;
+	RegistrationInfo regInfo;
-	std::vector<int> inliers;
 
 	Signature newFrame(data);
-	if(refFrame_.getWords().size())
+	if(refFrame_.sensorData().isValid())
 	{
-		std::string rejectedMsg;
+		output = registrationPipeline_->computeTransformationMod(
-		output = registration_.computeTransformationMod(
 				refFrame_,
 				newFrame,
 				guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
-				&rejectedMsg,
+				&regInfo);
-				&inliers,
-				&variance);
 
 		if(info && this->isInfoDataFilled())
 		{
@@ -106,11 +104,11 @@ Transform OdometryF2F::computeTransform(
 				idToIndex.insert(std::make_pair(iter->first, i));
 				++i;
 			}
-			info->cornerInliers.resize(inliers.size(), 1);
+			info->cornerInliers.resize(regInfo.inliersIndexes_.size(), 1);
 			i=0;
-			for(; i<(int)inliers.size(); ++i)
+			for(; i<(int)regInfo.inliersIndexes_.size(); ++i)
 			{
-				info->cornerInliers[i] = idToIndex.at(inliers[i]);
+				info->cornerInliers[i] = idToIndex.at(regInfo.inliersIndexes_[i]);
 			}
 
 		}
@@ -127,17 +125,24 @@ Transform OdometryF2F::computeTransform(
 		motionSinceLastKeyFrame_ *= output;
 
 		// new key-frame?
-		if(keyFrameThr_ <= 0 || (int)inliers.size() <= keyFrameThr_)
+		if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_)
 		{
 			UDEBUG("Update key frame");
-			// only generate features for the first frame
 			Signature newRefFrame(data);
-			Signature dummy;
-			registration_.computeTransformationMod(
-					newRefFrame,
-					dummy);
 
-			if((int)newRefFrame.getWords().size() >= this->getMinInliers())
+			int features = -1;
+			if(registrationPipeline_->isImageRequired())
+			{
+				// this will generate features only for the first frame
+				Signature dummy;
+				registrationPipeline_->computeTransformationMod(
+						newRefFrame,
+						dummy);
+				features = (int)newRefFrame.getWords().size();
+			}
+
+			if((features < 0 || features >= this->getMinInliers()) &&
+			   (!registrationPipeline_->isScanRequired() || newRefFrame.sensorData().laserScanRaw().cols))
 			{
 				refFrame_ = newRefFrame;
 
@@ -146,23 +151,33 @@ Transform OdometryF2F::computeTransform(
 			}
 			else
 			{
-				UWARN("Too low 2D corners (%d), keeping last key frame...",
+				if(features >= 0 && features < this->getMinInliers())
-						(int)newRefFrame.getWords().size());
+				{
+					UWARN("Too low 2D features (%d), keeping last key frame...", features);
+				}
+				if(registrationPipeline_->isScanRequired() && newRefFrame.sensorData().laserScanRaw().cols==0)
+				{
+					UWARN("Too low scan points (%d), keeping last key frame...", newRefFrame.sensorData().laserScanRaw().cols);
+				}
 			}
 		}
 	}
+	else if(!regInfo.rejectedMsg_.empty())
+	{
+		UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg_.c_str());
+	}
 
 	if(info)
 	{
 		info->type = 1;
-		info->variance = variance;
+		info->variance = regInfo.variance;
-		info->inliers = (int)inliers.size();
+		info->inliers = regInfo.inliers;
 	}
 
 	UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
 			timer.elapsed(),
 			output.isNull()?"true":"false",
-			(int)inliers.size(),
+			(int)regInfo.inliers,
 			(int)refFrame_.getWords().size(),
 			!output.isNull()?"true":"false");
 

corelib/src/OdometryICP.cpp

@@ -1,216 +0,0 @@
-/*
-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/OdometryICP.h"
-#include "rtabmap/core/util3d_registration.h"
-#include "rtabmap/core/util3d_filtering.h"
-#include "rtabmap/core/util3d_surface.h"
-#include "rtabmap/core/OdometryInfo.h"
-#include "rtabmap/utilite/ULogger.h"
-#include "rtabmap/utilite/UTimer.h"
-
-namespace rtabmap {
-
-OdometryICP::OdometryICP(int decimation,
-		float voxelSize,
-		int samples,
-		float maxCorrespondenceDistance,
-		int maxIterations,
-		float correspondenceRatio,
-		bool pointToPlane,
-		const ParametersMap & odometryParameter) :
-	Odometry(odometryParameter),
-	_decimation(decimation),
-	_voxelSize(voxelSize),
-	_samples(samples),
-	_maxCorrespondenceDistance(maxCorrespondenceDistance),
-	_maxIterations(maxIterations),
-	_correspondenceRatio(correspondenceRatio),
-	_pointToPlane(pointToPlane),
-	_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
-	_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
-{
-}
-
-void OdometryICP::reset(const Transform & initialPose)
-{
-	Odometry::reset(initialPose);
-	_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
-	_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
-}
-
-// return not null transform if odometry is correctly computed
-Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
-{
-	UTimer timer;
-	Transform output;
-
-	bool hasConverged = false;
-	double variance = 0;
-	unsigned int minPoints = 100;
-	int correspondences = 0;
-	if(!data.depthOrRightRaw().empty())
-	{
-		if(data.depthOrRightRaw().type() == CV_8UC1)
-		{
-			UERROR("ICP 3D cannot be done on stereo images!");
-			return output;
-		}
-
-		if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()))
-		{
-			UERROR("ICP 3D cannot be done without calibration or on multi-camera!");
-			return output;
-		}
-		const CameraModel & cameraModel = data.cameraModels()[0];
-
-		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
-						data.depthOrRightRaw(),
-						cameraModel.fx(),
-						cameraModel.fy(),
-						cameraModel.cx(),
-						cameraModel.cy(),
-						_decimation,
-						this->getMaxDepth(),
-						_voxelSize,
-						_samples,
-						cameraModel.localTransform());
-
-		if(_pointToPlane)
-		{
-			pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
-
-			std::vector<int> indices;
-			newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
-			if(newCloudXYZ->size() != newCloud->size())
-			{
-				UWARN("removed nan normals...");
-			}
-
-			if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
-			{
-				pcl::PointCloud<pcl::PointNormal>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointNormal>);
-				Transform transform = util3d::icpPointToPlane(
-						newCloud,
-						_previousCloudNormal,
-						_maxCorrespondenceDistance,
-						_maxIterations,
-						hasConverged,
-						*newCloudRegistered);
-
-				util3d::computeVarianceAndCorrespondences(
-						newCloudRegistered,
-						_previousCloudNormal,
-						_maxCorrespondenceDistance,
-						variance,
-						correspondences);
-
-				// verify if there are enough correspondences
-				float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
-
-				if(!transform.isNull() && hasConverged &&
-		   	   	   correspondencesRatio >= _correspondenceRatio)
-				{
-					output = transform;
-					_previousCloudNormal = newCloud;
-				}
-				else
-				{
-					UWARN("Transform not valid (hasConverged=%s variance = %f)",
-							hasConverged?"true":"false", variance);
-				}
-			}
-			else if(newCloud->size() > minPoints)
-			{
-				output.setIdentity();
-				_previousCloudNormal = newCloud;
-			}
-		}
-		else
-		{
-			//point to point
-			if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
-			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
-				Transform transform = util3d::icp(
-						newCloudXYZ,
-						_previousCloud,
-						_maxCorrespondenceDistance,
-						_maxIterations,
-						hasConverged,
-						*newCloudRegistered);
-
-				util3d::computeVarianceAndCorrespondences(
-						newCloudRegistered,
-						_previousCloud,
-						_maxCorrespondenceDistance,
-						variance,
-						correspondences);
-
-				// verify if there are enough correspondences
-				float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
-
-				if(!transform.isNull() && hasConverged &&
-				   correspondencesRatio >= _correspondenceRatio)
-				{
-					output = transform;
-					_previousCloud = newCloudXYZ;
-				}
-				else
-				{
-					UWARN("Transform not valid (hasConverged=%s variance = %f)",
-							hasConverged?"true":"false", variance);
-				}
-			}
-			else if(newCloudXYZ->size() > minPoints)
-			{
-				output.setIdentity();
-				_previousCloud = newCloudXYZ;
-			}
-		}
-	}
-	else
-	{
-		UERROR("Depth is empty?!?");
-	}
-
-	if(info)
-	{
-		info->variance = variance;
-		info->inliers = correspondences;
-	}
-
-	UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
-			timer.elapsed(),
-			hasConverged?"true":"false",
-			variance,
-			(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
-
-	return output;
-}
-
-} // namespace rtabmap

corelib/src/Parameters.cpp

@@ -158,7 +158,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		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/Force2D",                  std::make_pair(true,  Parameters::kRegForce3DoF())));
 		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())));
@@ -188,13 +188,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		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/Force2D",                  std::make_pair(false,  Parameters::kRegForce3DoF())));
 		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::kRGBDLoopClosureLinkRefining())));
+		removedParameters_.insert(std::make_pair("LccIcp/Type",                         std::make_pair(false,  Parameters::kRegStrategy())));
 
 		removedParameters_.insert(std::make_pair("LccIcp3/Decimation",                  std::make_pair(false, "")));
 		removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth",                    std::make_pair(false, "")));

corelib/src/Registration.cpp

@@ -0,0 +1,190 @@
+/*
+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/RegistrationIcp.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+Registration * Registration::create(const ParametersMap & parameters)
+{
+	int regTypeInt = Parameters::defaultRegStrategy();
+	Parameters::parse(parameters, Parameters::kRegStrategy(), regTypeInt);
+	Registration::Type type = (Registration::Type)regTypeInt;
+	return create(type, parameters);
+}
+
+Registration * Registration::create(Registration::Type & type, const ParametersMap & parameters)
+{
+	UDEBUG("type=%d", (int)type);
+	Registration * reg = 0;
+	switch(type)
+	{
+	case Registration::kTypeIcp:
+		reg = new RegistrationIcp(parameters);
+		break;
+	case Registration::kTypeVisIcp:
+		reg = new RegistrationVis(parameters, new RegistrationIcp(parameters));
+		break;
+	default: // kTypeVis
+		reg = new RegistrationVis(parameters);
+		type = Registration::kTypeVis;
+		break;
+	}
+	return reg;
+}
+
+Registration::Registration(const ParametersMap & parameters, Registration * child) :
+	varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
+	force3DoF_(Parameters::defaultRegForce3DoF()),
+	child_(child)
+{
+	this->parseParameters(parameters);
+}
+
+Registration::~Registration()
+{
+	if(child_)
+	{
+		delete child_;
+	}
+}
+void Registration::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
+	Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
+	if(child_)
+	{
+		child_->parseParameters(parameters);
+	}
+}
+
+bool Registration::isImageRequired() const
+{
+	bool val = isImageRequiredImpl();
+	if(!val && child_)
+	{
+		val = child_->isImageRequired();
+	}
+	return val;
+}
+
+bool Registration::isScanRequired() const
+{
+	bool val = isScanRequiredImpl();
+	if(!val && child_)
+	{
+		val = child_->isScanRequired();
+	}
+	return val;
+}
+
+bool Registration::isUserDataRequired() const
+{
+	bool val = isUserDataRequiredImpl();
+	if(!val && child_)
+	{
+		val = child_->isUserDataRequired();
+	}
+	return val;
+}
+
+void Registration::setChildRegistration(Registration * child)
+{
+	if(child_)
+	{
+		delete child_;
+	}
+	child_ = child;
+}
+
+Transform Registration::computeTransformation(
+		const Signature & from,
+		const Signature & to,
+		Transform guess,
+		RegistrationInfo * infoOut) const
+{
+	Signature fromCopy(from);
+	Signature toCopy(to);
+	return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
+}
+
+Transform Registration::computeTransformation(
+		const SensorData & from,
+		const SensorData & to,
+		Transform guess,
+		RegistrationInfo * infoOut) const
+{
+	Signature fromCopy(from);
+	Signature toCopy(to);
+	return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
+}
+
+Transform Registration::computeTransformationMod(
+		Signature & from,
+		Signature & to,
+		Transform guess,
+		RegistrationInfo * infoOut) const
+{
+	RegistrationInfo info;
+	Transform t = computeTransformationImpl(from, to, guess, info);
+	if(child_)
+	{
+		if(!t.isNull())
+		{
+			t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
+		}
+	}
+	else if(!t.isNull() && force3DoF_)
+	{
+		t = t.to3DoF();
+	}
+
+	if(varianceFromInliersCount_)
+	{
+		if(info.inliersRatio)
+		{
+			info.variance = info.inliersRatio > 0?1.0/double(info.inliersRatio):1.0;
+		}
+		else
+		{
+			info.variance = info.inliers > 0?1.0f/float(info.inliers):1.0f;
+		}
+		info.variance = info.variance>0.0f?info.variance:0.0001f; // epsilon if exact transform
+	}
+
+	if(infoOut)
+	{
+		*infoOut = info;
+	}
+
+	return t;
+}
+
+}

corelib/src/RegistrationIcp.cpp

@@ -39,10 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) :
+RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
+	Registration(parameters, child),
 	_maxTranslation(Parameters::defaultIcpMaxTranslation()),
 	_maxRotation(Parameters::defaultIcpMaxRotation()),
-	_icp2D(Parameters::defaultIcp2D()),
 	_voxelSize(Parameters::defaultIcpVoxelSize()),
 	_downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
 	_maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
@@ -60,7 +60,6 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 
 	Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
 	Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
-	Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D);
 	Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
 	Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
 	Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
@@ -77,42 +76,18 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
 }
 
-Transform RegistrationIcp::computeTransformationMod(
+Transform RegistrationIcp::computeTransformationImpl(
 			Signature & fromSignature,
 			Signature & toSignature,
 			Transform guess,
-			std::string * rejectedMsg,
+			RegistrationInfo & info) const
-			std::vector<int> * inliersOut,
-			float * varianceOut,
-			float * inliersRatioOut) const
-{
-	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,
-			std::vector<int> * inliersOut,
-			float * varianceOut,
-			float * inliersRatioOut) const
 {
 	UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
 	UDEBUG("Voxel size=%f", _voxelSize);
-	UDEBUG("2D=%d", _icp2D?1:0);
 	UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
 	UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
 	UDEBUG("Max corrrespondence distance=%f", _maxCorrespondenceDistance);
 	UDEBUG("Max Iterations=%d", _maxIterations);
-	UDEBUG("Variance from inliers count=%d", _varianceFromInliersCount?1:0);
 	UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
 	UDEBUG("Max translation=%f", _maxTranslation);
 	UDEBUG("Max rotation=%f", _maxRotation);
@@ -121,6 +96,9 @@ Transform RegistrationIcp::computeTransformation(
 	std::string msg;
 	Transform transform;
 
+	SensorData & dataFrom = fromSignature.sensorData();
+	SensorData & dataTo = toSignature.sensorData();
+
 	// ICP with guess transform
 	if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
 	{
@@ -157,7 +135,7 @@ Transform RegistrationIcp::computeTransformation(
 			double variance = 1.0;
 			bool correspondencesComputed = false;
 			pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-			if(!_icp2D) // 3D ICP
+			if(!force3DoF()) // 3D ICP
 			{
 				if(_pointToPlane)
 				{
@@ -287,23 +265,9 @@ Transform RegistrationIcp::computeTransformation(
 							maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
 							correspondencesRatio*100.0f);
 
-					if(_varianceFromInliersCount)
+					info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
-					{
+					info.inliers = correspondences;
-						variance = correspondencesRatio > 0?1.0/double(correspondencesRatio):1.0;
+					info.inliersRatio = correspondencesRatio;
-					}
-
-					if(varianceOut)
-					{
-						*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
-					}
-					if(inliersOut)
-					{
-						inliersOut->push_back(correspondences);
-					}
-					if(inliersRatioOut)
-					{
-						*inliersRatioOut = correspondencesRatio;
-					}
 
 					if(correspondencesRatio < _correspondenceRatio)
 					{
@@ -354,10 +318,7 @@ Transform RegistrationIcp::computeTransformation(
 	}
 
 
-	if(rejectedMsg)
+	info.rejectedMsg_ = msg;
-	{
-		*rejectedMsg = msg;
-	}
 
 	UDEBUG("New transform = %s", transform.prettyPrint().c_str());
 	return transform;

corelib/src/RegistrationVis.cpp

@@ -41,12 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
+RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
+		Registration(parameters, child),
 		_minInliers(Parameters::defaultVisMinInliers()),
 		_inlierDistance(Parameters::defaultVisInlierDistance()),
 		_iterations(Parameters::defaultVisIterations()),
 		_refineIterations(Parameters::defaultVisRefineIterations()),
-		_force2D(Parameters::defaultVisForce2D()),
 		_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
 		_estimationType(Parameters::defaultVisEstimationType()),
 		_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
@@ -82,7 +82,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
 	Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
 	Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
-	Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
 	Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
 	Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
@@ -154,19 +153,15 @@ RegistrationVis::~RegistrationVis()
 {
 }
 
-Transform RegistrationVis::computeTransformationMod(
+Transform RegistrationVis::computeTransformationImpl(
 			Signature & fromSignature,
 			Signature & toSignature,
 			Transform guess, // guess is only used by Optical Flow correspondences (flowMaxLevel is set to 0 when guess is used)
-			std::string * rejectedMsg,
+			RegistrationInfo & info) const
-			std::vector<int> * inliersOut,
-			float * varianceOut,
-			float * inliersRatioOut) const
 {
 	UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
 	UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
 	UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
-	UDEBUG("%s=%d", Parameters::kVisForce2D().c_str(), _force2D?1:0);
 	UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
 	UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
 	UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
@@ -337,7 +332,7 @@ Transform RegistrationVis::computeTransformationMod(
 				kptsFrom3DKept.resize(ki);
 
 				std::vector<cv::Point3f> kptsTo3D;
-				if(_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly)
+				if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
 				{
 					kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
 				}
@@ -433,7 +428,7 @@ Transform RegistrationVis::computeTransformationMod(
 			{
 				kptsFrom3D = uValues(fromSignature.getWords3());
 			}
-			if((_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly) &&
+			if((_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly) &&
 			   toSignature.getWords3().empty() &&
 			   !toSignature.sensorData().imageRaw().empty())
 			{
@@ -503,6 +498,7 @@ Transform RegistrationVis::computeTransformationMod(
 	/////////////////////
 	Transform transform;
 	float variance = 1.0f;
+	int inliersCount = 0;
 	if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
 	{
 		Transform transforms[2];
@@ -629,7 +625,7 @@ Transform RegistrationVis::computeTransformationMod(
 								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 								uMultimapToMapUnique(signatureA->getWords3()),
-								_varianceFromInliersCount?0:&variances[dir],
+								varianceFromInliersCount()?0:&variances[dir],
 								0,
 								&inliersV);
 						inliers[dir] = inliersV;
@@ -696,68 +692,27 @@ Transform RegistrationVis::computeTransformationMod(
 			if(transforms[0].isNull())
 			{
 				transform = transforms[1];
-				if(inliersOut)
+				info.inliersIndexes_ = inliers[1];
-				{
-					*inliersOut = inliers[1];
-				}
 
 				variance = variances[1];
-				if(_varianceFromInliersCount)
+				inliersCount = (int)inliers[1].size();
-				{
-					variance = inliers[1].size() > 0?1.0f/float(inliers[1].size()):1.0f;
-				}
 			}
 			else
 			{
-				/*if(!guess.isNull())
+				transform = transforms[0].interpolate(0.5f, transforms[1]);
-				{
+				info.inliersIndexes_ = inliers[0];
-					// use the transform nearest of the guess
-					int index = 0;
-					if(transforms[0].getDistance(guess) > transforms[1].getDistance(guess))
-					{
-						index = 1;
-					}
-					transform = transforms[index];
-					if(inliersOut)
-					{
-						*inliersOut = inliers[index];
-					}
 
-					variance = variances[index];
+				variance = (variances[0]+variances[1])/2.0f;
-					if(_varianceFromInliersCount)
+				inliersCount = (int)(inliers[0].size()+inliers[1].size())/2;
-					{
-						variance = inliers[index].size() > 0?1.0f/float(inliers[index].size()):1.0f;
-					}
-				}
-				else*/
-				{
-					transform = transforms[0].interpolate(0.5f, transforms[1]);
-					if(inliersOut)
-					{
-						*inliersOut = inliers[0];
-					}
-					variance = (variances[0]+variances[1])/2.0f;
-					if(_varianceFromInliersCount)
-					{
-						int avg = (inliers[0].size()+inliers[1].size())/2;
-						variance = avg>0?1.0f/float(avg):1.0f;
-					}
-				}
 			}
 		}
 		else
 		{
 			transform = transforms[0];
-			if(inliersOut)
+			info.inliersIndexes_ = inliers[0];
-			{
-				*inliersOut = inliers[0];
-			}
 
 			variance = variances[0];
-			if(_varianceFromInliersCount)
+			inliersCount = (int)inliers[0].size();
-			{
-				variance = inliers[0].size() > 0?1.0f/float(inliers[0].size()):1.0f;
-			}
 		}
 	}
 
@@ -776,21 +731,12 @@ Transform RegistrationVis::computeTransformationMod(
 					roll, pitch, yaw);
 			UWARN(msg.c_str());
 		}
-		else if(_force2D)
-		{
-			UDEBUG("Forcing 2D...");
-			transform = Transform(x,y,0, 0, 0, yaw);
-		}
 	}
 
-	if(rejectedMsg)
+	info.inliers = inliersCount;
-	{
+	info.rejectedMsg_ = msg;
-		*rejectedMsg = msg;
+	info.variance = variance>0.0f?variance:0.0001f; // epsilon if exact transform
-	}
+
-	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

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/core/BayesFilter.h"
 #include "rtabmap/core/Compression.h"
+#include "rtabmap/core/RegistrationInfo.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
@@ -89,7 +90,6 @@ Rtabmap::Rtabmap() :
 	_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
 	_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
 	_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
-	_loopClosureRefining(Parameters::defaultRGBDLoopClosureLinkRefining()),
 	_neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
 	_proximityByTime(Parameters::defaultRGBDProximityByTime()),
 	_proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
@@ -411,7 +411,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
 	Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
 	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
-	Parameters::parse(parameters, Parameters::kRGBDLoopClosureLinkRefining(), _loopClosureRefining);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1009,21 +1008,18 @@ bool Rtabmap::process(
 				{
 					UINFO("Odometry correction by scan matching");
 					Transform guess = signature->getLinks().begin()->second.transform().inverse();
-					float variance = 1.0f;
+					RegistrationInfo info;
-					int inliers = 0;
+					Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
-					float inliersRatio = 0;
-					std::string rejectedMsg;
-					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",
 								signature->id(),
 								oldId,
-								variance,
+								info.variance,
 								signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
 								t.prettyPrint().c_str());
-						UASSERT(variance > 0.0);
+						UASSERT(info.variance > 0.0);
-						_memory->updateLink(oldId, signature->id(), t, variance, variance);
+						_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
 
 						if(_optimizeFromGraphEnd)
 						{
@@ -1043,17 +1039,17 @@ bool Rtabmap::process(
 					}
 					else
 					{
-						UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
+						UINFO("Scan matching rejected: %s", info.rejectedMsg_.c_str());
-						if(variance > 0)
+						if(info.variance > 0)
 						{
-							double sqrtVar = sqrt(variance);
+							double sqrtVar = sqrt(info.variance);
 							_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
 						}
 					}
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), inliers);
+					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), inliersRatio);
+					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.inliersRatio);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), variance);
+					statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), info.variance);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
 				}
 			}
@@ -1161,13 +1157,9 @@ bool Rtabmap::process(
 				{
 					std::string rejectedMsg;
 					UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
-					float variance = 1.0f;
+					RegistrationInfo info;
-					int inliers = -1;
+					Transform transform = _memory->computeTransform(signature->id(), *iter, &info);
-					Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance);
+
-					if(!transform.isNull() && _loopClosureRefining)
-					{
-						transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance);
-					}
 					if(!transform.isNull())
 					{
 						UDEBUG("Add local loop closure in TIME (%d->%d) %s",
@@ -1175,8 +1167,8 @@ bool Rtabmap::process(
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						UASSERT(variance > 0.0);
+						UASSERT(info.variance > 0.0);
-						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
+						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.variance, info.variance)))
 						{
 							++localLoopClosuresInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1718,19 +1710,14 @@ bool Rtabmap::process(
 		float variance = 1.0f;
 		if(_rgbdSlamMode)
 		{
-			std::string rejectedMsg;
+			RegistrationInfo info;
-
+			transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info);
-			transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance);
+			loopClosureVisualInliers = info.inliers;
-
-			if(!transform.isNull() && _loopClosureRefining)
-			{
-				transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance);
-			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
 				UWARN("Rejected loop closure %d -> %d: %s",
-						_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
+						_loopClosureHypothesis.first, signature->id(), info.rejectedMsg_.c_str());
 			}
 		}
 		if(!rejectedHypothesis)
@@ -1822,12 +1809,8 @@ bool Rtabmap::process(
 						(_proximityFilteringRadius <= 0.0f ||
 						 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
 					{
-						float variance = 1.0f;
+						RegistrationInfo info;
-						Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance);
+						Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
-						if(!transform.isNull() && _loopClosureRefining)
-						{
-							transform  = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance);
-						}
 						if(!transform.isNull())
 						{
 							if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1836,8 +1819,8 @@ bool Rtabmap::process(
 										signature->id(),
 										nearestId,
 										transform.prettyPrint().c_str());
-								UASSERT(variance > 0.0);
+								UASSERT(info.variance > 0.0);
-								_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
+								_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
 								loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
 								if(_loopClosureHypothesis.first == 0)
@@ -1927,8 +1910,8 @@ bool Rtabmap::process(
 								//The nearest will be the reference for a loop closure transform
 								if(signature->getLinks().find(nearestId) == signature->getLinks().end())
 								{
-									float variance = 1.0f;
+									RegistrationInfo info;
-									Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, 0, 0, &variance);
+									Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, &info);
 									if(!transform.isNull())
 									{
 										if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1960,8 +1943,8 @@ bool Rtabmap::process(
 											}
 
 											// set Identify covariance for laser scan matching only
-											UASSERT(variance>0.0);
+											UASSERT(info.variance>0.0);
-											double sqrtVar = sqrt(variance);
+											double sqrtVar = sqrt(info.variance);
 											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
 											loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 

corelib/src/Transform.cpp

@@ -173,6 +173,13 @@ Transform Transform::translation() const
 					 0,0,1, data()[11]);
 }
 
+Transform Transform::to3DoF() const
+{
+	float x,y,z,roll,pitch,yaw;
+	this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+	return Transform(x,y,0, 0,0,yaw);
+}
+
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
 {
 	pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);

guilib/include/rtabmap/gui/CameraViewer.h

@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 
 class QSpinBox;
+class QCheckBox;
+class QPushButton;
 
 namespace rtabmap {
 
@@ -59,6 +61,9 @@ private:
 	bool processingImages_;
 	QSpinBox * decimationSpin_;
 	int validDecimationValue_;
+	QPushButton * pause_;
+	QCheckBox * showCloudCheckbox_;
+	QCheckBox * showScanCheckbox_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -193,9 +193,11 @@ public:
 	bool getSourceDatabaseStampsUsed() const;//Database group
 	bool isSourceRGBDColorOnly() const;
 	bool isSourceStereoDepthGenerated() const;
+	bool isSourceScanFromDepth() const;
+	int getSourceScanFromDepthDecimation() const;
+	double getSourceScanFromDepthMaxDepth() const;
 	Transform getSourceLocalTransform() const;    //Openni group
-	Transform getStereoLaserLocalTransform() const; // stereo images
+	Transform getLaserLocalTransform() const; // directory images
-	Transform getRGBDLaserLocalTransform() const; // rgbd images
 	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
@@ -259,25 +261,19 @@ private slots:
 	void openDatabaseViewer();
 	void selectSourceDatabase();
 	void selectCalibrationPath();
-	void selectSourceRGBDImagesStamps();
+	void selectSourceImagesStamps();
 	void selectSourceRGBDImagesPathRGB();
 	void selectSourceRGBDImagesPathDepth();
-	void selectSourceRGBDImagesPathScans();
+	void selectSourceImagesPathScans();
-	void selectSourceRGBDImagesPathGt();
+	void selectSourceImagesPathGt();
-	void selectSourceStereoImagesStamps();
 	void selectSourceStereoImagesPathLeft();
 	void selectSourceStereoImagesPathRight();
-	void selectSourceStereoImagesPathScans();
-	void selectSourceStereoImagesPathGt();
 	void selectSourceImagesPath();
 	void selectSourceVideoPath();
 	void selectSourceStereoVideoPath();
 	void selectSourceOniPath();
 	void selectSourceOni2Path();
 	void updateSourceGrpVisibility();
-	void updateRGBDCameraGroupBoxVisibility();
-	void updateRGBCameraGroupBoxVisibility();
-	void updateStereoCameraGroupBoxVisibility();
 	void testOdometry();
 	void testCamera();
 

guilib/src/CameraViewer.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraEvent.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util2d.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/gui/ImageView.h>
 #include <rtabmap/gui/CloudViewer.h>
 #include <rtabmap/utilite/UCv2Qt.h>
@@ -40,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QLabel>
 #include <QSpinBox>
 #include <QDialogButtonBox>
+#include <QCheckBox>
+#include <QPushButton>
 
 namespace rtabmap {
 
@@ -66,13 +69,24 @@ CameraViewer::CameraViewer(QWidget * parent) :
 	decimationSpin_->setMaximum(16);
 	decimationSpin_->setValue(1);
 
+	pause_ = new QPushButton("Pause", this);
+	pause_->setCheckable(true);
+	showCloudCheckbox_ = new QCheckBox("Show RGB-D cloud", this);
+	showCloudCheckbox_->setEnabled(false);
+	showCloudCheckbox_->setChecked(true);
+	showScanCheckbox_ = new QCheckBox("Show scan", this);
+	showScanCheckbox_->setEnabled(false);
+
 	QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
 	buttonBox->setStandardButtons(QDialogButtonBox::Close);
 	connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
 
 	QHBoxLayout * layout2 = new QHBoxLayout();
+	layout2->addWidget(pause_);
 	layout2->addWidget(decimationLabel);
 	layout2->addWidget(decimationSpin_);
+	layout2->addWidget(showCloudCheckbox_);
+	layout2->addWidget(showScanCheckbox_);
 	layout2->addStretch(1);
 	layout2->addWidget(buttonBox);
 
@@ -123,42 +137,68 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 	{
 		imageView_->setImageDepth(uCvMat2QImage(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_)));
 	}
-	if((data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid())))
+
+	if(!data.depthOrRightRaw().empty() &&
+	   (data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid())))
 	{
-		if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
+		if(showCloudCheckbox_->isChecked())
 		{
-			cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_));
+			if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
-			cloudView_->setVisible(true);
+			{
-			cloudView_->update();
+				showCloudCheckbox_->setEnabled(true);
-		}
+				cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_));
-		else if(!data.depthOrRightRaw().empty())
+			}
-		{
+			else if(!data.depthOrRightRaw().empty())
-			cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_));
+			{
-			cloudView_->setVisible(true);
+				showCloudCheckbox_->setEnabled(true);
-			cloudView_->update();
+				cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_));
+			}
 		}
 	}
-	else
+
+	if(!data.laserScanRaw().empty())
 	{
-		cloudView_->setVisible(false);
+		showScanCheckbox_->setEnabled(true);
+		if(showScanCheckbox_->isChecked())
+		{
+			cloudView_->addOrUpdateCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), validDecimationValue_), Transform::getIdentity(), Qt::yellow);
+		}
 	}
+
+	cloudView_->setVisible(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled());
+	if(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled())
+	{
+		cloudView_->update();
+	}
+	if(cloudView_->getAddedClouds().contains("cloud"))
+	{
+		cloudView_->setCloudVisibility("cloud", showCloudCheckbox_->isChecked());
+	}
+	if(cloudView_->getAddedClouds().contains("scan"))
+	{
+		cloudView_->setCloudVisibility("scan", showScanCheckbox_->isChecked());
+	}
+
 	processingImages_ = false;
 }
 
 void CameraViewer::handleEvent(UEvent * event)
 {
-	if(event->getClassName().compare("CameraEvent") == 0)
+	if(!pause_->isChecked())
 	{
-		CameraEvent * camEvent = (CameraEvent*)event;
+		if(event->getClassName().compare("CameraEvent") == 0)
-		if(camEvent->getCode() == CameraEvent::kCodeData)
 		{
-			if(camEvent->data().isValid())
+			CameraEvent * camEvent = (CameraEvent*)event;
+			if(camEvent->getCode() == CameraEvent::kCodeData)
 			{
-				if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+				if(camEvent->data().isValid())
 				{
-					processingImages_ = true;
+					if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
-					QMetaObject::invokeMethod(this, "showImage",
+					{
-							Q_ARG(rtabmap::SensorData, camEvent->data()));
+						processingImages_ = true;
+						QMetaObject::invokeMethod(this, "showImage",
+								Q_ARG(rtabmap::SensorData, camEvent->data()));
+					}
 				}
 			}
 		}

guilib/src/DatabaseViewer.cpp

@@ -3544,9 +3544,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 		}
 	}
 
-
-	float variance = -1.0f;
 	Transform transform;
+	RegistrationInfo info;
 
 	SensorData dataFrom, dataTo;
 	dbDriver_->getNodeData(currentLink.from(), dataFrom);
@@ -3582,12 +3581,12 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	UINFO("Uncompress time: %f s", timer.ticks());
 
 	RegistrationIcp registration(parameters);
-	transform = registration.computeTransformation(dataFrom, dataTo, t, 0, 0, &variance);
+	transform = registration.computeTransformation(dataFrom, dataTo, t, &info);
 	UINFO("Icp time: %f s", timer.ticks());
 
 	if(!transform.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, variance, variance);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.variance, info.variance);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -3652,9 +3651,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 
 	Transform t;
-	std::string rejectedMsg;
+	RegistrationInfo info;
-	float variance = -1.0f;
-	std::vector<int> inliers;
 
 	// Add sensor data to generate features
 	SensorData dataFrom;
@@ -3669,7 +3666,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	RegistrationVis reg(parameters);
 	Signature fromS(dataFrom);
 	Signature toS(dataTo);
-	t = reg.computeTransformationMod(fromS, toS, currentLink.transform(), &rejectedMsg, &inliers, &variance);
+	t = reg.computeTransformationMod(fromS, toS, currentLink.transform(), &info);
 	UDEBUG("");
 
 	if(!silent)
@@ -3681,7 +3678,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 
 	if(!t.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance, variance);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, info.variance, info.variance);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -3714,7 +3711,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	{
 		QMessageBox::warning(this,
 				tr("Add link"),
-				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
+				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg_.c_str()));
 	}
 }
 
@@ -3743,9 +3740,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 
 		Transform t;
-		std::string rejectedMsg;
+		RegistrationInfo info;
-		float variance = -1.0f;
-		std::vector<int> inliers;
 
 		// Add sensor data to generate features
 		SensorData dataFrom;
@@ -3760,7 +3755,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		RegistrationVis reg(parameters);
 		Signature fromS(dataFrom);
 		Signature toS(dataTo);
-		t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &rejectedMsg, &inliers, &variance);
+		t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &info);
 		UDEBUG("");
 
 		if(!silent)
@@ -3775,11 +3770,11 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			// transform is valid, make a link
 			if(from>to)
 			{
-				linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance, variance)));
+				linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
 			}
 			else
 			{
-				linksAdded_.insert(std::make_pair(to, Link(to, from, Link::kUserClosure, t.inverse(), variance, variance)));
+				linksAdded_.insert(std::make_pair(to, Link(to, from, Link::kUserClosure, t.inverse(), info.variance, info.variance)));
 			}
 			updateSlider = true;
 		}
@@ -3787,7 +3782,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		{
 			QMessageBox::warning(this,
 					tr("Add link"),
-					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
+					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg_.c_str()));
 		}
 	}
 	else if(containsLink(linksRemoved_, from, to))

guilib/src/MainWindow.cpp

@@ -735,9 +735,10 @@ void MainWindow::handleEvent(UEvent* anEvent)
 
 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
 {
-	_ui->statsToolBox->updateStat("Camera/Time capturing/ms", (float)info.id_, (float)info.timeCapture_*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time capturing/ms", (float)info.id, (float)info.timeCapture*1000.0);
-	_ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id_, (float)info.timeDisparity_*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id, (float)info.timeDisparity*1000.0);
-	_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id_, (float)info.timeMirroring_*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id, (float)info.timeMirroring*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", (float)info.id, (float)info.timeScanFromDepth*1000.0);
 }
 
 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
@@ -3014,6 +3015,7 @@ void MainWindow::startDetection()
 		_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
 		_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
 		_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
+		_camera->setScanFromDepth(_preferencesDialog->isSourceScanFromDepth(), _preferencesDialog->getSourceScanFromDepthDecimation(), _preferencesDialog->getSourceScanFromDepthMaxDepth());
 
 		//Create odometry thread if rgbd slam
 		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
@@ -3527,18 +3529,16 @@ void MainWindow::postProcessing()
 						}
 
 						Transform transform;
-						std::string rejectedMsg;
+						RegistrationInfo info;
-						float variance = -1.0f;
 						RegistrationVis registration(parameters);
-						std::vector<int> inliers;
+						transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &info);
-						transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &rejectedMsg, &inliers, &variance);
 
 						if(!transform.isNull())
 						{
 							UINFO("Added new loop closure between %d and %d.", from, to);
 							addedLinks.insert(from);
 							addedLinks.insert(to);
-							_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance, variance)));
+							_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
 							++loopClosuresAdded;
 							_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
 						}
@@ -3618,18 +3618,17 @@ void MainWindow::postProcessing()
 					if(!signatureFrom.sensorData().laserScanRaw().empty() &&
 					   !signatureTo.sensorData().laserScanRaw().empty())
 					{
-						std::string rejectedMsg;
+						RegistrationInfo info;
-						float variance = -1.0f;
+						Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &info);
-						Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &rejectedMsg, 0, &variance);
 
 						if(!transform.isNull())
 						{
-							Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
+							Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), info.variance, info.variance);
 							iter->second = newLink;
 						}
 						else
 						{
-							QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(rejectedMsg.c_str());
+							QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg_.c_str());
 							_initProgressDialog->appendText(str, Qt::darkYellow);
 							UWARN("%s", str.toStdString().c_str());
 							warn = true;

guilib/src/PreferencesDialog.cpp

@@ -210,9 +210,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	// Default Driver
 	connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
-	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
+	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
-	connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBCameraGroupBoxVisibility()));
+	connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
-	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoCameraGroupBoxVisibility()));
+	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	this->resetSettings(_ui->groupBox_source0);
 
 	_ui->predictionPlot->showLegend(false);
@@ -388,37 +388,27 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_freenect2EdgeAwareFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
-	connect(_ui->toolButton_cameraRGBDImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesStamps()));
+	connect(_ui->toolButton_cameraImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceImagesStamps()));
-	connect(_ui->lineEdit_cameraRGBDImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_cameraRGBDImages_path_rgb, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathRGB()));
 	connect(_ui->toolButton_cameraRGBDImages_path_depth, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathDepth()));
-	connect(_ui->toolButton_cameraRGBDImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathScans()));
+	connect(_ui->toolButton_cameraImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathScans()));
-	connect(_ui->toolButton_cameraRGBDImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathGt()));
+	connect(_ui->toolButton_cameraImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathGt()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->checkBox_cameraRGBDImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_cameraImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraRGBDImages_scale, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraRGBDImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraRGBDImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->spinBox_cameraRGBDImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraRGBDImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->comboBox_cameraRGBDImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
-	connect(_ui->toolButton_cameraStereoImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesStamps()));
-	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->toolButton_cameraStereoImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathGt()));
 	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()));
-	connect(_ui->lineEdit_cameraStereoImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->comboBox_cameraStereoImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->toolButton_cameraStereoVideo_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath()));
 	connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -433,7 +423,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openni2OniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 
-
+	connect(_ui->groupBox_scanFromDepth, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraScanFromDepth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 
 	//Rtabmap basic
 	connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
@@ -637,7 +629,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kVisInlierDistance().c_str());
 	_ui->loopClosure_bowIterations->setObjectName(Parameters::kVisIterations().c_str());
 	_ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kVisRefineIterations().c_str());
-	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kVisForce2D().c_str());
+	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kRegForce3DoF().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::defaultVisEstimationType());
@@ -646,7 +638,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
 	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
+
 	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
+	_ui->comboBox_registrationStrategy->setObjectName(Parameters::kRegStrategy().c_str());
 
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
 	_ui->loopClosure_correspondencesType->setObjectName(Parameters::kVisCorType().c_str());
@@ -663,10 +657,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
 	_ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str());
 
-	_ui->loopClosure_icp->setObjectName(Parameters::kRGBDLoopClosureLinkRefining().c_str());
 	_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str());
 	_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str());
-	_ui->loopClosure_icp2D->setObjectName(Parameters::kIcp2D().c_str());
 
 	_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kIcpVoxelSize().c_str());
 	_ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str());
@@ -1193,28 +1185,26 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_openni2OniPath->clear();
 		_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
 		_ui->lineEdit_cameraRGBDImages_path_depth->setText("");
-		_ui->checkBox_cameraRGBDImages_timestamps->setChecked(false);
 		_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0);
-		_ui->lineEdit_cameraRGBDImages_timestamps->setText("");
-		_ui->lineEdit_cameraRGBDImages_path_scans->setText("");
-		_ui->lineEdit_cameraRGBDImages_laser_transform->setText("0 0 0 0 0 0");
-		_ui->spinBox_cameraRGBDImages_max_scan_pts->setValue(0);
-		_ui->lineEdit_cameraRGBDImages_gt->setText("");
-		_ui->comboBox_cameraRGBDImages_gtFormat->setCurrentIndex(0);
 
 		_ui->source_comboBox_image_type->setCurrentIndex(kSrcDC1394-kSrcDC1394);
-		_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("");
 		_ui->checkBox_stereoVideo_rectify->setChecked(false);
-		_ui->lineEdit_cameraStereoImages_gt->setText("");
+
-		_ui->comboBox_cameraStereoImages_gtFormat->setCurrentIndex(0);
+		_ui->checkBox_cameraImages_timestamps->setChecked(false);
+		_ui->lineEdit_cameraImages_timestamps->setText("");
+		_ui->lineEdit_cameraImages_path_scans->setText("");
+		_ui->lineEdit_cameraImages_laser_transform->setText("0 0 0 0 0 0");
+		_ui->spinBox_cameraImages_max_scan_pts->setValue(0);
+		_ui->lineEdit_cameraImages_gt->setText("");
+		_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
+
+		_ui->groupBox_scanFromDepth->setChecked(false);
+		_ui->spinBox_cameraScanFromDepth_decimation->setValue(8);
+		_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
 	{
@@ -1491,27 +1481,13 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("RGBDImages");
 	_ui->lineEdit_cameraRGBDImages_path_rgb->setText(settings.value("path_rgb", _ui->lineEdit_cameraRGBDImages_path_rgb->text()).toString());
 	_ui->lineEdit_cameraRGBDImages_path_depth->setText(settings.value("path_depth", _ui->lineEdit_cameraRGBDImages_path_depth->text()).toString());
-	_ui->checkBox_cameraRGBDImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraRGBDImages_timestamps->isChecked()).toBool());
-	_ui->lineEdit_cameraRGBDImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraRGBDImages_timestamps->text()).toString());
 	_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(settings.value("scale", _ui->doubleSpinBox_cameraRGBDImages_scale->value()).toDouble());
-	_ui->lineEdit_cameraRGBDImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraRGBDImages_path_scans->text()).toString());
-	_ui->lineEdit_cameraRGBDImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraRGBDImages_laser_transform->text()).toString());
-	_ui->spinBox_cameraRGBDImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraRGBDImages_max_scan_pts->value()).toInt());
-	_ui->lineEdit_cameraRGBDImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraRGBDImages_gt->text()).toString());
-	_ui->comboBox_cameraRGBDImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraRGBDImages_gtFormat->currentIndex()).toInt());
 	settings.endGroup(); // RGBDImages
 
 	settings.beginGroup("StereoImages");
-	_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());
-	_ui->lineEdit_cameraStereoImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraStereoImages_gt->text()).toString());
-	_ui->comboBox_cameraStereoImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraStereoImages_gtFormat->currentIndex()).toInt());
 	settings.endGroup(); // StereoImages
 
 	settings.beginGroup("StereoVideo");
@@ -1524,6 +1500,14 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt());
 	_ui->source_images_refreshDir->setChecked(settings.value("refreshDir",_ui->source_images_refreshDir->isChecked()).toBool());
 	_ui->checkBox_rgbImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbImages_rectify->isChecked()).toBool());
+
+	_ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool());
+	_ui->lineEdit_cameraImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraImages_timestamps->text()).toString());
+	_ui->lineEdit_cameraImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraImages_path_scans->text()).toString());
+	_ui->lineEdit_cameraImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraImages_laser_transform->text()).toString());
+	_ui->spinBox_cameraImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value()).toInt());
+	_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
+	_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt());
 	settings.endGroup(); // images
 
 	settings.beginGroup("Video");
@@ -1531,6 +1515,12 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->checkBox_rgbVideo_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbVideo_rectify->isChecked()).toBool());
 	settings.endGroup(); // video
 
+	settings.beginGroup("ScanFromDepth");
+	_ui->groupBox_scanFromDepth->setChecked(settings.value("enabled", _ui->groupBox_scanFromDepth->isChecked()).toBool());
+	_ui->spinBox_cameraScanFromDepth_decimation->setValue(settings.value("decimation", _ui->spinBox_cameraScanFromDepth_decimation->value()).toInt());
+	_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(settings.value("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value()).toDouble());
+	settings.endGroup();//ScanFromDepth
+
 	settings.beginGroup("Database");
 	_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
@@ -1842,27 +1832,13 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.beginGroup("RGBDImages");
 	settings.setValue("path_rgb",            _ui->lineEdit_cameraRGBDImages_path_rgb->text());
 	settings.setValue("path_depth",          _ui->lineEdit_cameraRGBDImages_path_depth->text());
-	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraRGBDImages_timestamps->isChecked());
-	settings.setValue("stamps",              _ui->lineEdit_cameraRGBDImages_timestamps->text());
 	settings.setValue("scale",               _ui->doubleSpinBox_cameraRGBDImages_scale->value());
-	settings.setValue("path_scans",          _ui->lineEdit_cameraRGBDImages_path_scans->text());
-	settings.setValue("scan_transform",      _ui->lineEdit_cameraRGBDImages_laser_transform->text());
-	settings.setValue("scan_max_pts",        _ui->spinBox_cameraRGBDImages_max_scan_pts->value());
-	settings.setValue("gt_path",             _ui->lineEdit_cameraRGBDImages_gt->text());
-	settings.setValue("gt_format",           _ui->comboBox_cameraRGBDImages_gtFormat->currentIndex());
 	settings.endGroup(); // RGBDImages
 
 	settings.beginGroup("StereoImages");
-	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.setValue("gt_path",        _ui->lineEdit_cameraStereoImages_gt->text());
-	settings.setValue("gt_format",      _ui->comboBox_cameraStereoImages_gtFormat->currentIndex());
 	settings.endGroup(); // StereoImages
 
 	settings.beginGroup("StereoVideo");
@@ -1875,6 +1851,13 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
 	settings.setValue("refreshDir", 	_ui->source_images_refreshDir->isChecked());
 	settings.setValue("rectify", 	    _ui->checkBox_rgbImages_rectify->isChecked());
+	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked());
+	settings.setValue("stamps",              _ui->lineEdit_cameraImages_timestamps->text());
+	settings.setValue("path_scans",          _ui->lineEdit_cameraImages_path_scans->text());
+	settings.setValue("scan_transform",      _ui->lineEdit_cameraImages_laser_transform->text());
+	settings.setValue("scan_max_pts",        _ui->spinBox_cameraImages_max_scan_pts->value());
+	settings.setValue("gt_path",             _ui->lineEdit_cameraImages_gt->text());
+	settings.setValue("gt_format",           _ui->comboBox_cameraImages_gtFormat->currentIndex());
 	settings.endGroup(); // images
 
 	settings.beginGroup("Video");
@@ -1882,6 +1865,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("rectify", 	    _ui->checkBox_rgbVideo_rectify->isChecked());
 	settings.endGroup(); // video
 
+	settings.beginGroup("ScanFromDepth");
+	settings.setValue("enabled", 			_ui->groupBox_scanFromDepth->isChecked());
+	settings.setValue("decimation", 		_ui->spinBox_cameraScanFromDepth_decimation->value());
+	settings.setValue("maxDepth", 			_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
+	settings.endGroup();
+
 	settings.beginGroup("Database");
 	settings.setValue("path", 			   _ui->source_database_lineEdit_path->text());
 	settings.setValue("ignoreOdometry",    _ui->source_checkBox_ignoreOdometry->isChecked());
@@ -2454,9 +2443,9 @@ void PreferencesDialog::selectCalibrationPath()
 	}
 }
 
-void PreferencesDialog::selectSourceRGBDImagesStamps()
+void PreferencesDialog::selectSourceImagesStamps()
 {
-	QString dir = _ui->lineEdit_cameraRGBDImages_timestamps->text();
+	QString dir = _ui->lineEdit_cameraImages_timestamps->text();
 	if(dir.isEmpty())
 	{
 		dir = getWorkingDirectory();
@@ -2464,7 +2453,7 @@ void PreferencesDialog::selectSourceRGBDImagesStamps()
 	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Timestamps file (*.txt)"));
 	if(path.size())
 	{
-		_ui->lineEdit_cameraRGBDImages_timestamps->setText(path);
+		_ui->lineEdit_cameraImages_timestamps->setText(path);
 	}
 }
 
@@ -2483,9 +2472,9 @@ void PreferencesDialog::selectSourceRGBDImagesPathRGB()
 }
 
 
-void PreferencesDialog::selectSourceRGBDImagesPathScans()
+void PreferencesDialog::selectSourceImagesPathScans()
 {
-	QString dir = _ui->lineEdit_cameraRGBDImages_path_scans->text();
+	QString dir = _ui->lineEdit_cameraImages_path_scans->text();
 	if(dir.isEmpty())
 	{
 		dir = getWorkingDirectory();
@@ -2493,7 +2482,7 @@ void PreferencesDialog::selectSourceRGBDImagesPathScans()
 	QString path = QFileDialog::getExistingDirectory(this, tr("Select scans directory"), dir);
 	if(path.size())
 	{
-		_ui->lineEdit_cameraRGBDImages_path_scans->setText(path);
+		_ui->lineEdit_cameraImages_path_scans->setText(path);
 	}
 }
 
@@ -2511,9 +2500,9 @@ void PreferencesDialog::selectSourceRGBDImagesPathDepth()
 	}
 }
 
-void PreferencesDialog::selectSourceRGBDImagesPathGt()
+void PreferencesDialog::selectSourceImagesPathGt()
 {
-	QString dir = _ui->lineEdit_cameraRGBDImages_gt->text();
+	QString dir = _ui->lineEdit_cameraImages_gt->text();
 	if(dir.isEmpty())
 	{
 		dir = getWorkingDirectory();
@@ -2522,33 +2511,19 @@ void PreferencesDialog::selectSourceRGBDImagesPathGt()
 	if(path.size())
 	{
 		QStringList list;
-		for(int i=0; i<_ui->comboBox_cameraRGBDImages_gtFormat->count(); ++i)
+		for(int i=0; i<_ui->comboBox_cameraImages_gtFormat->count(); ++i)
 		{
-			list.push_back(_ui->comboBox_cameraRGBDImages_gtFormat->itemText(i));
+			list.push_back(_ui->comboBox_cameraImages_gtFormat->itemText(i));
 		}
 		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, 0, false);
 		if(!item.isEmpty())
 		{
-			_ui->lineEdit_cameraRGBDImages_gt->setText(path);
+			_ui->lineEdit_cameraImages_gt->setText(path);
-			_ui->comboBox_cameraRGBDImages_gtFormat->setCurrentIndex(_ui->comboBox_cameraRGBDImages_gtFormat->findText(item));
+			_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(_ui->comboBox_cameraImages_gtFormat->findText(item));
 		}
 	}
 }
 
-void PreferencesDialog::selectSourceStereoImagesStamps()
-{
-	QString dir = _ui->lineEdit_cameraStereoImages_timestamps->text();
-	if(dir.isEmpty())
-	{
-		dir = getWorkingDirectory();
-	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Timestamps file (*.txt)"));
-	if(path.size())
-	{
-		_ui->lineEdit_cameraStereoImages_timestamps->setText(path);
-	}
-}
-
 void PreferencesDialog::selectSourceStereoImagesPathLeft()
 {
 	QString dir = _ui->lineEdit_cameraStereoImages_path_left->text();
@@ -2577,44 +2552,6 @@ 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::selectSourceStereoImagesPathGt()
-{
-	QString dir = _ui->lineEdit_cameraStereoImages_gt->text();
-	if(dir.isEmpty())
-	{
-		dir = getWorkingDirectory();
-	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt)"));
-	if(path.size())
-	{
-		QStringList list;
-		for(int i=0; i<_ui->comboBox_cameraStereoImages_gtFormat->count(); ++i)
-		{
-			list.push_back(_ui->comboBox_cameraStereoImages_gtFormat->itemText(i));
-		}
-		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list);
-		if(!item.isEmpty())
-		{
-			_ui->lineEdit_cameraStereoImages_gt->setText(path);
-			_ui->comboBox_cameraStereoImages_gtFormat->setCurrentIndex(_ui->comboBox_cameraStereoImages_gtFormat->findText(item));
-		}
-	}
-}
-
 void PreferencesDialog::selectSourceImagesPath()
 {
 	QString dir = _ui->source_images_lineEdit_path->text();
@@ -2935,6 +2872,11 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 					this->addParameter(_ui->graphOptimization_stopEpsilon,       _ui->graphOptimization_stopEpsilon->value());
 					this->addParameter(_ui->graphOptimization_robust,            _ui->graphOptimization_robust->isChecked());
 				}
+				else if(comboBox == _ui->comboBox_registrationStrategy)
+				{
+					this->addParameters(_ui->groupBox_visualTransform2);
+					this->addParameters(_ui->groupBox_icp2);
+				}
 			}
 			// Add parameter
 			_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
@@ -2981,10 +2923,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 				this->addParameters(_ui->groupBox_visualTransform2);
 				this->addParameters(_ui->groupBox_icp2);
 			}
-			else if(value && checkbox == _ui->loopClosure_icp)
-			{
-				this->addParameters(_ui->groupBox_icp2);
-			}
 
 			if(groupBox)
 			{
@@ -3409,23 +3347,24 @@ void PreferencesDialog::updateSourceGrpVisibility()
 	_ui->groupBox_sourceStereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1);
 	_ui->groupBox_sourceRGB->setVisible(_ui->comboBox_sourceType->currentIndex() == 2);
 	_ui->groupBox_sourceDatabase->setVisible(_ui->comboBox_sourceType->currentIndex() == 3);
-}
 
-void PreferencesDialog::updateRGBDCameraGroupBoxVisibility()
+	_ui->groupBox_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1);
-{
-	_ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL);
-	_ui->groupBox_freenect2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcOpenNI_PCL);
-}
 
-void PreferencesDialog::updateRGBCameraGroupBoxVisibility()
+	_ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && (_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD));
-{
+	_ui->groupBox_openni2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD);
-	_ui->source_groupBox_images->setVisible(_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcUsbDevice);
+	_ui->groupBox_freenect2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD);
-	_ui->source_groupBox_video->setVisible(_ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcUsbDevice);
-}
 
-void PreferencesDialog::updateStereoCameraGroupBoxVisibility()
+	_ui->stackedWidget_stereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && (_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo));
-{
+	_ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo);
-	_ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcDC1394);
+
+	_ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && (_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB || _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB));
+	_ui->source_groupBox_images->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
+	_ui->source_groupBox_video->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB);
+
+	_ui->groupBox_sourceImages_optional->setVisible(
+			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) ||
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
+			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->comboBox_sourceType->currentIndex() == kSrcImages-kSrcRGB));
 }
 
 /*** GETTERS ***/
@@ -3701,19 +3640,9 @@ Transform PreferencesDialog::getSourceLocalTransform() const
 	return t;
 }
 
-Transform PreferencesDialog::getStereoLaserLocalTransform() const
+Transform PreferencesDialog::getLaserLocalTransform() const
 {
-	Transform t = Transform::fromString(_ui->lineEdit_cameraStereoImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
+	Transform t = Transform::fromString(_ui->lineEdit_cameraImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
-	if(t.isNull())
-	{
-		return Transform::getIdentity();
-	}
-	return t;
-}
-
-Transform PreferencesDialog::getRGBDLaserLocalTransform() const
-{
-	Transform t = Transform::fromString(_ui->lineEdit_cameraRGBDImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
 	if(t.isNull())
 	{
 		return Transform::getIdentity();
@@ -3753,6 +3682,18 @@ bool PreferencesDialog::isSourceStereoDepthGenerated() const
 {
 	return _ui->checkbox_stereo_depthGenerated->isChecked();
 }
+bool PreferencesDialog::isSourceScanFromDepth() const
+{
+	return _ui->groupBox_scanFromDepth->isChecked();
+}
+int PreferencesDialog::getSourceScanFromDepthDecimation() const
+{
+	return _ui->spinBox_cameraScanFromDepth_decimation->value();
+}
+double PreferencesDialog::getSourceScanFromDepthMaxDepth() const
+{
+	return _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value();
+}
 
 Camera * PreferencesDialog::createCamera(bool useRawImages)
 {
@@ -3848,12 +3789,12 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			_ui->doubleSpinBox_cameraRGBDImages_scale->value(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
-		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraRGBDImages_gt->text().toStdString(), _ui->comboBox_cameraRGBDImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
 		((CameraRGBDImages*)camera)->setScanPath(
-						_ui->lineEdit_cameraRGBDImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraRGBDImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
-						_ui->spinBox_cameraRGBDImages_max_scan_pts->value(),
+						_ui->spinBox_cameraImages_max_scan_pts->value(),
-						this->getRGBDLaserLocalTransform());
+						this->getLaserLocalTransform());
-		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraRGBDImages_timestamps->isChecked(), _ui->lineEdit_cameraRGBDImages_timestamps->text().toStdString());
+		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcDC1394)
 	{
@@ -3885,12 +3826,12 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			_ui->checkBox_stereoImages_rectify->isChecked(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
-		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraStereoImages_gt->text().toStdString(), _ui->comboBox_cameraStereoImages_gtFormat->currentIndex());
+		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
 		((CameraStereoImages*)camera)->setScanPath(
-						_ui->lineEdit_cameraStereoImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraStereoImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
-						_ui->spinBox_cameraStereoImages_max_scan_pts->value(),
+						_ui->spinBox_cameraImages_max_scan_pts->value(),
-						this->getStereoLaserLocalTransform());
+						this->getLaserLocalTransform());
-		((CameraStereoImages*)camera)->setTimestamps(_ui->checkBox_stereoImages_timestamps->isChecked(), _ui->lineEdit_cameraStereoImages_timestamps->text().toStdString());
+		((CameraStereoImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcStereoVideo)
 	{
@@ -3925,6 +3866,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		((CameraImages*)camera)->setStartIndex(_ui->source_images_spinBox_startPos->value());
 		((CameraImages*)camera)->setDirRefreshed(_ui->source_images_refreshDir->isChecked());
 		((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked());
+
+		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setScanPath(
+						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->spinBox_cameraImages_max_scan_pts->value(),
+						this->getLaserLocalTransform());
+		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcDatabase)
 	{
@@ -4153,6 +4101,7 @@ void PreferencesDialog::testOdometry()
 		cameraThread.setMirroringEnabled(isSourceMirroring());
 		cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 		cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
+		cameraThread.setScanFromDepth(_ui->groupBox_scanFromDepth->isChecked(), _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
 		UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
 		UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
 		UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
@@ -4220,6 +4169,7 @@ void PreferencesDialog::testCamera()
 			cameraThread.setMirroringEnabled(isSourceMirroring());
 			cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 			cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
+			cameraThread.setScanFromDepth(_ui->groupBox_scanFromDepth->isChecked(), _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
 			UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
 
 			cameraThread.start();

tools/OdometryViewer/main.cpp

@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/core/OdometryICP.h>
 #include <rtabmap/core/OdometryLocalMap.h>
 #include <rtabmap/core/OdometryF2F.h>
 #include <rtabmap/core/OdometryMono.h>
@@ -76,7 +75,6 @@ void showUsage()
 			"\n"
 			"  -d #                      ICP decimation (default 4)\n"
 			"  -v #                      ICP voxel size (default 0.005)\n"
-			"  -s #                      ICP samples (default 0, not used if voxel is set.)\n"
 			"  -cr #.#                   ICP correspondence ratio (default 0.7)\n"
 			"  -p2p                      ICP point to point (default point to plane)"
 			"\n"
@@ -117,7 +115,6 @@ int main (int argc, char * argv[])
 	int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
 	int decimation = 4;
 	float voxel = 0.005;
-	int samples = 10000;
 	float ratio = 0.7f;
 	int maxClouds = 10;
 	int briefBytes = rtabmap::Parameters::defaultBRIEFBytes();
@@ -402,23 +399,6 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-s") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				samples = std::atoi(argv[i]);
-				if(samples < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
 		if(strcmp(argv[i], "-cr") == 0)
 		{
 			++i;
@@ -689,15 +669,13 @@ int main (int argc, char * argv[])
 	}
 	else if(icp) // ICP
 	{
-		UINFO("ICP maximum correspondences distance = %f", distance);
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(distance)));
-		UINFO("ICP iterations =          %d", iterations);
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), uNumber2Str(iterations)));
-		UINFO("Cloud decimation =        %d", decimation);
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpVoxelSize(), uNumber2Str(voxel)));
-		UINFO("Cloud voxel size =        %f", voxel);
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), uNumber2Str(ratio)));
-		UINFO("Cloud samples =           %d", samples);
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), p2p?"false":"true"));
-		UINFO("Cloud correspondence ratio = %f", ratio);
-		UINFO("Cloud point to plane =    %s", p2p?"false":"true");
 
-		odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, ratio, !p2p);
+		odom = new rtabmap::OdometryF2F(parameters);
 	}
 
 	rtabmap::OdometryThread odomThread(odom);
@@ -811,6 +789,10 @@ int main (int argc, char * argv[])
 			if(camera->isCalibrated())
 			{
 				rtabmap::CameraThread cameraThread(camera);
+				if(icp)
+				{
+					cameraThread.setScanFromDepth(true, decimation, maxDepth);
+				}
 
 				odomThread.start();
 				cameraThread.start();