Refactoring of the cameraStereoImages and cameraRGBDImages classes (now inheriting from CameraImages) for easy setting of laser scan path, timestamps path and ground truth path.

Added graph::importPoses().
Can now have a ground truth published with SensorData (filled optionally by CameraImages classes).
Increased database closing time performance when the database is not saved.
Added ParametersToolBox widget in DatabaseViewer for core parameters (refactoring done to make easy access to all rtabmap parameters in DatabaseViewer).
Added Parameters::getType(key).
Added Transform::interpolate() to interpolate between two transforms (SLERP)
Updated pf_filter.m and added test_pf_filter.m MATLAB scripts (making easier to compare with a ground truth)
UPlot: can now save all curve data of a figure in one action (see right-click on legend area->"Copy all curve data to clipboard")

Matlab/ParticleFilter/pf_filter.m~

@@ -1,18 +0,0 @@
-
-function filtered = pf_filter(x, nParticles, noise, lambda)
-
-particles = zeros(nParticles,1) ;
-weights = zeros(nParticles,1);
-filtered=zeros(1,length(x));
-for i = 1:length(x);
-    for j = 1:nParticles 
-        rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++
-        bruit= noise*rn;
-        particles(j) = particles(j) + bruit ;
-        dist = abs(particles(j) - x(i));
-        weights(j) = exp(-lambda*dist);
-    end
-    weights = weights ./(sum(weights(:)));
-    filtered(i) = weights'*particles;
-    particles = Rresample2(particles,weights);
-end

Matlab/ParticleFilter/test_pf_filter.m

@@ -0,0 +1,47 @@
+
+
+clc
+%close all
+
+particles = 300;
+noiseXYZ = 0.02;
+lambdaXYZ = 1;
+noiseRPY = 0.005;
+lambdaRPY = 250;
+noise = [noiseXYZ noiseXYZ noiseXYZ noiseRPY noiseRPY noiseRPY];
+lambda = [lambdaXYZ lambdaXYZ lambdaXYZ lambdaRPY lambdaRPY lambdaRPY];
+axeNames = ['x' 'y' 'z' 'R' 'P' 'Y'];
+
+% P = [13 x t] (id, incremental odom, incremental ground truth)
+T = P(2:7,:);
+G = P(8:end,:);
+
+T(4:end,:) = T(4:end,:)*pi/180;
+G(4:end,:) = G(4:end,:)*pi/180;
+
+
+x_filtered = zeros(6, size(T, 2));
+for i=1:6
+    x=T(i,:);
+    x=x';
+    x_filtered(i,:) = pf_filter([x(2:end); 0], particles, noise(i), lambda(i));
+   
+end
+
+figure
+for i=1:6
+    subplot(2,3,i)
+    x=T(i,:);
+    x_gt = G(i,:);
+    plot(1:length(x),cumsum(x),'b', 1:length(x),cumsum(x_filtered(i,:)),'r', 1:length(x),cumsum(x_gt),'g');
+    legend(axeNames(i), [axeNames(i) ' filtered'], [axeNames(i) ' gt'])
+end
+
+figure
+for i=1:6
+    subplot(2,3,i)
+    x=T(i,:);
+    x_gt = G(i,:);
+    plot(1:length(x),x,'b', 1:length(x),x_filtered(i,:),'r', 1:length(x),x_gt,'g');
+    legend(axeNames(i), [axeNames(i) ' filtered'], [axeNames(i) ' gt'])
+end

corelib/include/rtabmap/core/CameraRGB.h

@@ -49,23 +49,11 @@ class RTABMAP_EXP CameraImages :
 	public Camera
 {
 public:
-	CameraImages(const std::string & path,
+	CameraImages();
-			int startAt = 1,
+	CameraImages(
-			bool refreshDir = false,
+			const std::string & path,
-			bool rectifyImages = false,
-			bool isDepth = false,
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
-	CameraImages(const std::string & scanPath,
-				const Transform & scanLocalTransform,
-				int scanMaxPts,
-			    const std::string & path,
-				int startAt = 1,
-				bool refreshDir = false,
-				bool rectifyImages = false,
-				bool isDepth = false,
-				float imageRate = 0,
-				const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -74,6 +62,41 @@ public:
 	std::string getPath() const {return _path;}
 	unsigned int imagesCount() const;
 	std::vector<std::string> filenames() const;
+	bool isImagesRectified() const {return _rectifyImages;}
+	const CameraModel & cameraModel() const {return _model;}
+
+	void setPath(const std::string & dir) {_path=dir;}
+	void setStartIndex(int index) {_startAt = index;} // negative means last
+	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
+	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
+
+	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "")
+	{
+		_filenamesAreTimestamps = fileNamesAreStamps;
+		timestampsPath_=filePath;
+	}
+
+	void setScanPath(
+			const std::string & dir,
+			int maxScanPts = 0,
+			const Transform & localTransform=Transform::getIdentity())
+	{
+		_scanPath = dir;
+		_scanLocalTransform = localTransform;
+		_scanMaxPts = maxScanPts;
+	}
+
+	void setGroundTruthPath(const std::string & filePath, int format = 0)
+	{
+		groundTruthPath_ = filePath;
+		_groundTruthFormat = format;
+	}
+
+	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
+	{
+		_isDepth = isDepth;
+		_depthScaleFactor=depthScaleFactor;
+	}
 
 protected:
 	virtual SensorData captureImage();
@@ -86,9 +109,11 @@ private:
 	bool _refreshDir;
 	bool _rectifyImages;
 	bool _isDepth;
+	float _depthScaleFactor;
 	int _count;
 	UDirectory * _dir;
 	std::string _lastFileName;
+
 	int _countScan;
 	UDirectory * _scanDir;
 	std::string _lastScanFileName;
@@ -96,7 +121,14 @@ private:
 	Transform _scanLocalTransform;
 	int _scanMaxPts;
 
-	std::string _cameraName;
+	bool _filenamesAreTimestamps;
+	std::string timestampsPath_;
+
+	std::string groundTruthPath_;
+	int _groundTruthFormat;
+
+	std::list<double> stamps_;
+	std::list<Transform> groundTruth_;
 	CameraModel _model;
 };
 

corelib/include/rtabmap/core/CameraRGBD.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/USemaphore.h"
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
@@ -267,7 +268,7 @@ private:
 /////////////////////////
 class CameraImages;
 class RTABMAP_EXP CameraRGBDImages :
-	public Camera
+	public CameraImages
 {
 public:
 	static bool available();
@@ -276,9 +277,7 @@ public:
 	CameraRGBDImages(
 			const std::string & pathRGBImages,
 			const std::string & pathDepthImages,
-			double depthScaleFactor = 1.0,
+			float depthScaleFactor = 1.0f,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRGBDImages();
@@ -291,14 +290,7 @@ protected:
 	virtual SensorData captureImage();
 
 private:
-	CameraImages * cameraRGB_;
+	CameraImages cameraDepth_;
-	CameraImages * cameraDepth_;
-	double depthScaleFactor_;
-	bool filenamesAreTimestamps_;
-	std::string timestampsPath_;
-	std::list<double> stamps_;
-	CameraModel cameraModel_;
-	std::string cameraName_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 #include <list>
 
 namespace FlyCapture2
@@ -98,7 +99,7 @@ private:
 /////////////////////////
 class CameraImages;
 class RTABMAP_EXP CameraStereoImages :
-	public Camera
+	public CameraImages
 {
 public:
 	static bool available();
@@ -107,26 +108,11 @@ public:
 	CameraStereoImages(
 			const std::string & pathLeftImages,
 			const std::string & pathRightImages,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoImages(
 			const std::string & pathLeftRightImages,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoImages(
-			const std::string & scanPath,
-			const Transform & scanLocalTransform,
-			int scanMaxPts,
-			const std::string & pathLeftImages,
-			const std::string & pathRightImages,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
@@ -140,14 +126,8 @@ protected:
 	virtual SensorData captureImage();
 
 private:
-	CameraImages * camera_;
 	CameraImages * camera2_;
-	bool filenamesAreTimestamps_;
-	std::string timestampsPath_;
-	bool rectifyImages_;
-	std::list<double> stamps_;
 	StereoCameraModel stereoModel_;
-	std::string cameraName_;
 };
 
 

corelib/include/rtabmap/core/Features2d.h

@@ -102,7 +102,9 @@ public:
 		kFeatureGfttOrb=8,   //new 0.10.11
 		kFeatureFastOrb=9};  //new 0.11.0
 
-	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters);
+	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
+	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
+	static Feature2D * create(Feature2D::Type type, int wordsPerImage, const ParametersMap & parameters = ParametersMap()); // for convenience
 
 	static void filterKeypointsByDepth(
 				std::vector<cv::KeyPoint> & keypoints,

corelib/include/rtabmap/core/Graph.h

@@ -51,6 +51,13 @@ bool RTABMAP_EXP exportPoses(
 		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
 		bool g2oRobust = false); // optional for format 4
 
+bool RTABMAP_EXP importPoses(
+		const std::string & filePath,
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
+		std::map<int, double> * stamps = 0); // optional for format 1
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,

corelib/include/rtabmap/core/Memory.h

@@ -78,6 +78,7 @@ public:
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
 			bool postInitClosingEvents = false);
+	void close(bool databaseSaved = true, bool postInitClosingEvents = false);
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
 	int incrementMapId(std::map<int, int> * reducedIds = 0);
@@ -253,7 +254,6 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
-	bool _postInitClosingEvents;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id

corelib/include/rtabmap/core/Odometry.h

@@ -100,6 +100,7 @@ private:
 	int _resetCurrentCount;
 	double previousStamp_;
 	Transform previousTransform_;
+	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 
 	std::vector<ParticleFilter *> filters_;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -61,6 +61,7 @@ public:
 	double interval;
 	Transform transform;
 	Transform transformFiltered;
+	Transform transformGroundTruth;
 	float distanceTravelled;
 
 	int type; // 0=BOW, 1=Optical Flow, 2=ICP

corelib/include/rtabmap/core/Parameters.h

@@ -62,10 +62,12 @@ typedef std::pair<std::string, std::string> ParametersPair;
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static TYPE default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+		static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
 			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE)); \
+								   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
 								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
@@ -94,10 +96,12 @@ typedef std::pair<std::string, std::string> ParametersPair;
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+		static std::string type##PREFIX##NAME() {return std::string("string");} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
 			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE)); \
+								   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, "string")); \
 								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
@@ -125,10 +129,12 @@ typedef std::pair<std::string, std::string> ParametersPair;
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static TYPE default##PREFIX##NAME() {return COND?DEFAULT_VALUE1:DEFAULT_VALUE2;} \
+		static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
 			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, COND?#DEFAULT_VALUE1:#DEFAULT_VALUE2)); \
+								   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
 								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
@@ -428,6 +434,12 @@ public:
 		return parameters_;
 	}
 	
+	/**
+	 * Get parameter type
+	 *
+	 */
+	static std::string getType(const std::string & paramKey);
+
 	/**
 	 * Get parameter description
 	 *
@@ -469,6 +481,7 @@ private:
 
 private:
 	static ParametersMap parameters_;
+	static ParametersMap parametersType_;
 	static ParametersMap descriptions_;
 	static Parameters instance_;
 	

corelib/include/rtabmap/core/Registration.h

@@ -49,6 +49,21 @@ public:
 			int * inliersOut = 0,
 			float * varianceOut = 0,
 			float * inliersRatioOut = 0) = 0;
+
+	Transform computeTransformation2(
+				const SensorData & fromData,
+				const SensorData & toData,
+				Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
+				std::string * rejectedMsg = 0,
+				int * inliersOut = 0,
+				float * varianceOut = 0,
+				float * inliersRatioOut = 0)
+	{
+		Signature fromSignature(fromData.id(), -1, 0, 0.0, "", Transform::getIdentity(), fromData);
+		Signature toSignature(toData.id(), -1, 0, 0.0, "", Transform::getIdentity(), toData);
+		return computeTransformation(fromSignature, toSignature, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
+	}
+
 protected:
 	Registration(const ParametersMap & parameters = ParametersMap()) :
 		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())

corelib/include/rtabmap/core/Rtabmap.h

@@ -67,7 +67,7 @@ public:
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");
 
-	void close();
+	void close(bool databaseSaved = true);
 
 	const std::string & getWorkingDir() const {return _wDir;}
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -61,7 +61,7 @@ public:
 	enum Cmd {
 			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
-			kCmdClose,
+			kCmdClose,            // params: [bool] database saved (default true)
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin

corelib/include/rtabmap/core/SensorData.h

@@ -191,6 +191,9 @@ public:
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
+	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
+	const Transform & groundTruth() const {return groundTruth_;}
+
 private:
 	int _id;
 	double _stamp;
@@ -215,6 +218,8 @@ private:
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	cv::Mat _descriptors;
+
+	Transform groundTruth_;
 };
 
 }

corelib/include/rtabmap/core/Transform.h

@@ -51,6 +51,8 @@ public:
 	Transform(const cv::Mat & transformationMatrix);
 	// x,y,z, roll,pitch,yaw
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
+	// x,y,z, qx,qy,qz,qw
+	Transform(float x, float y, float z, float qx, float qy, float qz, float qw);
 	// x,y, theta
 	Transform(float x, float y, float theta);
 
@@ -101,6 +103,7 @@ public:
 	float getNormSquared() const;
 	float getDistance(const Transform & t) const;
 	float getDistanceSquared(const Transform & t) const;
+	Transform interpolate(float t, const Transform & other) const;
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;
@@ -124,6 +127,14 @@ public:
 	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
+
+	/**
+	 * Format (3 values): x y z
+	 * Format (6 values): x y z roll pitch yaw
+	 * Format (7 values): x y z qx qy qz qw
+	 * Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
+	 * Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+	 */
 	static Transform fromString(const std::string & string);
 
 private:

corelib/src/Camera.cpp

@@ -55,10 +55,9 @@ Camera::Camera(float imageRate, const Transform & localTransform) :
 
 Camera::~Camera()
 {
-	if(_frameRateTimer)
+	UDEBUG("");
-	{
+	delete _frameRateTimer;
-		delete _frameRateTimer;
+	UDEBUG("");
-	}
 }
 
 SensorData Camera::takeImage(CameraInfo * info)

corelib/src/CameraRGB.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/CameraRGB.h"
-#include "rtabmap/core/DBDriver.h"
+#include "rtabmap/core/Graph.h"
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -50,57 +50,44 @@ namespace rtabmap
 /////////////////////////
 // CameraImages
 /////////////////////////
+CameraImages::CameraImages() :
+		_startAt(0),
+		_refreshDir(false),
+		_rectifyImages(false),
+		_isDepth(false),
+		_depthScaleFactor(1.0f),
+		_count(0),
+		_dir(0),
+		_countScan(0),
+		_scanDir(0),
+		_scanMaxPts(0),
+		_filenamesAreTimestamps(false),
+		_groundTruthFormat(0)
+	{}
 CameraImages::CameraImages(const std::string & path,
-					 int startAt,
-					 bool refreshDir,
-					 bool rectifyImages,
-					 bool isDepth,
 					 float imageRate,
 					 const Transform & localTransform) :
 	Camera(imageRate, localTransform),
 	_path(path),
-	_startAt(startAt),
+	_startAt(0),
-	_refreshDir(refreshDir),
+	_refreshDir(false),
-	_rectifyImages(rectifyImages),
+	_rectifyImages(false),
-	_isDepth(isDepth),
+	_isDepth(false),
+	_depthScaleFactor(1.0f),
 	_count(0),
 	_dir(0),
 	_countScan(0),
 	_scanDir(0),
-	_scanMaxPts(0)
+	_scanMaxPts(0),
+	_filenamesAreTimestamps(false),
+	_groundTruthFormat(0)
 {
 
 }
 
-CameraImages::CameraImages(const std::string & scanPath,
+CameraImages::~CameraImages()
-					const Transform & scanLocalTransform,
-					int scanMaxPts,
-					const std::string & path,
-					 int startAt,
-					 bool refreshDir,
-					 bool rectifyImages,
-					 bool isDepth,
-					 float imageRate,
-					 const Transform & localTransform) :
-	Camera(imageRate, localTransform),
-	_path(path),
-	_startAt(startAt),
-	_refreshDir(refreshDir),
-	_rectifyImages(rectifyImages),
-	_isDepth(isDepth),
-	_count(0),
-	_dir(0),
-	_countScan(0),
-	_scanDir(0),
-	_scanPath(scanPath),
-	_scanLocalTransform(scanLocalTransform),
-	_scanMaxPts(scanMaxPts)
-{
-
-}
-
-CameraImages::~CameraImages(void)
 {
+	UDEBUG("");
 	if(_dir)
 	{
 		delete _dir;
@@ -113,7 +100,6 @@ CameraImages::~CameraImages(void)
 
 bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-	_cameraName = cameraName;
 	_lastFileName.clear();
 	_lastScanFileName.clear();
 	_count = 0;
@@ -204,6 +190,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					_model.cy());
 		}
 	}
+	_model.setName(cameraName);
 
 	_model.setLocalTransform(this->getLocalTransform());
 	if(_rectifyImages && !_model.isValid())
@@ -212,7 +199,140 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
-	return _dir->isValid();
+	bool success = _dir->isValid();
+	stamps_.clear();
+	groundTruth_.clear();
+	if(success)
+	{
+		if(_filenamesAreTimestamps)
+		{
+			const std::list<std::string> & filenames = _dir->getFileNames();
+			for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
+			{
+				// format is 12234456.12334.png
+				std::list<std::string> list = uSplit(*iter, '.');
+				if(list.size() == 3)
+				{
+					list.pop_back(); // remove extension
+					double stamp = uStr2Double(uJoin(list, "."));
+					if(stamp > 0.0)
+					{
+						stamps_.push_back(stamp);
+					}
+					else
+					{
+						UERROR("Conversion filename to timestamp failed! (filename=%s)", iter->c_str());
+					}
+				}
+			}
+			if(stamps_.size() != this->imagesCount())
+			{
+				UERROR("The stamps count is not the same as the images (%d vs %d)! "
+					   "Converting filenames to timestamps is activated.",
+						(int)stamps_.size(), this->imagesCount());
+				stamps_.clear();
+				success = false;
+			}
+		}
+		else if(timestampsPath_.size())
+		{
+			FILE * file = 0;
+#ifdef _MSC_VER
+			fopen_s(&file, timestampsPath_.c_str(), "r");
+#else
+			file = fopen(timestampsPath_.c_str(), "r");
+#endif
+			if(file)
+			{
+				char line[16];
+				while ( fgets (line , 16 , file) != NULL )
+				{
+					stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
+				}
+				fclose(file);
+			}
+			if(stamps_.size() != this->imagesCount())
+			{
+				UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
+						"the timestamps file path if you don't want to use them (current file path=%s).",
+						(int)stamps_.size(), this->imagesCount(), timestampsPath_.c_str());
+				stamps_.clear();
+				success = false;
+			}
+		}
+
+		if(groundTruthPath_.size())
+		{
+			std::map<int, Transform> poses;
+			std::map<int, double> stamps;
+			if(!graph::importPoses(groundTruthPath_, _groundTruthFormat, poses, 0, &stamps))
+			{
+				UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
+				success = false;
+			}
+			else if(_groundTruthFormat != 1 && poses.size() != this->imagesCount())
+			{
+				UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
+						"the ground truth file path if you don't want to use it (current file path=%s).",
+						(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
+				success = false;
+			}
+			else if(_groundTruthFormat == 1 && stamps_.size() == 0)
+			{
+				UERROR("When using rgbd-slam format for ground truth, images must have timestamps!");
+				success = false;
+			}
+			else if(_groundTruthFormat == 1)
+			{
+				//Match ground truth values with images
+				groundTruth_.resize(stamps_.size(), Transform());
+				std::map<double, int> stampsToIds;
+				for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
+				{
+					stampsToIds.insert(std::make_pair(iter->second, iter->first));
+				}
+				std::vector<double> values = uValues(stamps);
+
+				for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
+				{
+					Transform pose; // null transform
+					std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
+					if(endIter != stampsToIds.end())
+					{
+						if(endIter->first == *ster)
+						{
+							pose = poses.at(endIter->second);
+						}
+						else if(endIter != stampsToIds.begin())
+						{
+							//interpolate
+							std::map<double, int>::iterator beginIter = endIter;
+							--beginIter;
+							double stampBeg = beginIter->first;
+							double stampEnd = endIter->first;
+							UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
+							float t = (*ster - stampBeg) / (stampEnd-stampBeg);
+							Transform & ta = poses.at(beginIter->second);
+							Transform & tb = poses.at(endIter->second);
+							if(!ta.isNull() && !tb.isNull())
+							{
+								pose = ta.interpolate(t, tb);
+							}
+						}
+
+					}
+					groundTruth_.push_back(pose);
+				}
+			}
+			else
+			{
+				groundTruth_ = uValuesList(poses);
+			}
+			UASSERT(groundTruth_.size() == stamps_.size());
+		}
+	}
+
+	return success;
 }
 
 bool CameraImages::isCalibrated() const
@@ -222,7 +342,7 @@ bool CameraImages::isCalibrated() const
 
 std::string CameraImages::getSerial() const
 {
-	return _cameraName;
+	return _model.name();
 }
 
 unsigned int CameraImages::imagesCount() const
@@ -247,6 +367,8 @@ SensorData CameraImages::captureImage()
 {
 	cv::Mat img;
 	cv::Mat scan;
+	double stamp = UTimer::now();
+	Transform groundTruthPose;
 	UDEBUG("");
 	if(_dir->isValid())
 	{
@@ -258,7 +380,7 @@ SensorData CameraImages::captureImage()
 				_scanDir->update();
 			}
 		}
-		if(_startAt == 0)
+		if(_startAt < 0)
 		{
 			const std::list<std::string> & fileNames = _dir->getFileNames();
 			if(fileNames.size())
@@ -295,13 +417,24 @@ SensorData CameraImages::captureImage()
 		}
 		else
 		{
+			if(stamps_.size())
+			{
+				stamp = stamps_.front();
+				stamps_.pop_front();
+				if(groundTruth_.size())
+				{
+					groundTruthPose = groundTruth_.front();
+					groundTruth_.pop_front();
+				}
+			}
+
 			std::string fileName;
 			std::string fullPath;
 			fileName = _dir->getNextFileName();
 			if(fileName.size())
 			{
 				fullPath = _path + fileName;
-				while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
+				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
 				{
 					fullPath = _path + fileName;
 				}
@@ -326,6 +459,11 @@ SensorData CameraImages::captureImage()
 									fileName.c_str());
 							img = cv::Mat();
 						}
+
+						if(_depthScaleFactor > 1.0f)
+						{
+							img /= _depthScaleFactor;
+						}
 					}
 					else
 					{
@@ -390,11 +528,9 @@ SensorData CameraImages::captureImage()
 		UWARN("Directory is not set, camera must be initialized.");
 	}
 
-	if(_isDepth)
+	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:cv::Mat(), _model, this->getNextSeqID(), stamp);
-	{
+	data.setGroundTruth(groundTruthPose);
-		return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
+	return data;
-	}
-	return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, img, cv::Mat(), _model, this->getNextSeqID(), UTimer::now());
 }
 
 

corelib/src/CameraRGBD.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/Graph.h"
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -1672,129 +1673,33 @@ bool CameraRGBDImages::available()
 CameraRGBDImages::CameraRGBDImages(
 		const std::string & pathRGBImages,
 		const std::string & pathDepthImages,
-		double depthScaleFactor,
+		float depthScaleFactor,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
 		float imageRate,
 		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
+		CameraImages(pathRGBImages, imageRate, localTransform)
-		cameraRGB_(0),
-		cameraDepth_(0),
-		depthScaleFactor_(depthScaleFactor),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath)
 {
 	UASSERT(depthScaleFactor >= 1.0);
-	cameraRGB_ = new CameraImages(pathRGBImages);
+	cameraDepth_.setPath(pathDepthImages);
-	cameraDepth_ = new CameraImages(pathDepthImages, 1, false, false, true);
+	cameraDepth_.setDepth(true, depthScaleFactor);
 }
 
 CameraRGBDImages::~CameraRGBDImages()
 {
-	if(cameraRGB_)
-	{
-		delete cameraRGB_;
-	}
-	if(cameraDepth_)
-	{
-		delete cameraDepth_;
-	}
 }
 
 bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-	// look for calibration files
-	cameraName_ = cameraName;
-	if(!calibrationFolder.empty() && !cameraName.empty())
-	{
-		if(!cameraModel_.load(calibrationFolder, cameraName))
-		{
-			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-					cameraName.c_str(), calibrationFolder.c_str());
-		}
-		else
-		{
-			UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
-					cameraModel_.fx(),
-					cameraModel_.fy(),
-					cameraModel_.cx(),
-					cameraModel_.cy());
-		}
-	}
-	cameraModel_.setLocalTransform(this->getLocalTransform());
-
 	bool success = false;
-	if(cameraRGB_->init() && cameraDepth_->init())
+	if(CameraImages::init() && cameraDepth_.init())
 	{
-		if(cameraRGB_->imagesCount() == cameraDepth_->imagesCount())
+		if(this->imagesCount() == cameraDepth_.imagesCount())
 		{
 			success = true;
 		}
 		else
 		{
 			UERROR("Cameras don't have the same number of images (%d vs %d)",
-					cameraRGB_->imagesCount(), cameraDepth_->imagesCount());
+					this->imagesCount(), cameraDepth_.imagesCount());
-		}
-	}
-
-	stamps_.clear();
-	if(success)
-	{
-		if(filenamesAreTimestamps_)
-		{
-			std::vector<std::string> filenames = cameraRGB_->filenames();
-			for(unsigned int i=0; i<filenames.size(); ++i)
-			{
-				// format is 12234456.12334.png
-				std::list<std::string> list = uSplit(filenames.at(i), '.');
-				if(list.size() == 3)
-				{
-					list.pop_back(); // remove extension
-					double stamp = uStr2Double(uJoin(list, "."));
-					if(stamp > 0.0)
-					{
-						stamps_.push_back(stamp);
-					}
-					else
-					{
-						UERROR("Conversion filename to timestamp failed! (filename=%s)", filenames.at(i).c_str());
-					}
-				}
-			}
-			if(stamps_.size() != cameraRGB_->imagesCount())
-			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! "
-					   "Converting filenames to timestamps is activated.",
-						(int)stamps_.size(), cameraRGB_->imagesCount());
-				stamps_.clear();
-				success = false;
-			}
-		}
-		else if(timestampsPath_.size())
-		{
-			FILE * file = 0;
-#ifdef _MSC_VER
-			fopen_s(&file, timestampsPath_.c_str(), "r");
-#else
-			file = fopen(timestampsPath_.c_str(), "r");
-#endif
-			if(file)
-			{
-				char line[16];
-				while ( fgets (line , 16 , file) != NULL )
-				{
-					stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
-				}
-				fclose(file);
-			}
-			if(stamps_.size() != cameraRGB_->imagesCount())
-			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
-						"the timestamps file path if you don't want to use them (current file path=%s).",
-						(int)stamps_.size(), cameraRGB_->imagesCount(), timestampsPath_.c_str());
-				stamps_.clear();
-				success = false;
-			}
 		}
 	}
 
@@ -1803,41 +1708,27 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st
 
 bool CameraRGBDImages::isCalibrated() const
 {
-	return cameraModel_.isValid();
+	return this->cameraModel().isValid();
 }
 
 std::string CameraRGBDImages::getSerial() const
 {
-	return cameraName_;
+	return this->cameraModel().name();
 }
 
 SensorData CameraRGBDImages::captureImage()
 {
 	SensorData data;
 
-	double stamp;
-	if(stamps_.size())
-	{
-		stamp = stamps_.front();
-		stamps_.pop_front();
-	}
-	else
-	{
-		stamp = UTimer::now();
-	}
 	SensorData rgb, depth;
-	rgb = cameraRGB_->takeImage();
+	rgb = CameraImages::captureImage();
 	if(!rgb.imageRaw().empty())
 	{
-		depth = cameraDepth_->takeImage();
+		depth = cameraDepth_.takeImage();
 		if(!depth.depthRaw().empty())
 		{
-			cv::Mat depthScaled = depth.depthRaw();
+			data = SensorData(rgb.imageRaw(), depth.depthRaw(), rgb.cameraModels(), rgb.id(), rgb.stamp());
-			if(depthScaleFactor_ > 1.0)
+			data.setGroundTruth(rgb.groundTruth());
-			{
-				depthScaled /= depthScaleFactor_;
-			}
-			data = SensorData(rgb.imageRaw(), depthScaled, cameraModel_, this->getNextSeqID(), stamp);
 		}
 	}
 	return data;

corelib/src/CameraStereo.cpp

@@ -733,44 +733,34 @@ bool CameraStereoImages::available()
 CameraStereoImages::CameraStereoImages(
 		const std::string & pathLeftImages,
 		const std::string & pathRightImages,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
 		bool rectifyImages,
 		float imageRate,
 		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
+		CameraImages(pathLeftImages, imageRate, localTransform),
-		camera_(0),
+		camera2_(new CameraImages(pathRightImages))
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
 {
-	camera_ = new CameraImages(pathLeftImages);
+	this->setImagesRectified(rectifyImages);
-	camera2_ = new CameraImages(pathRightImages);
+	camera2_->setImagesRectified(rectifyImages);
 }
 
 CameraStereoImages::CameraStereoImages(
 		const std::string & pathLeftRightImages,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
 		bool rectifyImages,
 		float imageRate,
 		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
+		CameraImages("", imageRate, localTransform),
-		camera_(0),
+		camera2_(0)
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
 {
 	std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
 	if(paths.size() >= 1)
 	{
-		camera_ = new CameraImages(paths[0]);
+		this->setPath(paths[0]);
+		this->setImagesRectified(rectifyImages);
 
 		if(paths.size() >= 2)
 		{
 			camera2_ = new CameraImages(paths[1]);
+			camera2_->setImagesRectified(rectifyImages);
 		}
 	}
 	else
@@ -779,44 +769,19 @@ CameraStereoImages::CameraStereoImages(
 	}
 }
 
-CameraStereoImages::CameraStereoImages(
-		const std::string & scanPath,
-		const Transform & scanLocalTransform,
-		int scanMaxPts,
-		const std::string & pathLeftImages,
-		const std::string & pathRightImages,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
-		bool rectifyImages,
-		float imageRate,
-		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
-		camera_(0),
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
-{
-	camera_ = new CameraImages(scanPath, scanLocalTransform, scanMaxPts, pathLeftImages);
-	camera2_ = new CameraImages(pathRightImages);
-}
-
 CameraStereoImages::~CameraStereoImages()
 {
-	if(camera_)
+	UDEBUG("");
-	{
-		delete camera_;
-	}
 	if(camera2_)
 	{
 		delete camera2_;
 	}
+	UDEBUG("");
 }
 
 bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	// look for calibration files
-	cameraName_ = cameraName;
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
 		if(!stereoModel_.load(calibrationFolder, cameraName))
@@ -835,31 +800,32 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	}
 
 	stereoModel_.setLocalTransform(this->getLocalTransform());
-	if(rectifyImages_ && !stereoModel_.isValid())
+	stereoModel_.setName(cameraName);
+	if(this->isImagesRectified() && !stereoModel_.isValid())
 	{
 		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 		return false;
 	}
 
+	//desactivate before init as we will do it in this class instead for convenience
+	this->setImagesRectified(false);
+
 	bool success = false;
-	if(camera_ == 0)
+	if(CameraImages::init())
-	{
-		UERROR("Cannot initialize the camera.");
-	}
-	else if(camera_->init())
 	{
 		if(camera2_)
 		{
+			camera2_->setImagesRectified(false);
 			if(camera2_->init())
 			{
-				if(camera_->imagesCount() == camera2_->imagesCount())
+				if(this->imagesCount() == camera2_->imagesCount())
 				{
 					success = true;
 				}
 				else
 				{
 					UERROR("Cameras don't have the same number of images (%d vs %d)",
-							camera_->imagesCount(), camera2_->imagesCount());
+							this->imagesCount(), camera2_->imagesCount());
 				}
 			}
 			else
@@ -872,68 +838,6 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 			success = true;
 		}
 	}
-
-	stamps_.clear();
-	if(success)
-	{
-		if(filenamesAreTimestamps_)
-		{
-			std::vector<std::string> filenames = camera_->filenames();
-			for(unsigned int i=0; i<filenames.size(); ++i)
-			{
-				// format is 12234456.12334.png
-				std::list<std::string> list = uSplit(filenames.at(i), '.');
-				if(list.size() == 3)
-				{
-					list.pop_back(); // remove extension
-					double stamp = uStr2Double(uJoin(list, "."));
-					if(stamp > 0.0)
-					{
-						stamps_.push_back(stamp);
-					}
-					else
-					{
-						UERROR("Conversion filename to timestamp failed! (filename=%s)", filenames.at(i).c_str());
-					}
-				}
-			}
-			if(stamps_.size() != camera_->imagesCount())
-			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! "
-					   "Converting filenames to timestamps is activated.",
-						(int)stamps_.size(), camera_->imagesCount());
-				stamps_.clear();
-				success = false;
-			}
-		}
-		else if(timestampsPath_.size())
-		{
-			FILE * file = 0;
-#ifdef _MSC_VER
-			fopen_s(&file, timestampsPath_.c_str(), "r");
-#else
-			file = fopen(timestampsPath_.c_str(), "r");
-#endif
-			if(file)
-			{
-				char line[16];
-				while ( fgets (line , 16 , file) != NULL )
-				{
-					stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
-				}
-				fclose(file);
-			}
-			if(stamps_.size() != camera_->imagesCount())
-			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
-						"the timestamps file path if you don't want to use them (current file path=%s).",
-						(int)stamps_.size(), camera_->imagesCount(), timestampsPath_.c_str());
-				stamps_.clear();
-				success = false;
-			}
-		}
-	}
-
 	return success;
 }
 
@@ -944,55 +848,44 @@ bool CameraStereoImages::isCalibrated() const
 
 std::string CameraStereoImages::getSerial() const
 {
-	return cameraName_;
+	return stereoModel_.name();
 }
 
 SensorData CameraStereoImages::captureImage()
 {
 	SensorData data;
-	if(camera_)
+
+	SensorData left, right;
+	left = CameraImages::captureImage();
+	if(!left.imageRaw().empty())
 	{
-		double stamp;
+		if(camera2_)
-		if(stamps_.size())
 		{
-			stamp = stamps_.front();
+			right = camera2_->takeImage();
-			stamps_.pop_front();
 		}
 		else
 		{
-			stamp = UTimer::now();
+			right = this->takeImage();
 		}
-		SensorData left, right;
-		left = camera_->takeImage();
-		if(!left.imageRaw().empty())
-		{
-			if(camera2_)
-			{
-				right = camera2_->takeImage();
-			}
-			else
-			{
-				right = camera_->takeImage();
-			}
 
-			if(!right.imageRaw().empty())
+		if(!right.imageRaw().empty())
+		{
+			// Rectification
+			cv::Mat leftImage = left.imageRaw();
+			cv::Mat rightImage = right.imageRaw();
+			if(rightImage.type() != CV_8UC1)
 			{
-				// Rectification
+				cv::Mat tmp;
-				cv::Mat leftImage = left.imageRaw();
+				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
-				cv::Mat rightImage = right.imageRaw();
+				rightImage = tmp;
-				if(rightImage.type() != CV_8UC1)
-				{
-					cv::Mat tmp;
-					cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
-					rightImage = tmp;
-				}
-				if(rectifyImages_ && stereoModel_.left().isValid() && stereoModel_.right().isValid())
-				{
-					leftImage = stereoModel_.left().rectifyImage(leftImage);
-					rightImage = stereoModel_.right().rectifyImage(rightImage);
-				}
-				data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
 			}
+			if(this->isImagesRectified() && stereoModel_.left().isValid() && stereoModel_.right().isValid())
+			{
+				leftImage = stereoModel_.left().rectifyImage(leftImage);
+				rightImage = stereoModel_.right().rectifyImage(rightImage);
+			}
+			data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
+			data.setGroundTruth(left.groundTruth());
 		}
 	}
 	return data;

corelib/src/CameraThread.cpp

@@ -52,6 +52,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 
 CameraThread::~CameraThread()
 {
+	UDEBUG("");
 	join(true);
 	if(_camera)
 	{
@@ -130,6 +131,7 @@ void CameraThread::mainLoop()
 
 void CameraThread::mainLoopKill()
 {
+	UDEBUG("");
 	if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
 	{
 		int i=20;

corelib/src/Features2d.cpp

@@ -343,7 +343,19 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 {
 	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
 }
+Feature2D * Feature2D::create(const ParametersMap & parameters)
+{
+	int type = Parameters::defaultKpDetectorStrategy();
+	Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
+	return create((Feature2D::Type)type, parameters);
+}
 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
+{
+	int wordsPerImage = Parameters::defaultKpWordsPerImage();
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), wordsPerImage);
+	return create(type, wordsPerImage, parameters);
+}
+Feature2D * Feature2D::create(Feature2D::Type type, int wordsPerImage, const ParametersMap & parameters)
 {
 	if(RTABMAP_NONFREE == 0)
 	{

corelib/src/Graph.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/common/common.h>
 #include <set>
 #include <queue>
+#include <fstream>
 
 #include <rtabmap/core/OptimizerTORO.h>
 #include <rtabmap/core/OptimizerG2O.h>
@@ -54,6 +55,7 @@ bool exportPoses(
 		const std::map<int, double> & stamps, // required for format 1
 		bool g2oRobust) // optional for format 4
 {
+	UDEBUG("%s", filePath.c_str());
 	std::string tmpPath = filePath;
 	if(format==3) // TORO
 	{
@@ -149,6 +151,98 @@ bool exportPoses(
 	return false;
 }
 
+bool importPoses(
+		const std::string & filePath,
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
+		std::map<int, double> * stamps) // optional for format 1
+{
+	UDEBUG("%s", filePath.c_str());
+	if(format==3) // TORO
+	{
+		std::multimap<int, Link> constraintsTmp;
+		if(OptimizerTORO::loadGraph(filePath, poses, constraintsTmp))
+		{
+			if(constraints)
+			{
+				*constraints = constraintsTmp;
+			}
+			return true;
+		}
+		return false;
+	}
+	else if(format == 4) // g2o
+	{
+		std::multimap<int, Link> constraintsTmp;
+		UERROR("Cannot import from g2o format because it is not yet supported!");
+		return false;
+	}
+	else
+	{
+		std::ifstream file;
+		file.open(filePath.c_str(), std::ifstream::in);
+		if(!file.good())
+		{
+			return false;
+		}
+		int id=1;
+		while(file.good())
+		{
+			std::string str;
+			std::getline(file, str);
+
+			if(str.front() == '#' || str.empty())
+			{
+				continue;
+			}
+
+			if(format == 1) // rgbd-slam format
+			{
+				std::list<std::string> strList = uSplit(str);
+				if(strList.size() ==  8)
+				{
+					double stamp = uStr2Float(strList.front());
+					strList.pop_front();
+					str = uJoin(strList, " ");
+					Transform pose = Transform::fromString(str);
+					if(pose.isNull())
+					{
+						UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
+					}
+					if(stamps)
+					{
+						stamps->insert(std::make_pair(id, stamp));
+					}
+					poses.insert(std::make_pair(id, pose));
+				}
+				else
+				{
+					UERROR("Error parsing \"%s\" with RGBD-SLAM format (should have 8 values: stamp x y z qw qx qy qz)", str.c_str());
+				}
+			}
+			else // default / KITTI format
+			{
+				Transform pose = Transform::fromString(str);
+				if(format == 2)
+				{
+					// for KITTI, we need to remove optical rotation
+					// z pointing front, x left, y down
+					Transform t( 0, 0, 1, 0,
+								-1, 0, 0, 0,
+								 0,-1, 0, 0);
+					pose = t * pose * t.inverse();
+				}
+				poses.insert(std::make_pair(id, pose));
+			}
+			++id;
+		}
+		file.close();
+		return true;
+	}
+	return false;
+}
+
 
 ////////////////////////////////////////////
 // Graph utilities

corelib/src/Memory.cpp

@@ -95,7 +95,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_memoryChanged(false),
 	_linksChanged(false),
 	_signaturesAdded(0),
-	_postInitClosingEvents(false),
 
 	_featureType((Feature2D::Type)Parameters::defaultKpDetectorStrategy()),
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
@@ -120,15 +119,14 @@ Memory::Memory(const ParametersMap & parameters) :
 
 bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters, bool postInitClosingEvents)
 {
-	_postInitClosingEvents = postInitClosingEvents;
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
 
 	UDEBUG("");
 	this->parseParameters(parameters);
 	bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
 	Parameters::parse(parameters, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);
 
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
 	DBDriver * tmpDriver = 0;
 	if((!_memoryChanged && !_linksChanged) || dbOverwritten)
 	{
@@ -143,7 +141,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		_dbDriver->setTimestampUpdateEnabled(false); // update links only
 	}
 	this->clear();
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
 
 	if(tmpDriver)
 	{
@@ -152,9 +150,9 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 
 	if(_dbDriver)
 	{
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection..."));
 		_dbDriver->closeConnection();
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database connection, done!"));
 	}
 
 	if(_dbDriver == 0 && !dbUrl.empty())
@@ -167,18 +165,18 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	{
 		_dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above)
 		success = false;
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
 		if(_dbDriver->openConnection(dbUrl, dbOverwritten))
 		{
 			success = true;
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
 
 			// Load the last working memory...
 			std::list<Signature*> dbSignatures;
 
 			if(loadAllNodesInWM)
 			{
-				if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
+				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
 				std::set<int> ids;
 				_dbDriver->getAllNodeIds(ids, true);
 				_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
@@ -186,7 +184,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			else
 			{
 				// load previous session working memory
-				if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
+				if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
 				_dbDriver->loadLastNodes(dbSignatures);
 			}
 			for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
@@ -207,7 +205,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 					delete *iter;
 				}
 			}
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
 
 			// Assign the last signature
 			if(_stMem.size()>0)
@@ -225,7 +223,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		}
 		else
 		{
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kError, std::string("Connecting to database ") + dbUrl + ", path is invalid!"));
 		}
 	}
 	else
@@ -243,7 +241,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	// Now load the dictionary if we have a connection
 	if(_dbDriver && _dbDriver->isConnected())
 	{
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary..."));
 		if(loadAllNodesInWM)
 		{
 			// load all referenced words in working memory
@@ -276,10 +274,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 		}
 		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
 		_vwd->update();
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
 	}
 
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
 	// Enable loaded signatures
 	const std::map<int, Signature *> & signatures = this->getSignatures();
 	for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
@@ -298,7 +296,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			s->setEnabled(true);
 		}
 	}
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
 
 	if(_vwd->getUnusedWordsSize())
 	{
@@ -306,35 +304,36 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	}
 	UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
 
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
 	return success;
 }
 
-Memory::~Memory()
+void Memory::close(bool databaseSaved, bool postInitClosingEvents)
 {
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
+	UDEBUG("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
 
-	if(!_memoryChanged && !_linksChanged)
+	if(!databaseSaved || (!_memoryChanged && !_linksChanged))
 	{
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
 
 		UDEBUG("");
 		if(_dbDriver)
 		{
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
 			_dbDriver->closeConnection();
 			delete _dbDriver;
 			_dbDriver = 0;
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
 		}
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
 		this->clear();
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
 	}
 	else
 	{
 		UDEBUG("");
-		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
+		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
 		if(!_memoryChanged && _linksChanged && _dbDriver)
 		{
 			// don't update the time stamps!
@@ -345,19 +344,29 @@ Memory::~Memory()
 		if(_dbDriver)
 		{
 			_dbDriver->emptyTrashes();
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
 			_dbDriver->closeConnection();
 			delete _dbDriver;
 			_dbDriver = 0;
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
 		}
 		else
 		{
-			if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
 		}
 	}
+	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
+}
 
+Memory::~Memory()
+{
+	this->close();
+
+	if(_dbDriver)
+	{
+		UWARN("Please call Memory::close() before");
+	}
 	if(_feature2D)
 	{
 		delete _feature2D;
@@ -378,7 +387,6 @@ Memory::~Memory()
 	{
 		delete _stereo;
 	}
-	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
 }
 
 void Memory::parseParameters(const ParametersMap & parameters)
@@ -3120,16 +3128,16 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 
 				if(keypoints.size())
 				{
+					// descriptors should be extracted before subpixel
+					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
+
 					std::vector<cv::Point2f> leftCorners;
+					cv::KeyPoint::convert(keypoints, leftCorners);
 					if(subPixelOn)
 					{
-						// descriptors should be extracted before subpixel
-						descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-						t = timer.ticks();
-						if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-						UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
-
-						cv::KeyPoint::convert(keypoints, leftCorners);
 						cv::cornerSubPix( imageMono, leftCorners,
 								cv::Size( _subPixWinSize, _subPixWinSize ),
 								cv::Size( -1, -1 ),
@@ -3139,15 +3147,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						{
 							keypoints[i].pt = leftCorners[i];
 						}
-
 						t = timer.ticks();
 						if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
 						UDEBUG("time subpix left kpts=%fs", t);
 					}
-					else
+
-					{
+					UASSERT(keypoints.size() == leftCorners.size());
-						cv::KeyPoint::convert(keypoints, leftCorners);
-					}
 
 					//generate a disparity map
 					std::vector<unsigned char> status;
@@ -3181,19 +3186,15 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 
 					if(keypoints.size())
 					{
-						if(!subPixelOn)
+						UASSERT(keypoints.size() == descriptors.rows);
-						{
-							descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-							t = timer.ticks();
-							if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-							UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
-						}
 
+						UASSERT(leftCorners.size() == keypoints.size());
 						keypoints3D = util3d::generateKeypoints3DStereo(
 								leftCorners,
 								rightCorners,
 								data.stereoCameraModel(),
 								status);
+						UASSERT(keypoints.size() == keypoints3D->size());
 
 						t = timer.ticks();
 						if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
@@ -3257,11 +3258,13 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 							if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
 							UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 						}
+						UASSERT(keypoints.size() == descriptors.rows);
 
 						keypoints3D = util3d::generateKeypoints3DDepth(
 								keypoints,
 								data.depthOrRightRaw(),
 								data.cameraModels());
+						UASSERT(keypoints.size() == keypoints3D->size());
 						t = timer.ticks();
 						if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 						UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);

corelib/src/Odometry.cpp

@@ -124,6 +124,7 @@ Odometry::~Odometry()
 void Odometry::reset(const Transform & initialPose)
 {
 	previousTransform_.setIdentity();
+	previousGroundTruthPose_.setNull();
 	_resetCurrentCount = 0;
 	previousStamp_ = 0;
 	distanceTravelled_ = 0;
@@ -212,6 +213,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		info->stamp = data.stamp();
 		info->interval = dt;
 		info->transform = t;
+
+		if(!data.groundTruth().isNull())
+		{
+			if(!previousGroundTruthPose_.isNull())
+			{
+				info->transformGroundTruth = previousGroundTruthPose_.inverse() * data.groundTruth();
+			}
+			previousGroundTruthPose_ = data.groundTruth();
+		}
 	}
 
 	previousTransform_.setIdentity();
@@ -304,10 +314,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 							x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
 			t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
 
-			if(info && _filteringStrategy > 0)
+			info->transformFiltered = t;
-			{
-				info->transformFiltered = t;
-			}
 		}
 
 		previousTransform_ = t;

corelib/src/Parameters.cpp

@@ -39,6 +39,7 @@ namespace rtabmap
 {
 
 ParametersMap Parameters::parameters_;
+ParametersMap Parameters::parametersType_;
 ParametersMap Parameters::descriptions_;
 Parameters Parameters::instance_;
 std::map<std::string, std::pair<bool, std::string> > Parameters::removedParameters_;
@@ -260,6 +261,21 @@ const ParametersMap & Parameters::getBackwardCompatibilityMap()
 	return backwardCompatibilityMap_;
 }
 
+std::string Parameters::getType(const std::string & paramKey)
+{
+	std::string type;
+	ParametersMap::iterator iter = parametersType_.find(paramKey);
+	if(iter != parametersType_.end())
+	{
+		type = iter->second;
+	}
+	else
+	{
+		UERROR("Parameters \"%s\" doesn't exist!", paramKey.c_str());
+	}
+	return type;
+}
+
 std::string Parameters::getDescription(const std::string & paramKey)
 {
 	std::string description;

corelib/src/RegistrationVis.cpp

@@ -94,7 +94,8 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	// override feature parameters
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		if(Parameters::isFeatureParameter(iter->first))
+		std::string group = uSplit(iter->first, '/').front();
+		if(Parameters::isFeatureParameter(iter->first) || group.compare("Stereo") == 0)
 		{
 			uInsert(_featureParameters, ParametersPair(iter->first, iter->second));
 		}

corelib/src/Rtabmap.cpp

@@ -312,7 +312,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
 	this->init(param, databasePath);
 }
 
-void Rtabmap::close()
+void Rtabmap::close(bool databaseSaved)
 {
 	UINFO("");
 	_highestHypothesis = std::make_pair(0,0.0f);
@@ -346,6 +346,7 @@ void Rtabmap::close()
 	}
 	if(_memory)
 	{
+		_memory->close(databaseSaved, true);
 		delete _memory;
 		_memory = 0;
 	}

corelib/src/RtabmapThread.cpp

@@ -210,7 +210,7 @@ void RtabmapThread::mainLoop()
 			UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
 			this->clearBufferedData();
 		}
-		_rtabmap->close();
+		_rtabmap->close(uStr2Bool(parameters.at("saved")));
 		break;
 	case kStateDumpingMemory:
 		_rtabmap->dumpData();
@@ -358,7 +358,10 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdClose)
 		{
 			ULOGGER_DEBUG("CMD_CLOSE");
-			pushNewState(kStateClose);
+			UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
+			ParametersMap param;
+			param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
+			pushNewState(kStateClose, param);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdResetMemory)
 		{

corelib/src/Stereo.cpp

@@ -41,6 +41,7 @@ Stereo::Stereo(const ParametersMap & parameters) :
 		maxDisparity_(Parameters::defaultStereoMaxDisparity()),
 		winSSD_(Parameters::defaultStereoSSD())
 {
+	this->parseParameters(parameters);
 }
 
 void Stereo::parseParameters(const ParametersMap & parameters)

corelib/src/Transform.cpp

@@ -67,6 +67,23 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
 	*this = fromEigen3f(t);
 }
 
+Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw)
+{
+	Eigen::Matrix3f rotation = Eigen::Quaternionf(qw, qx, qy, qz).toRotationMatrix();
+	data()[0] = rotation(0,0);
+	data()[1] = rotation(0,1);
+	data()[2] = rotation(0,2);
+	data()[3] = 0.0f;
+	data()[4] = rotation(1,0);
+	data()[5] = rotation(1,1);
+	data()[6] = rotation(1,2);
+	data()[7] = 0.0f;
+	data()[8] = rotation(2,0);
+	data()[9] = rotation(2,1);
+	data()[10] = rotation(2,2);
+	data()[11] = 0.0f;
+}
+
 Transform::Transform(float x, float y, float theta)
 {
 	Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta);
@@ -192,6 +209,19 @@ float Transform::getDistanceSquared(const Transform & t) const
 	return uNormSquared(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
 }
 
+Transform Transform::interpolate(float t, const Transform & other) const
+{
+	Eigen::Quaternionf qa=this->getQuaternionf();
+	Eigen::Quaternionf qb=other.getQuaternionf();
+	Eigen::Quaternionf qres = qa.slerp(t, qb);
+
+	float x = this->x() + t*(other.x() - this->x());
+	float y = this->y() + t*(other.y() - this->y());
+	float z = this->z() + t*(other.z() - this->z());
+
+	return Transform(x,y,z, qres.x(), qres.y(), qres.z(), qres.w());
+}
+
 std::string Transform::prettyPrint() const
 {
 	float x,y,z,roll,pitch,yaw;
@@ -317,42 +347,50 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
 }
 
 /**
- * Format (6 values): x y z roll pitch yaw.
+ * Format (3 values): x y z
- * Format (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz].
+ * Format (6 values): x y z roll pitch yaw
+ * Format (7 values): x y z qx qy qz qw
+ * Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
+ * Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
  */
 Transform Transform::fromString(const std::string & string)
 {
-	Transform t;
 	std::list<std::string> list = uSplit(string, ' ');
-	if(list.size() == 6 || list.size() == 9 || list.size() == 12)
+	UASSERT_MSG(list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12,
-	{
+			uFormat("Cannot parse \"%s\"", string.c_str()).c_str());
-		std::vector<float> numbers(list.size());
-		int i = 0;
-		for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
-		{
-			numbers[i++] = uStr2Float(*iter);
-		}
 
-		if(numbers.size() == 6)
+	std::vector<float> numbers(list.size());
-		{
+	int i = 0;
-			t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
+	for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
-		}
-		else if(numbers.size() == 9)
-		{
-			t = Transform(numbers[0], numbers[1], numbers[2], 0,
-						  numbers[3], numbers[4], numbers[5], 0,
-						  numbers[6], numbers[7], numbers[8], 0);
-		}
-		else if(numbers.size() == 12)
-		{
-			t = Transform(numbers[0], numbers[1], numbers[2], numbers[9],
-						  numbers[3], numbers[4], numbers[5], numbers[10],
-						  numbers[6], numbers[7], numbers[8], numbers[11]);
-		}
-	}
-	else
 	{
-		UERROR("Local transform is wrong! must have 6 or 9 items (%s)", string.c_str());
+		numbers[i++] = uStr2Float(*iter);
+	}
+
+	Transform t;
+	if(numbers.size() == 3)
+	{
+		t = Transform(numbers[0], numbers[1], numbers[2]);
+	}
+	else if(numbers.size() == 6)
+	{
+		t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
+	}
+	else if(numbers.size() == 7)
+	{
+
+		t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5], numbers[6]);
+	}
+	else if(numbers.size() == 9)
+	{
+		t = Transform(numbers[0], numbers[1], numbers[2], 0,
+					  numbers[3], numbers[4], numbers[5], 0,
+					  numbers[6], numbers[7], numbers[8], 0);
+	}
+	else if(numbers.size() == 12)
+	{
+		t = Transform(numbers[0], numbers[1], numbers[2], numbers[3],
+					  numbers[4], numbers[5], numbers[6], numbers[7],
+					  numbers[8], numbers[9], numbers[10], numbers[11]);
 	}
 	return t;
 }

corelib/src/util2d.cpp

@@ -156,6 +156,7 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 	status = std::vector<unsigned char>(leftCorners.size(), 0);
 	int totalIterations = 0;
 	int noSubPixel = 0;
+	int added = 0;
 	for(unsigned int i=0; i<leftCorners.size(); ++i)
 	{
 		int oi=0;
@@ -320,10 +321,14 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 
 			rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
 			status[i] = reject?0:1;
+			if(!reject)
+			{
+				++added;
+			}
 		}
 		subpixelTime+=timer.ticks();
 	}
-	UDEBUG("noSubPixel=%d", noSubPixel);
+	UDEBUG("noSubPixel=%d/%d", noSubPixel, added);
 	UDEBUG("totalIterations=%d", totalIterations);
 	UDEBUG("Time pyramid = %f s", pyramidTime);
 	UDEBUG("Time disparity = %f s", disparityTime);

examples/NoEventsExample/main.cpp

@@ -97,8 +97,6 @@ int main(int argc, char * argv[])
 	CameraStereoImages camera(
 			pathLeftImages,
 			pathRightImages,
-			false, // no timestamps in filenames
-			"",    // no file with timestamps
 			false, // assume that images are already rectified
 			(float)cameraRate,
 			opticalRotation);

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -110,6 +110,7 @@ private slots:
 	void updateConstraintView();
 	void updateLoggerLevel();
 	void updateStereo();
+	void notifyParametersChanged(const QStringList &);
 
 private:
 	QString getIniFilePath() const;

guilib/include/rtabmap/gui/MainWindow.h

@@ -302,7 +302,6 @@ private:
 	Transform _odometryCorrection;
 	Transform _lastOdomPose;
 	bool _processingOdometry;
-	double _lastOdomInfoUpdateTime;
 
 	QTimer * _oneSecondTimer;
 	QTime * _elapsedTime;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -194,6 +194,7 @@ public:
 	bool isSourceStereoDepthGenerated() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getStereoLaserLocalTransform() const; // stereo images
+	Transform getRGBDLaserLocalTransform() const; // rgbd images
 	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
@@ -260,10 +261,13 @@ private slots:
 	void selectSourceRGBDImagesStamps();
 	void selectSourceRGBDImagesPathRGB();
 	void selectSourceRGBDImagesPathDepth();
+	void selectSourceRGBDImagesPathScans();
+	void selectSourceRGBDImagesPathGt();
 	void selectSourceStereoImagesStamps();
 	void selectSourceStereoImagesPathLeft();
 	void selectSourceStereoImagesPathRight();
 	void selectSourceStereoImagesPathScans();
+	void selectSourceStereoImagesPathGt();
 	void selectSourceImagesPath();
 	void selectSourceVideoPath();
 	void selectSourceStereoVideoPath();

guilib/include/rtabmap/utilite/UPlot.h

@@ -140,6 +140,7 @@ public:
 	bool isVisible() const {return _visible;}
 	void setData(QVector<UPlotItem*> & data); // take the ownership
 	void getData(QVector<float> & x, QVector<float> & y) const; // only call in Qt MainThread
+	void getData(QMap<float,float> & data) const; // only call in Qt MainThread
 	void draw(QPainter * painter, const QRect & limits);
 
 public slots:
@@ -416,6 +417,7 @@ private:
 	bool _flat;
 	QMenu * _menu;
 	QAction * _aUseFlatButtons;
+	QAction * _aCopyAllCurveToClipboard;
 };
 
 

guilib/src/CMakeLists.txt

@@ -25,6 +25,7 @@ SET(headers_ui
 	./MapVisibilityWidget.h
 	../include/${PROJECT_PREFIX}/gui/GraphViewer.h
 	./CreateSimpleCalibrationDialog.h
+	./ParametersToolBox.h
 )
 
 SET(uis
@@ -86,6 +87,7 @@ SET(SRC_FILES
 	./MapVisibilityWidget.cpp
 	./GraphViewer.cpp
 	./CreateSimpleCalibrationDialog.cpp
+	./ParametersToolBox.cpp
 	${moc_srcs} 
 	${moc_uis} 
 	${srcs_qrc}

guilib/src/DatabaseViewer.cpp

@@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/core/Graph.h"
+#include "rtabmap/core/Stereo.h"
 #include "rtabmap/core/Optimizer.h"
 #include "rtabmap/core/RegistrationVis.h"
 #include "rtabmap/core/RegistrationIcp.h"
@@ -67,6 +68,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/SensorData.h"
 #include "ExportDialog.h"
 #include "rtabmap/gui/ProgressDialog.h"
+#include "ParametersToolBox.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
@@ -103,7 +105,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 
 	ui_->dockWidget_constraints->setVisible(false);
 	ui_->dockWidget_graphView->setVisible(false);
-	ui_->dockWidget_parameters->setVisible(false);
+	ui_->dockWidget_guiparameters->setVisible(false);
+	ui_->dockWidget_coreparameters->setVisible(false);
 	ui_->dockWidget_info->setVisible(false);
 	ui_->dockWidget_stereoView->setVisible(false);
 	ui_->dockWidget_view3d->setVisible(false);
@@ -113,51 +116,34 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 
 	ui_->graphicsView_stereo->setAlpha(255);
 
+	QSet<QString> ignoredGroups;
+	ignoredGroups.insert("Rtabmap");
+	ignoredGroups.insert("Mem");
+	ignoredGroups.insert("Kp");
+	ignoredGroups.insert("Odom");
+	ignoredGroups.insert("OdomBow");
+	ignoredGroups.insert("OdomFlow");
+	ignoredGroups.insert("OdomMono");
+	ignoredGroups.insert("VhEp");
+	ignoredGroups.insert("StereoBM");
+	ignoredGroups.insert("RGBD");
+	ignoredGroups.insert("DbSqlite3");
+	ignoredGroups.insert("Bayes");
+	ui_->parameters_toolbox->setupUi(ignoredGroups);
+
 	this->readSettings();
 
-	if(RTABMAP_NONFREE == 0)
-	{
-		ui_->comboBox_featureType->setItemData(0, 0, Qt::UserRole - 1);
-		ui_->comboBox_featureType->setItemData(1, 0, Qt::UserRole - 1);
-
-		if(ui_->comboBox_featureType->currentIndex() <= 1)
-		{
-			UWARN("SURF/SIFT not available, setting feature default to FAST/BRIEF.");
-			ui_->comboBox_featureType->setCurrentIndex(4);
-			ui_->comboBox_nnType->setCurrentIndex(3);
-		}
-	}
-	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
-	{
-		ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1);
-		if(ui_->comboBox_graphOptimizer->currentIndex() == 1)
-		{
-			UWARN("g2o is not available, setting optimization default to TORO.");
-			ui_->comboBox_graphOptimizer->setCurrentIndex(0);
-		}
-	}
-	if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
-	{
-		ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1);
-		if(ui_->comboBox_graphOptimizer->currentIndex() == 2)
-		{
-			UWARN("GTSAM is not available, setting optimization default to TORO.");
-			ui_->comboBox_graphOptimizer->setCurrentIndex(0);
-		}
-	}
-	if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
-	{
-		ui_->checkBox_robust->setEnabled(false);
-	}
-
 	ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
-	ui_->menuView->addAction(ui_->dockWidget_parameters->toggleViewAction());
+	ui_->menuView->addAction(ui_->dockWidget_guiparameters->toggleViewAction());
+	ui_->menuView->addAction(ui_->dockWidget_coreparameters->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
 	connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
 
+	connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
+
 	connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
 
 	// connect actions with custom slots
@@ -225,19 +211,14 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	ui_->spinBox_optimizationsFrom->setEnabled(false);
 	connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
 	connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
-	connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
-	connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
-	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
 	connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
-	connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
-	connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_gridFillUnkownSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
@@ -252,18 +233,6 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->doubleSpinBox_projMaxAngle, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
 	connect(ui_->spinBox_projClusterSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
 
-	connect(ui_->spinBox_stereo_flowIterations, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
-	connect(ui_->spinBox_stereo_flowMaxLevel, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
-	connect(ui_->spinBox_stereo_flowWinSize, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
-	connect(ui_->spinBox_stereo_gfttBlockSize, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
-	connect(ui_->doubleSpinBox_stereo_flowEps, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
-	connect(ui_->doubleSpinBox_stereo_gfttMinDistance, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
-	connect(ui_->spinBox_stereo_gfttMaxFeatures, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
-	connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
-	connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
-	connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(updateStereo()));
-	connect(ui_->checkBox_stereo_opticalFlow, SIGNAL(stateChanged(int)), this, SLOT(updateStereo()));
-	connect(ui_->checkBox_stereo_ssd, SIGNAL(stateChanged(int)), this, SLOT(updateStereo()));
 	ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
 	ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
 	ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
@@ -276,17 +245,12 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	//connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	// Graph view
-	connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_gridFillUnkownSpace, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
@@ -299,45 +263,14 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	// ICP parameters
+
 	connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_icp_voxel, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->spinBox_icp_downsamplingStepSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_icp_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->spinBox_icp_iteration, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_icp_p2plane, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->spinBox_icp_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_icp_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_icp_laserScan, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_icp_minCorrespondenceRatio, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
+	
-	// Visual parameters
-	connect(ui_->groupBox_visual_recomputeFeatures, SIGNAL(clicked(bool)), this, SLOT(configModified()));
-	connect(ui_->comboBox_featureType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
-	connect(ui_->comboBox_nnType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_visual_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_visual_var_inliers, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_visual_nndr, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->spinBox_visual_minCorrespondences, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_visual_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->spinBox_visual_iteration, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_visual_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	//stereo parameters
-	connect(ui_->spinBox_stereo_flowIterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->spinBox_stereo_flowMaxLevel, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->spinBox_stereo_flowWinSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->spinBox_stereo_gfttBlockSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_stereo_flowEps, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_stereo_gfttMinDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->spinBox_stereo_gfttMaxFeatures, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_stereo_opticalFlow, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_stereo_ssd, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 
 	// dockwidget
 	QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
@@ -350,7 +283,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	ui_->dockWidget_graphView->installEventFilter(this);
 	ui_->dockWidget_stereoView->installEventFilter(this);
 	ui_->dockWidget_view3d->installEventFilter(this);
-	ui_->dockWidget_parameters->installEventFilter(this);
+	ui_->dockWidget_guiparameters->installEventFilter(this);
+	ui_->dockWidget_coreparameters->installEventFilter(this);
 	ui_->dockWidget_info->installEventFilter(this);
 }
 
@@ -409,17 +343,12 @@ void DatabaseViewer::readSettings()
 	ui_->graphViewer->loadSettings(settings, "GraphView");
 
 	settings.beginGroup("optimization");
-	ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt());
 	ui_->checkBox_spanAllMaps->setChecked(settings.value("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()).toBool());
-	ui_->checkBox_robust->setChecked(settings.value("robust", ui_->checkBox_robust->isChecked()).toBool());
-	ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
 	ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool());
 	ui_->checkBox_ignoreGlobalLoop->setChecked(settings.value("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()).toBool());
 	ui_->checkBox_ignoreLocalLoopSpace->setChecked(settings.value("ignoreLocalLoopSpace", ui_->checkBox_ignoreLocalLoopSpace->isChecked()).toBool());
 	ui_->checkBox_ignoreLocalLoopTime->setChecked(settings.value("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked()).toBool());
 	ui_->checkBox_ignoreUserLoop->setChecked(settings.value("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->isChecked()).toBool());
-	ui_->comboBox_graphOptimizer->setCurrentIndex(settings.value("strategy", ui_->comboBox_graphOptimizer->currentIndex()).toInt());
-	ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
 	ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
 	ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
 	ui_->checkBox_gridFillUnkownSpace->setChecked(settings.value("unknownSpaceFilled", ui_->checkBox_gridFillUnkownSpace->isChecked()).toBool());
@@ -445,53 +374,23 @@ void DatabaseViewer::readSettings()
 	settings.beginGroup("icp");
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
 	ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
-	ui_->doubleSpinBox_icp_voxel->setValue(settings.value("voxel", ui_->doubleSpinBox_icp_voxel->value()).toDouble());
-	ui_->spinBox_icp_downsamplingStepSize->setValue(settings.value("samplingStep", ui_->spinBox_icp_downsamplingStepSize->value()).toInt());
-	ui_->doubleSpinBox_icp_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value()).toDouble());
-	ui_->spinBox_icp_iteration->setValue(settings.value("iterations", ui_->spinBox_icp_iteration->value()).toInt());
-	ui_->checkBox_icp_p2plane->setChecked(settings.value("point2place", ui_->checkBox_icp_p2plane->isChecked()).toBool());
-	ui_->spinBox_icp_normalKSearch->setValue(settings.value("normalKSearch", ui_->spinBox_icp_normalKSearch->value()).toInt());
-	ui_->checkBox_icp_2d->setChecked(settings.value("icp2d", ui_->checkBox_icp_2d->isChecked()).toBool());
 	ui_->checkBox_icp_laserScan->setChecked(settings.value("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked()).toBool());
-	ui_->doubleSpinBox_icp_minCorrespondenceRatio->setValue(settings.value("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()).toDouble());
 	settings.endGroup();
-
 	// Visual parameters
 	settings.beginGroup("visual");
-	ui_->comboBox_estimationType->setCurrentIndex(settings.value("estimationType", ui_->comboBox_estimationType->currentIndex()).toInt());
-	ui_->comboBox_pnpFlags->setCurrentIndex(settings.value("pnpFlags", ui_->comboBox_pnpFlags->currentIndex()).toInt());
-	ui_->groupBox_visual_recomputeFeatures->setChecked(settings.value("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked()).toBool());
-	ui_->comboBox_featureType->setCurrentIndex(settings.value("featureType", ui_->comboBox_featureType->currentIndex()).toInt());
-	ui_->comboBox_nnType->setCurrentIndex(settings.value("nnType", ui_->comboBox_nnType->currentIndex()).toInt());
-	ui_->checkBox_visual_2d->setChecked(settings.value("force2d", ui_->checkBox_visual_2d->isChecked()).toBool());
-	ui_->checkBox_visual_var_inliers->setChecked(settings.value("varianceInliers", ui_->checkBox_visual_var_inliers->isChecked()).toBool());
-	ui_->doubleSpinBox_visual_nndr->setValue(settings.value("nndr", ui_->doubleSpinBox_visual_nndr->value()).toDouble());
-	ui_->spinBox_visual_minCorrespondences->setValue(settings.value("minCorr", ui_->spinBox_visual_minCorrespondences->value()).toInt());
-	ui_->doubleSpinBox_visual_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value()).toDouble());
-	ui_->spinBox_visual_iteration->setValue(settings.value("iterations", ui_->spinBox_visual_iteration->value()).toDouble());
-	ui_->doubleSpinBox_visual_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_visual_maxDepth->value()).toDouble());
 	ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("detectMoreRadius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
 	ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("detectMoreAngle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
 	ui_->spinBox_detectMore_iterations->setValue(settings.value("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()).toInt());
 	settings.endGroup();
 
-	//Stereo parameters
-	settings.beginGroup("stereo");
-	ui_->spinBox_stereo_flowIterations->setValue(settings.value("flowIterations", ui_->spinBox_stereo_flowIterations->value()).toInt());
-	ui_->spinBox_stereo_flowMaxLevel->setValue(settings.value("flowMaxLevel", ui_->spinBox_stereo_flowMaxLevel->value()).toInt());
-	ui_->spinBox_stereo_flowWinSize->setValue(settings.value("flowWinSize", ui_->spinBox_stereo_flowWinSize->value()).toInt());
-	ui_->spinBox_stereo_gfttBlockSize->setValue(settings.value("gfttBlockSize", ui_->spinBox_stereo_gfttBlockSize->value()).toInt());
-	ui_->doubleSpinBox_stereo_flowEps->setValue(settings.value("flowEps", ui_->doubleSpinBox_stereo_flowEps->value()).toDouble());
-	ui_->doubleSpinBox_stereo_gfttMinDistance->setValue(settings.value("gfttMinDistance", ui_->doubleSpinBox_stereo_gfttMinDistance->value()).toDouble());
-	ui_->spinBox_stereo_gfttMaxFeatures->setValue(settings.value("gfttMaxFeatures", ui_->spinBox_stereo_gfttMaxFeatures->value()).toInt());
-	ui_->doubleSpinBox_stereo_gfttQuality->setValue(settings.value("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value()).toDouble());
-	ui_->doubleSpinBox_stereo_maxSlope->setValue(settings.value("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value()).toDouble());
-	ui_->checkBox_stereo_subpix->setChecked(settings.value("subpix", ui_->checkBox_stereo_subpix->isChecked()).toBool());
-	ui_->checkBox_stereo_opticalFlow->setChecked(settings.value("opticalFlow", ui_->checkBox_stereo_opticalFlow->isChecked()).toBool());
-	ui_->checkBox_stereo_ssd->setChecked(settings.value("ssd", ui_->checkBox_stereo_ssd->isChecked()).toBool());
-	settings.endGroup();
-
 	settings.endGroup(); // DatabaseViewer
+
+	ParametersMap parameters;
+	Parameters::readINI(path.toStdString(), parameters);
+	for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
+	{
+		ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
+	}
 }
 
 void DatabaseViewer::writeSettings()
@@ -516,17 +415,16 @@ void DatabaseViewer::writeSettings()
 
 	// save optimization settings
 	settings.beginGroup("optimization");
-	settings.setValue("iterations", ui_->spinBox_iterations->value());
+	//settings.setValue("iterations", ui_->spinBox_iterations->value());
 	settings.setValue("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked());
-	settings.setValue("robust", ui_->checkBox_robust->isChecked());
+	//settings.setValue("robust", ui_->checkBox_robust->isChecked());
-	settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
 	settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked());
 	settings.setValue("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked());
 	settings.setValue("ignoreLocalLoopSpace", ui_->checkBox_ignoreLocalLoopSpace->isChecked());
 	settings.setValue("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked());
 	settings.setValue("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->isChecked());
-	settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
+	//settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
-	settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
+	//settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
 	settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
 	settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
 	settings.setValue("unknownSpaceFilled", ui_->checkBox_gridFillUnkownSpace->isChecked());
@@ -553,54 +451,21 @@ void DatabaseViewer::writeSettings()
 	settings.beginGroup("icp");
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
 	settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
-	settings.setValue("voxel", ui_->doubleSpinBox_icp_voxel->value());
-	settings.setValue("samplingStep", ui_->spinBox_icp_downsamplingStepSize->value());
-	settings.setValue("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value());
-	settings.setValue("iterations", ui_->spinBox_icp_iteration->value());
-	settings.setValue("point2place", ui_->checkBox_icp_p2plane->isChecked());
-	settings.setValue("normalKSearch", ui_->spinBox_icp_normalKSearch->value());
-	settings.setValue("icp2d", ui_->checkBox_icp_2d->isChecked());
 	settings.setValue("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked());
-	settings.setValue("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value());
 	settings.endGroup();
 	
 	// save Visual parameters
 	settings.beginGroup("visual");
-	settings.setValue("estimationType", ui_->comboBox_estimationType->currentIndex());
-	settings.setValue("pnpFlags", ui_->comboBox_pnpFlags->currentIndex());
-	settings.setValue("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked());
-	settings.setValue("featureType", ui_->comboBox_featureType->currentIndex());
-	settings.setValue("nnType", ui_->comboBox_nnType->currentIndex());
-	settings.setValue("force2d", ui_->checkBox_visual_2d->isChecked());
-	settings.setValue("varianceInliers", ui_->checkBox_visual_var_inliers->isChecked());
-	settings.setValue("nndr", ui_->doubleSpinBox_visual_nndr->value());
-	settings.setValue("minCorr", ui_->spinBox_visual_minCorrespondences->value());
-	settings.setValue("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value());
-	settings.setValue("iterations", ui_->spinBox_visual_iteration->value());
-	settings.setValue("maxDepth", ui_->doubleSpinBox_visual_maxDepth->value());
 	settings.setValue("detectMoreRadius", ui_->doubleSpinBox_detectMore_radius->value());
 	settings.setValue("detectMoreAngle", ui_->doubleSpinBox_detectMore_angle->value());
 	settings.setValue("detectMoreIterations", ui_->spinBox_detectMore_iterations->value());
 	settings.endGroup();
 
-	//Stereo parameters
-	settings.beginGroup("stereo");
-	settings.setValue("flowIterations", ui_->spinBox_stereo_flowIterations->value());
-	settings.setValue("flowMaxLevel", ui_->spinBox_stereo_flowMaxLevel->value());
-	settings.setValue("flowWinSize", ui_->spinBox_stereo_flowWinSize->value());
-	settings.setValue("gfttBlockSize", ui_->spinBox_stereo_gfttBlockSize->value());
-	settings.setValue("flowEps", ui_->doubleSpinBox_stereo_flowEps->value());
-	settings.setValue("gfttMinDistance", ui_->doubleSpinBox_stereo_gfttMinDistance->value());
-	settings.setValue("gfttMaxFeatures", ui_->spinBox_stereo_gfttMaxFeatures->value());
-	settings.setValue("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value());
-	settings.setValue("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value());
-	settings.setValue("subpix", ui_->checkBox_stereo_subpix->isChecked());
-	settings.setValue("opticalFlow", ui_->checkBox_stereo_opticalFlow->isChecked());
-	settings.setValue("ssd", ui_->checkBox_stereo_ssd->isChecked());
-	settings.endGroup();
-
 	settings.endGroup(); // DatabaseViewer
 
+	const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
+	Parameters::writeINI(path.toStdString(), parameters);
+
 	this->setWindowModified(false);
 }
 
@@ -1430,7 +1295,8 @@ void DatabaseViewer::generateTOROGraph()
 		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
 		if(!path.isEmpty())
 		{
-			if(ui_->checkBox_ignoreCovariance->isChecked())
+			bool varianceIgnored = uStr2Bool(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerVarianceIgnored()));
+			if(varianceIgnored)
 			{
 				std::multimap<int, rtabmap::Link> links = graphLinks_;
 				for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -1472,18 +1338,20 @@ void DatabaseViewer::generateG2OGraph()
 		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".g2o", tr("g2o file (*.g2o)"));
 		if(!path.isEmpty())
 		{
-			if(ui_->checkBox_ignoreCovariance->isChecked())
+			bool varianceIgnored = uStr2Bool(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerVarianceIgnored()));
+			bool robust = uStr2Bool(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerRobust()));
+			if(varianceIgnored)
 			{
 				std::multimap<int, rtabmap::Link> links = graphLinks_;
 				for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
 					iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
 				}
-				graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), links, std::map<int, double>(), ui_->checkBox_robust->isChecked());
+				graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), links, std::map<int, double>(), robust);
 			}
 			else
 			{
-				graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), graphLinks_, std::map<int, double>(), ui_->checkBox_robust->isChecked());
+				graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), graphLinks_, std::map<int, double>(), robust);
 			}
 		}
 	}
@@ -1680,7 +1548,8 @@ void DatabaseViewer::view3DLaserScans()
 							color = (Qt::GlobalColor)(mapId % 12 + 7 );
 						}
 
-						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, ui_->spinBox_icp_normalKSearch->value());
+						int normalK = uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kIcpPointToPlaneNormalNeighbors()));
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalK);
 
 						viewer->addCloud(uFormat("cloud%d", iter->first), cloudNormals, pose, color);
 
@@ -2460,18 +2329,24 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		}
 
 		UTimer timer;
+		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
+		bool opticalFlow = uStr2Bool(parameters.at(Parameters::kStereoOpticalFlow()));
+		Stereo * stereo = 0;
+		if(opticalFlow)
+		{
+			stereo = new StereoOpticalFlow(parameters);
+		}
+		else
+		{
+			stereo = new Stereo(parameters);
+		}
 
 		// generate kpts
 		std::vector<cv::KeyPoint> kpts;
 		cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
-		ParametersMap parameters;
+		uInsert(parameters, ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kVisMaxFeatures())));
-		parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(ui_->spinBox_stereo_gfttMaxFeatures->value())));
+		Feature2D * kptDetector = Feature2D::create(parameters);
-		parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), uNumber2Str(ui_->doubleSpinBox_stereo_gfttMinDistance->value())));
+		kpts = kptDetector->generateKeypoints(leftMono, opticalFlow?roi:cv::Rect());
-		parameters.insert(ParametersPair(Parameters::kGFTTQualityLevel(), uNumber2Str(ui_->doubleSpinBox_stereo_gfttQuality->value())));
-		parameters.insert(ParametersPair(Parameters::kGFTTBlockSize(), uNumber2Str(ui_->spinBox_stereo_gfttBlockSize->value())));
-		Feature2D::Type type = Feature2D::kFeatureGfttBrief;
-		Feature2D * kptDetector = Feature2D::create(type, parameters);
-		kpts = kptDetector->generateKeypoints(leftMono, roi);
 		delete kptDetector;
 
 		float timeKpt = timer.ticks();
@@ -2479,55 +2354,18 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		std::vector<cv::Point2f> leftCorners;
 		cv::KeyPoint::convert(kpts, leftCorners);
 
-		int subPixWinSize = 3;
-		int subPixIterations = 30;
-		double subPixEps = 0.02;
-		if(ui_->checkBox_stereo_subpix->isChecked())
-		{
-			UDEBUG("cv::cornerSubPix() begin");
-			cv::cornerSubPix(leftMono, leftCorners,
-				cv::Size( subPixWinSize, subPixWinSize ),
-				cv::Size( -1, -1 ),
-				cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) );
-			UDEBUG("cv::cornerSubPix() end");
-		}
-
 		// Find features in the new right image
 		std::vector<unsigned char> status;
 		std::vector<cv::Point2f> rightCorners;
 
-		if(ui_->checkBox_stereo_opticalFlow->isChecked())
+		rightCorners = stereo->computeCorrespondences(
-		{
+				leftMono,
-			UDEBUG("");
+				data->rightRaw(),
-			std::vector<float> err;
+				leftCorners,
-			util2d::calcOpticalFlowPyrLKStereo(
+				status);
-					leftMono,
+		delete stereo;
-					data->rightRaw(),
+
-					leftCorners,
+		float timeStereo = timer.ticks();
-					rightCorners,
-					status,
-					err,
-					cv::Size(ui_->spinBox_stereo_flowWinSize->value(), 3), ui_->spinBox_stereo_flowMaxLevel->value(),
-					cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, ui_->spinBox_stereo_flowIterations->value(), ui_->doubleSpinBox_stereo_flowEps->value()));
-			UDEBUG("");
-		}
-		else
-		{
-			UDEBUG("");
-			rightCorners = util2d::calcStereoCorrespondences(
-					leftMono,
-					data->rightRaw(),
-					leftCorners,
-					status,
-					cv::Size(ui_->spinBox_stereo_flowWinSize->value(), 3),
-					ui_->spinBox_stereo_flowMaxLevel->value(),
-					ui_->spinBox_stereo_flowIterations->value(),
-					0,
-					64,
-					ui_->checkBox_stereo_ssd->isChecked());
-			UDEBUG("");
-		}
-		float timeFlow = timer.ticks();
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(kpts.size());
@@ -2543,35 +2381,20 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 			pcl::PointXYZ pt(bad_point, bad_point, bad_point);
 			if(status[i])
 			{
-				//UDEBUG("Left(%f,%f), right(%f,%f) disparity=%f", leftCorners[i].x, leftCorners[i].y, rightCorners[i].x, rightCorners[i].y, leftCorners[i].x - rightCorners[i].x);
-
 				float disparity = leftCorners[i].x - rightCorners[i].x;
 				if(disparity > 0.0f)
 				{
-					if(fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)) < ui_->doubleSpinBox_stereo_maxSlope->value())
+					pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
-					{
+							leftCorners[i],
-						pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
+							disparity,
-								leftCorners[i],
+							data->stereoCameraModel());
-								disparity,
-								data->stereoCameraModel());
 
-						if(pcl::isFinite(tmpPt))
+					if(pcl::isFinite(tmpPt))
-						{
-							pt = pcl::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform().toEigen3f());
-							status[i] = 100; //blue
-							++inliers;
-							cloud->at(oi++) = pt;
-						}
-					}
-					else if(fabs(leftCorners[i].y-rightCorners[i].y) <=1.0f)
 					{
-						status[i] = 110; //cyan
+						pt = pcl::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform().toEigen3f());
+						status[i] = 100; //blue
 						++inliers;
-					}
+						cloud->at(oi++) = pt;
-					else
-					{
-						status[i] = 101; //yellow
-						++slopeOutliers;
 					}
 				}
 				else
@@ -2587,8 +2410,8 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		}
 		cloud->resize(oi);
 
-		UINFO("correspondences = %d/%d (%f) (time kpt=%fs flow=%fs)",
+		UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
-				(int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeFlow);
+				(int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
 
 		ui_->stereoViewer->updateCameraTargetPosition(Transform::getIdentity());
 		ui_->stereoViewer->addOrUpdateCloud("stereo", cloud);
@@ -3081,13 +2904,7 @@ void DatabaseViewer::updateConstraintView(
 						}
 						if(poses.size())
 						{
-							ParametersMap optimizerParameters;
+							Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
-							uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerStrategy(), uNumber2Str(ui_->comboBox_graphOptimizer->currentIndex())));
-							uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(ui_->spinBox_iterations->value())));
-							uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerSlam2D(), uBool2Str(ui_->checkBox_2dslam->isChecked())));
-							uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ui_->checkBox_ignoreCovariance->isChecked())));
-							uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerRobust(), uBool2Str(ui_->checkBox_robust->isChecked())));
-							Optimizer * optimizer = Optimizer::create(optimizerParameters);
 
 							UASSERT(uContains(poses, link.to()));
 							std::map<int, rtabmap::Transform> posesOut;
@@ -3557,13 +3374,7 @@ void DatabaseViewer::updateGraphView()
 				.arg(totalLocalTime)
 				.arg(totalUser));
 
-		ParametersMap optimizerParameters;
+		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
-		uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerStrategy(), uNumber2Str(ui_->comboBox_graphOptimizer->currentIndex())));
-		uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(ui_->spinBox_iterations->value())));
-		uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerSlam2D(), uBool2Str(ui_->checkBox_2dslam->isChecked())));
-		uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ui_->checkBox_ignoreCovariance->isChecked())));
-		uInsert(optimizerParameters, std::make_pair(Parameters::kOptimizerRobust(), uBool2Str(ui_->checkBox_robust->isChecked())));
-		Optimizer * optimizer = Optimizer::create(optimizerParameters);
 
 		std::map<int, rtabmap::Transform> posesOut;
 		std::multimap<int, rtabmap::Link> linksOut;
@@ -3711,6 +3522,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	dbDriver_->getNodeData(currentLink.from(), dataFrom);
 	dbDriver_->getNodeData(currentLink.to(), dataTo);
 
+	ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 
 	UTimer timer;
 	if(!ui_->checkBox_icp_laserScan->isChecked())
@@ -3739,16 +3551,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	}
 	UINFO("Uncompress time: %f s", timer.ticks());
 
-	ParametersMap parameters;
-	parameters.insert(ParametersPair(Parameters::kIcpDownsamplingStep(), uNumber2Str(ui_->spinBox_icp_downsamplingStepSize->value())));
-	parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), uNumber2Str(ui_->doubleSpinBox_icp_voxel->value())));
-	parameters.insert(ParametersPair(Parameters::kIcp2D(), uBool2Str(ui_->checkBox_icp_2d->isChecked())));
-	parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), uBool2Str(ui_->checkBox_icp_p2plane->isChecked())));
-	parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneNormalNeighbors(), uNumber2Str(ui_->spinBox_icp_normalKSearch->value())));
-	parameters.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(ui_->doubleSpinBox_icp_maxCorrespDistance->value())));
-	parameters.insert(ParametersPair(Parameters::kIcpIterations(), uNumber2Str(ui_->spinBox_icp_iteration->value())));
-	parameters.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())));
-
 	RegistrationIcp registration(parameters);
 	transform = registration.computeTransformation(dataFrom, dataTo, t, 0, 0, &variance);
 	UINFO("Icp time: %f s", timer.ticks());
@@ -3803,6 +3605,7 @@ void DatabaseViewer::refineConstraintVisually()
 
 void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, bool updateGraph)
 {
+	UDEBUG("");
 	if(from == to)
 	{
 		UWARN("Cannot refine link to same node");
@@ -3816,72 +3619,26 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 		return;
 	}
 
-	ParametersMap parameters;
+	ParametersMap parameters = ui_->parameters_toolbox->getParameters();
-	parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
-	parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
-	parameters.insert(ParametersPair(Parameters::kVisInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
-	parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
-	parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
-	parameters.insert(ParametersPair(Parameters::kVisIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
-	parameters.insert(ParametersPair(Parameters::kVisMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
-	parameters.insert(ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
-	parameters.insert(ParametersPair(Parameters::kVisPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
-	parameters.insert(ParametersPair(Parameters::kVisForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
-	parameters.insert(ParametersPair(Parameters::kRegVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
-	parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
-	parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
-	parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
 
 	Transform t;
 	std::string rejectedMsg;
 	float variance = -1.0f;
 	int inliers = -1;
-	if(ui_->groupBox_visual_recomputeFeatures->isChecked())
-	{
-		// create a fake memory to compute transform
-		Memory tmpMemory(parameters);
 
-		// Add sensor data to generate features
+	// Add sensor data to generate features
-		SensorData dataFrom;
+	SensorData dataFrom;
-		dbDriver_->getNodeData(from, dataFrom);
+	dbDriver_->getNodeData(from, dataFrom);
-		dataFrom.uncompressData();
+	dataFrom.uncompressData();
-		SensorData dataTo;
+	SensorData dataTo;
-		dbDriver_->getNodeData(to, dataTo);
+	dbDriver_->getNodeData(to, dataTo);
-		dataTo.uncompressData();
+	dataTo.uncompressData();
-
-		if(from > to)
-		{
-			tmpMemory.update(dataTo);
-			tmpMemory.update(dataFrom);
-		}
-		else
-		{
-			tmpMemory.update(dataFrom);
-			tmpMemory.update(dataTo);
-		}
 
 
-		t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
+	UDEBUG("");
-	}
+	RegistrationVis reg(parameters);
-	else
+	t = reg.computeTransformation2(dataFrom, dataTo, Transform::getIdentity(), &rejectedMsg, &inliers, &variance);
-	{
+	UDEBUG("");
-		std::list<int> ids;
-		ids.push_back(to);
-		ids.push_back(from);
-		std::list<Signature*> signatures;
-		dbDriver_->loadSignatures(ids, signatures);
-
-		if(signatures.size() == 2)
-		{
-			RegistrationVis registration(parameters);
-			t = registration.computeTransformation(*signatures.back(), *signatures.front(), Transform(), &rejectedMsg, &inliers, &variance);
-		}
-		//cleanup
-		for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
-		{
-			delete *iter;
-		}
-	}
 
 	if(!t.isNull())
 	{
@@ -3944,79 +3701,49 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		UASSERT(!containsLink(linksRemoved_, from, to));
 		UASSERT(!containsLink(linksRefined_, from, to));
 
-		ParametersMap parameters;
+		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
-		parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
+		uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
-		parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
+		uInsert(parameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisNNType())));
-		parameters.insert(ParametersPair(Parameters::kVisInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
+		uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
-		parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
+		uInsert(parameters, ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kVisMaxFeatures())));
-		parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
+		uInsert(parameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
-		parameters.insert(ParametersPair(Parameters::kVisIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
+		uInsert(parameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
-		parameters.insert(ParametersPair(Parameters::kVisMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
-		parameters.insert(ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
-		parameters.insert(ParametersPair(Parameters::kVisPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
-		parameters.insert(ParametersPair(Parameters::kVisForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
-		parameters.insert(ParametersPair(Parameters::kRegVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
-		parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
-		parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
-		parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
 
 		Transform t;
 		std::string rejectedMsg;
 		float variance = -1.0f;
 		int inliers = -1;
-		if(ui_->groupBox_visual_recomputeFeatures->isChecked())
+
+		// create a fake memory to compute the transform
+		Memory tmpMemory(parameters);
+
+		// Add sensor data to generate features
+		SensorData dataFrom;
+		dbDriver_->getNodeData(from, dataFrom);
+		dataFrom.uncompressData();
+		SensorData dataTo;
+		dbDriver_->getNodeData(to, dataTo);
+		dataTo.uncompressData();
+
+		if(from > to)
 		{
-			// create a fake memory to compute the transform
+			tmpMemory.update(dataTo);
-			Memory tmpMemory(parameters);
+			tmpMemory.update(dataFrom);
-
-			// Add sensor data to generate features
-			SensorData dataFrom;
-			dbDriver_->getNodeData(from, dataFrom);
-			dataFrom.uncompressData();
-			SensorData dataTo;
-			dbDriver_->getNodeData(to, dataTo);
-			dataTo.uncompressData();
-
-			if(from > to)
-			{
-				tmpMemory.update(dataTo);
-				tmpMemory.update(dataFrom);
-			}
-			else
-			{
-				tmpMemory.update(dataFrom);
-				tmpMemory.update(dataTo);
-			}
-
-
-			t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
-
-			if(!silent)
-			{
-				ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depthRaw());
-				ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depthRaw());
-				updateWordsMatching();
-			}
 		}
 		else
 		{
-			std::list<int> ids;
+			tmpMemory.update(dataFrom);
-			ids.push_back(to);
+			tmpMemory.update(dataTo);
-			ids.push_back(from);
+		}
-			std::list<Signature*> signatures;
-			dbDriver_->loadSignatures(ids, signatures);
 
-			if(signatures.size() == 2)
-			{
-				RegistrationVis registration(parameters);
 
-				t = registration.computeTransformation(*signatures.back(), *signatures.front(), Transform(), &rejectedMsg, &inliers, &variance);
+		t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
-			}
+
-			//cleanup
+		if(!silent)
-			for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
-			{
+			ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depthRaw());
-				delete *iter;
+			ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depthRaw());
-			}
+			updateWordsMatching();
 		}
 
 		if(!t.isNull())
@@ -4264,4 +3991,36 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
 	}
 }
 
+void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
+{
+	bool updateStereo = false;
+	bool updateGraphView = false;
+	for(QStringList::const_iterator iter=parametersChanged.constBegin();
+	   iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
+	   ++iter)
+	{
+		QString group = iter->split('/').first();
+		if(!updateStereo && group == "Stereo")
+		{
+			updateStereo = true;
+			continue;
+		}
+		if(!updateGraphView && group == "Optimize")
+		{
+			updateGraphView = true;
+			continue;
+		}
+	}
+
+	if(updateStereo)
+	{
+		this->updateStereo();
+	}
+	if(updateGraphView)
+	{
+		this->updateGraphView();
+	}
+	this->configModified();
+}
+
 } // namespace rtabmap

guilib/src/MainWindow.cpp

@@ -145,7 +145,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_waypointsIndex(0),
 	_odometryCorrection(Transform::getIdentity()),
 	_processingOdometry(false),
-	_lastOdomInfoUpdateTime(0),
 	_oneSecondTimer(0),
 	_elapsedTime(0),
 	_posteriorCurve(0),
@@ -509,6 +508,7 @@ MainWindow::~MainWindow()
 	this->stopDetection();
 	delete _ui;
 	delete _elapsedTime;
+	UDEBUG("");
 }
 
 void MainWindow::setupMainLayout(bool vertical)
@@ -698,22 +698,19 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	}
 	else if(anEvent->getClassName().compare("OdometryEvent") == 0)
 	{
-		// limit 10 Hz max
+		OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
-		if(UTimer::now() - _lastOdomInfoUpdateTime > 0.1)
+		if(!_processingOdometry && !_processingStatistics)
 		{
-			_lastOdomInfoUpdateTime = UTimer::now();
+			_processingOdometry = true; // if we receive too many odometry events!
-			OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
+			emit odometryReceived(*odomEvent);
-			if(!_processingOdometry && !_processingStatistics)
+		}
-			{
+		else
-				_processingOdometry = true; // if we receive too many odometry events!
+		{
-				emit odometryReceived(*odomEvent);
+			// we receive too many odometry events! just send without data
-			}
+			SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
-			else
+			data.setGroundTruth(odomEvent->data().groundTruth());
-			{
+			OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info());
-				// we receive too many odometry events! just send without data
+			emit odometryReceived(tmp);
-				OdometryEvent tmp(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->pose(), odomEvent->covariance(), odomEvent->info());
-				emit odometryReceived(tmp);
-			}
 		}
 	}
 	else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -1034,12 +1031,45 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 	if(!odom.info().transformFiltered.isNull())
 	{
 		odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-		_ui->statsToolBox->updateStat("Odometry/Fx/m", (float)odom.data().id(), x);
+		_ui->statsToolBox->updateStat("Odometry/TFx/m", (float)odom.data().id(), x);
-		_ui->statsToolBox->updateStat("Odometry/Fy/m", (float)odom.data().id(), y);
+		_ui->statsToolBox->updateStat("Odometry/TFy/m", (float)odom.data().id(), y);
-		_ui->statsToolBox->updateStat("Odometry/Fz/m", (float)odom.data().id(), z);
+		_ui->statsToolBox->updateStat("Odometry/TFz/m", (float)odom.data().id(), z);
-		_ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/TFroll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/TFpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/TFyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
+	}
+
+	if(!odom.info().transformGroundTruth.isNull())
+	{
+		odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		_ui->statsToolBox->updateStat("Odometry/TGx/m", (float)odom.data().id(), x);
+		_ui->statsToolBox->updateStat("Odometry/TGy/m", (float)odom.data().id(), y);
+		_ui->statsToolBox->updateStat("Odometry/TGz/m", (float)odom.data().id(), z);
+		_ui->statsToolBox->updateStat("Odometry/TGroll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/TGpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/TGyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
+	}
+
+	//cumulative pose
+	if(!odom.pose().isNull())
+	{
+		odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		_ui->statsToolBox->updateStat("Odometry/Px/m", (float)odom.data().id(), x);
+		_ui->statsToolBox->updateStat("Odometry/Py/m", (float)odom.data().id(), y);
+		_ui->statsToolBox->updateStat("Odometry/Pz/m", (float)odom.data().id(), z);
+		_ui->statsToolBox->updateStat("Odometry/Proll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Ppitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Pyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
+	}
+	if(!odom.data().groundTruth().isNull())
+	{
+		odom.data().groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		_ui->statsToolBox->updateStat("Odometry/PGx/m", (float)odom.data().id(), x);
+		_ui->statsToolBox->updateStat("Odometry/PGy/m", (float)odom.data().id(), y);
+		_ui->statsToolBox->updateStat("Odometry/PGz/m", (float)odom.data().id(), z);
+		_ui->statsToolBox->updateStat("Odometry/PGroll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/PGpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/PGyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
 	}
 
 	if(odom.info().interval > 0)
@@ -2826,7 +2856,7 @@ bool MainWindow::closeDatabase()
 		}
 	}
 
-	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose));
+	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose, !_newDatabasePathOutput.isEmpty()));
 	return true;
 }
 

guilib/src/ParametersToolBox.cpp

@@ -0,0 +1,516 @@
+/*
+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.
+*/
+
+//
+// Original version from Find-Object: https://github.com/introlab/find-object
+//
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Optimizer.h>
+
+#include "ParametersToolBox.h"
+#include <QComboBox>
+#include <QDoubleSpinBox>
+#include <QLineEdit>
+#include <QLabel>
+#include <QGroupBox>
+#include <QCheckBox>
+#include <QVBoxLayout>
+#include <QMessageBox>
+#include <stdio.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <opencv2/opencv_modules.hpp>
+
+namespace rtabmap {
+
+ParametersToolBox::ParametersToolBox(QWidget *parent) :
+	QToolBox(parent),
+	parameters_(Parameters::getDefaultParameters())
+{
+}
+
+ParametersToolBox::~ParametersToolBox()
+{
+}
+
+QWidget * ParametersToolBox::getParameterWidget(const QString & key)
+{
+	return this->findChild<QWidget*>(key);
+}
+
+QStringList ParametersToolBox::resetPage(int index)
+{
+	QStringList paramChanged;
+	const QObjectList & children = this->widget(index)->children();
+	for(int j=0; j<children.size();++j)
+	{
+		QString key = children.at(j)->objectName();
+		// ignore working memory
+		QString group = key.split("/").first();
+		if(!ignoredGroups_.contains(group))
+		{
+			std::string value = Parameters::getDefaultParameters().at(key.toStdString());
+			parameters_.at(key.toStdString()) = value;
+
+			if(qobject_cast<QComboBox*>(children.at(j)))
+			{
+				if(((QComboBox*)children.at(j))->currentIndex() != QString::fromStdString(value).split(':').first().toInt())
+				{
+					((QComboBox*)children.at(j))->setCurrentIndex(QString::fromStdString(value).split(':').first().toInt());
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QSpinBox*>(children.at(j)))
+			{
+				if(((QSpinBox*)children.at(j))->value() != uStr2Int(value))
+				{
+					((QSpinBox*)children.at(j))->setValue(uStr2Int(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QDoubleSpinBox*>(children.at(j)))
+			{
+				if(((QDoubleSpinBox*)children.at(j))->value() != uStr2Double(value))
+				{
+					((QDoubleSpinBox*)children.at(j))->setValue(uStr2Double(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QCheckBox*>(children.at(j)))
+			{
+				if(((QCheckBox*)children.at(j))->isChecked() != uStr2Bool(value))
+				{
+					((QCheckBox*)children.at(j))->setChecked(uStr2Bool(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QLineEdit*>(children.at(j)))
+			{
+				if(((QLineEdit*)children.at(j))->text().compare(QString::fromStdString(value)) != 0)
+				{
+					((QLineEdit*)children.at(j))->setText(QString::fromStdString(value));
+					paramChanged.append(key);
+				}
+			}
+		}
+	}
+	return paramChanged;
+}
+
+void ParametersToolBox::resetCurrentPage()
+{
+	this->blockSignals(true);
+	QStringList paramChanged = this->resetPage(this->currentIndex());
+	this->blockSignals(false);
+	Q_EMIT parametersChanged(paramChanged);
+}
+
+void ParametersToolBox::resetAllPages()
+{
+	QStringList paramChanged;
+	this->blockSignals(true);
+	for(int i=0; i< this->count(); ++i)
+	{
+		paramChanged.append(this->resetPage(i));
+	}
+	this->blockSignals(false);
+	Q_EMIT parametersChanged(paramChanged);
+}
+
+void ParametersToolBox::updateParametersVisibility()
+{
+	//show/hide not used parameters
+	/*QComboBox * descriptorBox = this->findChild<QComboBox*>(Parameters::kFeature2D_2Descriptor());
+	QComboBox * detectorBox = this->findChild<QComboBox*>(Parameters::kFeature2D_1Detector());
+	if(descriptorBox && detectorBox)
+	{
+		QString group = Parameters::kFeature2D_2Descriptor().split('/').first();
+		QWidget * panel = 0;
+		for(int i=0; i<this->count(); ++i)
+		{
+			if(this->widget(i)->objectName().compare(group) == 0)
+			{
+				panel = this->widget(i);
+				break;
+			}
+		}
+		if(panel)
+		{
+			const QObjectList & objects = panel->children();
+			QString descriptorName = descriptorBox->currentText();
+			QString detectorName = detectorBox->currentText();
+
+			for(int i=0; i<objects.size(); ++i)
+			{
+				if(!objects[i]->objectName().isEmpty())
+				{
+					if(objects[i]->objectName().contains(descriptorName) || objects[i]->objectName().contains(detectorName))
+					{
+						((QWidget*)objects[i])->setVisible(true);
+					}
+					else if(objects[i]->objectName().contains("Fast") && detectorName == QString("ORB"))
+					{
+						((QWidget*)objects[i])->setVisible(true);	// ORB uses some FAST parameters
+					}
+					else if(!objects[i]->objectName().split('/').at(1).at(0).isDigit())
+					{
+						((QWidget*)objects[i])->setVisible(false);
+					}
+				}
+			}
+		}
+	}*/
+}
+
+void ParametersToolBox::setupUi(const QSet<QString> & ignoredGroups)
+{
+	ignoredGroups_ = ignoredGroups;
+	this->removeItem(0); // remove dummy page used in .ui
+	QWidget * currentItem = 0;
+	const ParametersMap & parameters = Parameters::getDefaultParameters();
+	for(ParametersMap::const_iterator iter=parameters.begin();
+			iter!=parameters.end();
+			++iter)
+	{
+		QStringList splitted = QString::fromStdString(iter->first).split('/');
+		QString group = splitted.first();
+		if(!ignoredGroups_.contains(group))
+		{
+			QString name = splitted.last();
+			if(currentItem == 0 || currentItem->objectName().compare(group) != 0)
+			{
+				currentItem = new QWidget(this);
+				this->addItem(currentItem, group);
+				currentItem->setObjectName(group);
+				QVBoxLayout * layout = new QVBoxLayout(currentItem);
+				currentItem->setLayout(layout);
+				layout->setContentsMargins(0,0,0,0);
+				layout->setSpacing(0);
+				layout->addSpacerItem(new QSpacerItem(0,0, QSizePolicy::Minimum, QSizePolicy::Expanding));
+
+				addParameter(layout, iter->first, iter->second);
+			}
+			else
+			{
+				addParameter((QVBoxLayout*)currentItem->layout(), iter->first, iter->second);
+			}
+		}
+	}
+
+	updateParametersVisibility();
+}
+
+void ParametersToolBox::updateParameter(const std::string & key, const std::string & value)
+{
+	QString group = QString::fromStdString(key).split("/").first();
+	if(!ignoredGroups_.contains(group))
+	{
+		QWidget * widget = this->findChild<QWidget*>(key.c_str());
+		QString type = QString::fromStdString(Parameters::getType(key));
+		if(type.compare("string") == 0)
+		{
+			QString valueQt = QString::fromStdString(value);
+			if(valueQt.contains(';'))
+			{
+				// It's a list, just change the index
+				QStringList splitted = valueQt.split(':');
+				((QComboBox*)widget)->setCurrentIndex(splitted.first().toInt());
+			}
+			else
+			{
+				((QLineEdit*)widget)->setText(valueQt);
+			}
+		}
+		else if(type.compare("int") == 0)
+		{
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
+		}
+		else if(type.compare("uint") == 0)
+		{
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
+		}
+		else if(type.compare("double") == 0)
+		{
+			((QDoubleSpinBox*)widget)->setValue(uStr2Double(value));
+		}
+		else if(type.compare("float") == 0)
+		{
+			((QDoubleSpinBox*)widget)->setValue(uStr2Float(value));
+		}
+		else if(type.compare("bool") == 0)
+		{
+			((QCheckBox*)widget)->setChecked(uStr2Bool(value));
+		}
+	}
+}
+
+void ParametersToolBox::addParameter(
+		QVBoxLayout * layout,
+		const std::string & key,
+		const std::string & value)
+{
+	std::string type = Parameters::getType(key);
+	if(type.compare("string") == 0)
+	{
+		addParameter(layout, key.c_str(), QString::fromStdString(value));
+	}
+	else if(type.compare("int") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Int(value));
+	}
+	else if(type.compare("uint") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Int(value));
+	}
+	else if(type.compare("double") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Double(value));
+	}
+	else if(type.compare("float") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Double(value));
+	}
+	else if(type.compare("bool") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Bool(value));
+	}
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const QString & value)
+{
+	if(value.contains(';'))
+	{
+		QComboBox * widget = new QComboBox(this);
+		widget->setObjectName(key);
+		QStringList splitted = value.split(':');
+		widget->addItems(splitted.last().split(';'));
+
+		widget->setCurrentIndex(splitted.first().toInt());
+		connect(widget, SIGNAL(currentIndexChanged(int)), this, SLOT(changeParameter(int)));
+		addParameter(layout, key, widget);
+	}
+	else
+	{
+		QLineEdit * widget = new QLineEdit(value, this);
+		widget->setObjectName(key);
+		connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+		addParameter(layout, key, widget);
+	}
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const double & value)
+{
+	QDoubleSpinBox * widget = new QDoubleSpinBox(this);
+	double def = uStr2Double(Parameters::getDefaultParameters().at(key.toStdString()));
+	if(def<0.01)
+	{
+		widget->setDecimals(4);
+	}
+	else if(def<0.1)
+	{
+		widget->setDecimals(3);
+	}
+
+	if(def>=0.0)
+	{
+		widget->setMaximum(def*1000000.0);
+	}
+	else if(def==0.0)
+	{
+		widget->setMaximum(1000000.0);
+	}
+	else if(def<0.0)
+	{
+		widget->setMinimum(def*1000000.0);
+		widget->setMaximum(0.0);
+	}
+	widget->setValue(value);
+	widget->setObjectName(key);
+	connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const int & value)
+{
+	QSpinBox * widget = new QSpinBox(this);
+	int def = uStr2Int(Parameters::getDefaultParameters().at(key.toStdString()));
+
+	if(def>0)
+	{
+		widget->setMaximum(def*1000000);
+	}
+	else if(def == 0)
+	{
+		widget->setMaximum(1000000);
+	}
+	else if(def<0)
+	{
+		widget->setMinimum(def*1000000);
+		widget->setMaximum(0);
+	}
+	widget->setValue(value);
+	widget->setObjectName(key);
+
+	if(key.compare(Parameters::kVisFeatureType().c_str()) == 0)
+	{
+		if(RTABMAP_NONFREE == 0)
+		{
+			if(value <= 1)
+			{
+				UWARN("SURF/SIFT not available, setting feature default to FAST/BRIEF.");
+				widget->setValue(4);
+			}
+		}
+	}
+	if(key.compare(Parameters::kOptimizerStrategy().c_str()) == 0)
+	{
+		if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			if(value == 1)
+			{
+				UWARN("g2o is not available, setting optimization default to TORO.");
+				widget->setValue(0);
+			}
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		{
+			if(value == 2)
+			{
+				UWARN("GTSAM is not available, setting optimization default to TORO.");
+				widget->setValue(0);
+			}
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		{
+			widget->setEnabled(false);
+		}
+	}
+
+	connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const bool & value)
+{
+	QCheckBox * widget = new QCheckBox(this);
+	widget->setChecked(value);
+	widget->setObjectName(key);
+	connect(widget, SIGNAL(stateChanged(int)), this, SLOT(changeParameter(int)));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout, const QString & key, QWidget * widget)
+{
+	QHBoxLayout * hLayout = new QHBoxLayout();
+	layout->insertLayout(layout->count()-1, hLayout);
+	QString tmp = key.split('/').last();
+	if(tmp.at(0).isDigit())
+	{
+		tmp.remove(0,1);
+	}
+	QLabel * label = new QLabel(tmp, this);
+	label->setObjectName(key+"/label");
+	label->setToolTip(QString("<FONT>%1</FONT>").arg(Parameters::getDescription(key.toStdString()).c_str()));
+	label->setTextInteractionFlags(Qt::TextSelectableByMouse);
+	hLayout->addWidget(label);
+	hLayout->addWidget(widget);
+}
+
+void ParametersToolBox::changeParameter(const QString & value)
+{
+	if(sender())
+	{
+		parameters_.at(sender()->objectName().toStdString()) = value.toStdString();
+		QStringList paramChanged;
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+void ParametersToolBox::changeParameter()
+{
+	if(sender())
+	{
+		QDoubleSpinBox * doubleSpinBox = qobject_cast<QDoubleSpinBox*>(sender());
+		QSpinBox * spinBox = qobject_cast<QSpinBox*>(sender());
+		QLineEdit * lineEdit = qobject_cast<QLineEdit*>(sender());
+		if(doubleSpinBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uNumber2Str(doubleSpinBox->value());
+		}
+		else if(spinBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uNumber2Str(spinBox->value());
+		}
+		else if(lineEdit)
+		{
+			parameters_.at(sender()->objectName().toStdString()) =  lineEdit->text().toStdString();
+		}
+		QStringList paramChanged;
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+
+void ParametersToolBox::changeParameter(const int & value)
+{
+	if(sender())
+	{
+		QStringList paramChanged;
+		QComboBox * comboBox = qobject_cast<QComboBox*>(sender());
+		QCheckBox * checkBox = qobject_cast<QCheckBox*>(sender());
+		if(comboBox)
+		{
+			QStringList items;
+			for(int i=0; i<comboBox->count(); ++i)
+			{
+				items.append(comboBox->itemText(i));
+			}
+			QString merged = QString::number(value) + QString(":") + items.join(";");
+			parameters_.at(sender()->objectName().toStdString()) = merged.toStdString();
+
+			this->updateParametersVisibility();
+		}
+		else if(checkBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uBool2Str(value==Qt::Checked?true:false);
+		}
+
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+
+} // namespace find_object

guilib/src/ParametersToolBox.h

@@ -0,0 +1,86 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+//
+// Original version from Find-Object: https://github.com/introlab/find-object
+//
+
+#ifndef PARAMETERSTOOLBOX_H_
+#define PARAMETERSTOOLBOX_H_
+
+#include <rtabmap/core/Parameters.h>
+#include <QToolBox>
+#include <QSet>
+
+class QVBoxLayout;
+class QAbstractButton;
+
+namespace rtabmap {
+
+class ParametersToolBox: public QToolBox
+{
+	Q_OBJECT
+
+public:
+	ParametersToolBox(QWidget *parent = 0);
+	virtual ~ParametersToolBox();
+
+	void setupUi(const QSet<QString> & ignoredGroups);
+	QWidget * getParameterWidget(const QString & key);
+	void updateParameter(const std::string & key, const std::string & value);
+	const ParametersMap & getParameters() const {return parameters_;}
+
+private:
+	void addParameter(QVBoxLayout * layout, const std::string & key, const std::string & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const QString & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const int & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const double & value);
+	void addParameter(QVBoxLayout * layout, const QString & key, const bool & value);
+	void addParameter(QVBoxLayout * layout, const QString & name, QWidget * widget);
+
+Q_SIGNALS:
+	void parametersChanged(const QStringList & name);
+
+private Q_SLOTS:
+	void changeParameter();
+	void changeParameter(const QString & value);
+	void changeParameter(const int & value);
+	void resetCurrentPage();
+	void resetAllPages();
+
+private:
+	QStringList resetPage(int index);
+	void updateParametersVisibility();
+
+private:
+	ParametersMap parameters_;
+	QSet<QString> ignoredGroups_;
+};
+
+} // namespace find_object
+
+#endif /* PARAMETERSTOOLBOX_H_ */

guilib/src/PreferencesDialog.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QTimer>
 
 #include <QFileDialog>
+#include <QInputDialog>
 #include <QMessageBox>
 #include <QtGui/QStandardItemModel>
 #include <QMainWindow>
@@ -390,16 +391,24 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_cameraRGBDImages_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_cameraRGBDImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathGt()));
 	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_RGBDImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_cameraRGBDImages_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_cameraRGBDImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraRGBDImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraRGBDImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_cameraRGBDImages_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()));
@@ -407,6 +416,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	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()));
@@ -1113,7 +1124,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_sourceLocalTransform->setText("0 0 1 -1 0 0 0 -1 0");
 
 		_ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice);
-		_ui->source_images_spinBox_startPos->setValue(1);
+		_ui->source_images_spinBox_startPos->setValue(0);
 		_ui->source_images_refreshDir->setChecked(false);
 		_ui->checkBox_rgbImages_rectify->setChecked(false);
 		_ui->checkBox_rgbVideo_rectify->setChecked(false);
@@ -1171,9 +1182,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_openni2OniPath->clear();
 		_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
 		_ui->lineEdit_cameraRGBDImages_path_depth->setText("");
-		_ui->checkBox_RGBDImages_timestamps->setChecked(false);
+		_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("");
@@ -1186,6 +1202,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_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);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
 	{
@@ -1461,9 +1479,14 @@ 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_RGBDImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_RGBDImages_timestamps->isChecked()).toBool());
+	_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");
@@ -1475,6 +1498,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_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");
@@ -1802,9 +1827,14 @@ 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_RGBDImages_timestamps->isChecked());
+	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");
@@ -1816,6 +1846,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	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");
@@ -2434,6 +2466,21 @@ void PreferencesDialog::selectSourceRGBDImagesPathRGB()
 	}
 }
 
+
+void PreferencesDialog::selectSourceRGBDImagesPathScans()
+{
+	QString dir = _ui->lineEdit_cameraRGBDImages_path_scans->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getExistingDirectory(this, tr("Select scans directory"), dir);
+	if(path.size())
+	{
+		_ui->lineEdit_cameraRGBDImages_path_scans->setText(path);
+	}
+}
+
 void PreferencesDialog::selectSourceRGBDImagesPathDepth()
 {
 	QString dir = _ui->lineEdit_cameraRGBDImages_path_depth->text();
@@ -2448,6 +2495,30 @@ void PreferencesDialog::selectSourceRGBDImagesPathDepth()
 	}
 }
 
+void PreferencesDialog::selectSourceRGBDImagesPathGt()
+{
+	QString dir = _ui->lineEdit_cameraRGBDImages_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_cameraRGBDImages_gtFormat->count(); ++i)
+		{
+			list.push_back(_ui->comboBox_cameraRGBDImages_gtFormat->itemText(i));
+		}
+		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list);
+		if(!item.isEmpty())
+		{
+			_ui->lineEdit_cameraRGBDImages_gt->setText(path);
+			_ui->comboBox_cameraRGBDImages_gtFormat->setCurrentIndex(_ui->comboBox_cameraRGBDImages_gtFormat->findText(item));
+		}
+	}
+}
+
 void PreferencesDialog::selectSourceStereoImagesStamps()
 {
 	QString dir = _ui->lineEdit_cameraStereoImages_timestamps->text();
@@ -2504,6 +2575,30 @@ void PreferencesDialog::selectSourceStereoImagesPathScans()
 	}
 }
 
+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();
@@ -2515,7 +2610,7 @@ void PreferencesDialog::selectSourceImagesPath()
 	if(!path.isEmpty())
 	{
 		_ui->source_images_lineEdit_path->setText(path);
-		_ui->source_images_spinBox_startPos->setValue(1);
+		_ui->source_images_spinBox_startPos->setValue(0);
 	}
 }
 
@@ -3596,6 +3691,16 @@ Transform PreferencesDialog::getStereoLaserLocalTransform() const
 	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();
+	}
+	return t;
+}
+
 QString PreferencesDialog::getSourceDatabasePath() const
 {
 	return _ui->source_database_lineEdit_path->text();
@@ -3721,10 +3826,14 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			_ui->lineEdit_cameraRGBDImages_path_rgb->text().append(QDir::separator()).toStdString(),
 			_ui->lineEdit_cameraRGBDImages_path_depth->text().append(QDir::separator()).toStdString(),
 			_ui->doubleSpinBox_cameraRGBDImages_scale->value(),
-			_ui->checkBox_RGBDImages_timestamps->isChecked(),
-			_ui->lineEdit_cameraRGBDImages_timestamps->text().toStdString(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
+		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraRGBDImages_gt->text().toStdString(), _ui->comboBox_cameraRGBDImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setScanPath(
+						_ui->lineEdit_cameraRGBDImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraRGBDImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->spinBox_cameraRGBDImages_max_scan_pts->value(),
+						this->getRGBDLaserLocalTransform());
+		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraRGBDImages_timestamps->isChecked(), _ui->lineEdit_cameraRGBDImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcDC1394)
 	{
@@ -3751,16 +3860,17 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	else if(driver == kSrcStereoImages)
 	{
 		camera = new CameraStereoImages(
-			_ui->lineEdit_cameraStereoImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraStereoImages_path_scans->text().append(QDir::separator()).toStdString(),
-			this->getStereoLaserLocalTransform(),
-			_ui->spinBox_cameraStereoImages_max_scan_pts->value(),
 			_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
 			_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
-			_ui->checkBox_stereoImages_timestamps->isChecked(),
-			_ui->lineEdit_cameraStereoImages_timestamps->text().toStdString(),
 			_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)->setScanPath(
+						_ui->lineEdit_cameraStereoImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraStereoImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->spinBox_cameraStereoImages_max_scan_pts->value(),
+						this->getStereoLaserLocalTransform());
+		((CameraStereoImages*)camera)->setTimestamps(_ui->checkBox_stereoImages_timestamps->isChecked(), _ui->lineEdit_cameraStereoImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcStereoVideo)
 	{
@@ -3789,12 +3899,12 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	{
 		camera = new CameraImages(
 			_ui->source_images_lineEdit_path->text().toStdString(),
-			_ui->source_images_spinBox_startPos->value(),
-			_ui->source_images_refreshDir->isChecked(),
-			_ui->checkBox_rgbImages_rectify->isChecked(),
-			false,
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
+
+		((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());
 	}
 	else if(driver == kSrcDatabase)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-762</y>
             <width>633</width>
             <height>2118</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>17</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1673,6 +1673,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       </item>
                       <item row="3" column="0">
                        <widget class="QLineEdit" name="lineEdit_sourceLocalTransform">
+                        <property name="toolTip">
+                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt; Format (3 values): x y z&lt;/p&gt;&lt;p&gt; Format (6 values): x y z roll pitch yaw&lt;/p&gt;&lt;p&gt; Format (7 values): x y z qx qy qz qw&lt;/p&gt;&lt;p&gt; Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;/p&gt;&lt;p&gt; Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                        </property>
                         <property name="text">
                          <string>0 0 1 -1 0 0 0 -1 0</string>
                         </property>
@@ -1681,7 +1684,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <item row="3" column="1">
                        <widget class="QLabel" name="label_42">
                         <property name="text">
-                         <string>Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz].</string>
+                         <string>Local transform from /base_link to /camera_link. Mouse over the box to show formats.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -1963,7 +1966,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>2</number>
+                              <number>6</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -2408,7 +2411,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="title">
                                   <string>RGB-D Images</string>
                                  </property>
-                                 <layout class="QGridLayout" name="gridLayout_67" columnstretch="0,0,1">
+                                 <layout class="QGridLayout" name="gridLayout_35" columnstretch="0,0,1">
                                   <item row="0" column="2">
                                    <widget class="QLabel" name="label_252">
                                     <property name="text">
@@ -2422,10 +2425,24 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="4" column="2">
+                                  <item row="0" column="0">
-                                   <widget class="QLabel" name="label_251">
+                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_path_rgb">
                                     <property name="text">
-                                     <string>Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use RGB file names as timestamps&quot; above is checked. </string>
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_timestamps">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="2">
+                                   <widget class="QLabel" name="label_257">
+                                    <property name="text">
+                                     <string>Depth scale factor (depth = pixel value / factor).</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -2442,40 +2459,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="1" column="1">
-                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_path_depth">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="3" column="1">
-                                   <widget class="QCheckBox" name="checkBox_RGBDImages_timestamps">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="3" column="2">
-                                   <widget class="QLabel" name="label_255">
-                                    <property name="text">
-                                     <string>Use RGB file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="textInteractionFlags">
-                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="4" column="1">
-                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_timestamps">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
                                   <item row="0" column="1">
                                    <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_path_rgb">
                                     <property name="text">
@@ -2483,30 +2466,24 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="0">
+                                  <item row="1" column="1">
-                                   <spacer name="verticalSpacer_40">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_path_depth">
-                                    <property name="orientation">
+                                    <property name="text">
-                                     <enum>Qt::Vertical</enum>
+                                     <string/>
                                     </property>
-                                    <property name="sizeHint" stdset="0">
+                                   </widget>
-                                     <size>
-                                      <width>20</width>
-                                      <height>0</height>
-                                     </size>
-                                    </property>
-                                   </spacer>
                                   </item>
-                                  <item row="0" column="0">
+                                  <item row="7" column="0">
-                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_path_rgb">
+                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_path_scans">
                                     <property name="text">
                                      <string>...</string>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="4" column="0">
+                                  <item row="3" column="1">
-                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_timestamps">
+                                   <widget class="QCheckBox" name="checkBox_cameraRGBDImages_timestamps">
                                     <property name="text">
-                                     <string>...</string>
+                                     <string/>
                                     </property>
                                    </widget>
                                   </item>
@@ -2523,19 +2500,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="2">
-                                   <widget class="QLabel" name="label_257">
-                                    <property name="text">
-                                     <string>Depth scale factor (depth = pixel value / factor).</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="textInteractionFlags">
-                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                    </property>
-                                   </widget>
-                                  </item>
                                   <item row="2" column="1">
                                    <widget class="QDoubleSpinBox" name="doubleSpinBox_cameraRGBDImages_scale">
                                     <property name="decimals">
@@ -2549,6 +2513,179 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="4" column="2">
+                                   <widget class="QLabel" name="label_251">
+                                    <property name="text">
+                                     <string>Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use RGB file names as timestamps&quot; above is checked. </string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="2">
+                                   <widget class="QLabel" name="label_288">
+                                    <property name="text">
+                                     <string>Optional ground truth file. Select the correct format below.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="3" column="2">
+                                   <widget class="QLabel" name="label_255">
+                                    <property name="text">
+                                     <string>Use RGB file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="6" column="2">
+                                   <widget class="QLabel" name="label_289">
+                                    <property name="text">
+                                     <string>Ground truth format.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_gt">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_timestamps">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="7" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_path_scans">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_gt">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="7" column="2">
+                                   <widget class="QLabel" name="label_293">
+                                    <property name="text">
+                                     <string>Path to directory containing optional laser scans. The directory should have the same size has the left images directory. </string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="6" column="1">
+                                   <widget class="QComboBox" name="comboBox_cameraRGBDImages_gtFormat">
+                                    <item>
+                                     <property name="text">
+                                      <string>Raw</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>RGBD-SLAM</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>KITTI</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>TORO</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
+                                  <item row="9" column="2">
+                                   <widget class="QLabel" name="label_292">
+                                    <property name="text">
+                                     <string>Maximum laser scan points.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="9" column="1">
+                                   <widget class="QSpinBox" name="spinBox_cameraRGBDImages_max_scan_pts">
+                                    <property name="maximum">
+                                     <number>99999999</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="8" column="2">
+                                   <widget class="QLabel" name="label_294">
+                                    <property name="text">
+                                     <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="8" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_laser_transform">
+                                    <property name="toolTip">
+                                     <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt; Format (3 values): x y z&lt;/p&gt;&lt;p&gt; Format (6 values): x y z roll pitch yaw&lt;/p&gt;&lt;p&gt; Format (7 values): x y z qx qy qz qw&lt;/p&gt;&lt;p&gt; Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;/p&gt;&lt;p&gt; Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                                    </property>
+                                    <property name="text">
+                                     <string>0 0 0 0 0 0</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="10" column="1">
+                                   <spacer name="verticalSpacer_40">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -2646,7 +2783,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>3</number>
+                              <number>2</number>
                              </property>
                              <widget class="QWidget" name="page_49"/>
                              <widget class="QWidget" name="page_48"/>
@@ -2671,7 +2808,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="8" column="0">
+                                  <item row="10" column="0">
                                    <spacer name="verticalSpacer_38">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -2684,7 +2821,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </spacer>
                                   </item>
-                                  <item row="3" column="2">
+                                  <item row="4" column="2">
                                    <widget class="QLabel" name="label_248">
                                     <property name="text">
                                      <string>Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use left file names as timestamps&quot; above is checked.  </string>
@@ -2697,6 +2834,32 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="5" column="2">
+                                   <widget class="QLabel" name="label_290">
+                                    <property name="text">
+                                     <string>Optional ground truth file. Select the correct format below.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="2">
+                                   <widget class="QLabel" name="label_250">
+                                    <property name="text">
+                                     <string>Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                   <item row="0" column="2">
                                    <widget class="QLabel" name="label_249">
                                     <property name="text">
@@ -2710,21 +2873,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="0">
+                                  <item row="7" column="0">
                                    <widget class="QToolButton" name="toolButton_cameraStereoImages_path_scans">
                                     <property name="text">
                                      <string>...</string>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="1">
+                                  <item row="4" column="1">
                                    <widget class="QLineEdit" name="lineEdit_cameraStereoImages_timestamps">
                                     <property name="text">
                                      <string/>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="0">
+                                  <item row="4" column="0">
                                    <widget class="QToolButton" name="toolButton_cameraStereoImages_timestamps">
                                     <property name="text">
                                      <string>...</string>
@@ -2738,26 +2901,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="4" column="2">
-                                   <widget class="QLabel" name="label_250">
-                                    <property name="text">
-                                     <string>Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="textInteractionFlags">
-                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="4" column="1">
-                                   <widget class="QCheckBox" name="checkBox_stereoImages_rectify">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
                                   <item row="1" column="2">
                                    <widget class="QLabel" name="label_253">
                                     <property name="text">
@@ -2771,24 +2914,27 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="1">
+                                  <item row="7" column="1">
                                    <widget class="QLineEdit" name="lineEdit_cameraStereoImages_path_scans">
                                     <property name="text">
                                      <string/>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="6" column="1">
+                                  <item row="8" column="1">
                                    <widget class="QLineEdit" name="lineEdit_cameraStereoImages_laser_transform">
+                                    <property name="toolTip">
+                                     <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt; Format (3 values): x y z&lt;/p&gt;&lt;p&gt; Format (6 values): x y z roll pitch yaw&lt;/p&gt;&lt;p&gt; Format (7 values): x y z qx qy qz qw&lt;/p&gt;&lt;p&gt; Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;/p&gt;&lt;p&gt; Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                                    </property>
                                     <property name="text">
                                      <string>0 0 0 0 0 0</string>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="6" column="2">
+                                  <item row="8" column="2">
                                    <widget class="QLabel" name="label_272">
                                     <property name="text">
-                                     <string>Local transform from /base_link to /scan_link. Format (6 values): x y z roll pitch yaw. Format (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz].</string>
+                                     <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -2812,14 +2958,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="1">
+                                  <item row="3" column="1">
                                    <widget class="QCheckBox" name="checkBox_stereoImages_timestamps">
                                     <property name="text">
                                      <string/>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="2">
+                                  <item row="3" column="2">
                                    <widget class="QLabel" name="label_256">
                                     <property name="text">
                                      <string>Use left file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
@@ -2832,7 +2978,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="2">
+                                  <item row="7" column="2">
                                    <widget class="QLabel" name="label_265">
                                     <property name="text">
                                      <string>Path to directory containing optional laser scans. The directory should have the same size has the left images directory. </string>
@@ -2845,14 +2991,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="7" column="1">
+                                  <item row="9" column="1">
                                    <widget class="QSpinBox" name="spinBox_cameraStereoImages_max_scan_pts">
                                     <property name="maximum">
                                      <number>99999999</number>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="7" column="2">
+                                  <item row="9" column="2">
                                    <widget class="QLabel" name="label_274">
                                     <property name="text">
                                      <string>Maximum laser scan points.</string>
@@ -2865,6 +3011,64 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="2" column="1">
+                                   <widget class="QCheckBox" name="checkBox_stereoImages_rectify">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="6" column="2">
+                                   <widget class="QLabel" name="label_291">
+                                    <property name="text">
+                                     <string>Ground truth format.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraStereoImages_gt">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraStereoImages_gt">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="6" column="1">
+                                   <widget class="QComboBox" name="comboBox_cameraStereoImages_gtFormat">
+                                    <item>
+                                     <property name="text">
+                                      <string>Raw</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>RGBD-SLAM</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>KITTI</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>TORO</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>

guilib/src/utilite/UPlot.cpp

@@ -1016,6 +1016,18 @@ void UPlotCurve::getData(QVector<float> & x, QVector<float> & y) const
 	}
 }
 
+void UPlotCurve::getData(QMap<float,float> & data) const
+{
+	data.clear();
+	if(_items.size())
+	{
+		for(int i=0; i<_items.size(); i+=2)
+		{
+			data.insert(((UPlotItem*)_items.at(i))->data().x(), ((UPlotItem*)_items.at(i))->data().y());
+		}
+	}
+}
+
 
 
 
@@ -1352,7 +1364,7 @@ UPlotLegendItem::UPlotLegendItem(UPlotCurve * curve, QWidget * parent) :
 	_aChangeText = new QAction(tr("Change text..."), this);
 	_aResetText = new QAction(tr("Reset text..."), this);
 	_aChangeColor = new QAction(tr("Change color..."), this);
-	_aCopyToClipboard = new QAction(tr("Copy curve data to the clipboard"), this);
+	_aCopyToClipboard = new QAction(tr("Copy curve data to clipboard"), this);
 	_aShowStdDev = new QAction(tr("Show std deviation"), this);
 	_aShowStdDev->setCheckable(true);
 	_aMoveUp = new QAction(tr("Move up"), this);
@@ -1495,8 +1507,10 @@ UPlotLegend::UPlotLegend(QWidget * parent) :
 	_aUseFlatButtons = new QAction(tr("Use flat buttons"), this);
 	_aUseFlatButtons->setCheckable(true);
 	_aUseFlatButtons->setChecked(_flat);
+	_aCopyAllCurveToClipboard = new QAction(tr("Copy all curve data to clipboard"), this);
 	_menu = new QMenu(tr("Legend"), this);
 	_menu->addAction(_aUseFlatButtons);
+	_menu->addAction(_aCopyAllCurveToClipboard);
 
 	QVBoxLayout * vLayout = new QVBoxLayout(this);
 	vLayout->setContentsMargins(0,0,0,0);
@@ -1635,6 +1649,63 @@ void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
 	{
 		this->setFlat(_aUseFlatButtons->isChecked());
 	}
+	else if(action == _aCopyAllCurveToClipboard)
+	{
+		QList<UPlotLegendItem *> items = this->findChildren<UPlotLegendItem*>();
+		if(items.size())
+		{
+			QMap<float,float> firstData;
+			QVector<QVector<float> > axes;
+			for(int i=0; i<items.size(); ++i)
+			{
+				QMap<float, float> data;
+				items.at(i)->curve()->getData(data);
+
+				if(i==0)
+				{
+					firstData = data;
+				}
+				else
+				{
+					QVector<float> y(firstData.size(), 0.0f);
+					// ust to make usre that we have the same number of data on each curve, set 0 for unknowns
+					int j=0;
+					for(QMap<float,float>::iterator iter=firstData.begin(); iter!=firstData.end(); ++iter)
+					{
+						if(data.contains(iter.key()))
+						{
+							y[j] = data.value(iter.key());
+						}
+						++j;
+					}
+					axes.push_back(y);
+				}
+			}
+			if(firstData.size())
+			{
+				axes.push_front(firstData.values().toVector());
+				axes.push_front(firstData.keys().toVector());
+				QString text;
+				for(int i=0; i<axes.size(); ++i)
+				{
+					for(int j=0; j<axes[i].size(); ++j)
+					{
+						text.append(QString::number(axes[i][j]));
+						if(j+1<axes[i].size())
+						{
+							text.append(' ');
+						}
+					}
+					if(i+1<axes.size())
+					{
+						text.append("\n");
+					}
+				}
+				QClipboard * clipboard = QApplication::clipboard();
+				clipboard->setText(text);
+			}
+		}
+	}
 }
 
 void UPlotLegend::redirectToggled(bool toggled)

tools/Camera/main.cpp

@@ -149,7 +149,7 @@ int main(int argc, char * argv[])
 		}
 		else if(UDirectory::exists(path))
 		{
-			camera = new rtabmap::CameraImages(path, 1, false, false, false, rate);
+			camera = new rtabmap::CameraImages(path, rate);
 		}
 		else
 		{

tools/ConsoleApp/main.cpp

@@ -360,7 +360,8 @@ int main(int argc, char * argv[])
 	Camera * camera = 0;
 	if(UDirectory::exists(path))
 	{
-		camera = new CameraImages(path, startAt, false, false, false, rate>0.0f?1.0f/rate:0.0f);
+		camera = new CameraImages(path, rate>0.0f?1.0f/rate:0.0f);
+		((CameraImages*)camera)->setStartIndex(startAt);
 	}
 	else
 	{