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")
2026-09-01 17:10:26 +08:00 · 2015-12-17 14:13:19 -05:00
parent f5c062448d
commit 3292fb1146
49 changed files with 2135 additions and 2010 deletions
--- a/corelib/include/rtabmap/core/CameraRGB.h
+++ b/corelib/include/rtabmap/core/CameraRGB.h
@@ -49,23 +49,11 @@ class RTABMAP_EXP CameraImages :
 	public Camera
 {
 public:
-	CameraImages(const std::string & path,
-			int startAt = 1,
-			bool refreshDir = false,
-			bool rectifyImages = false,
-			bool isDepth = false,
+	CameraImages();
+	CameraImages(
+			const std::string & path,
 			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;
 };

--- a/corelib/include/rtabmap/core/CameraRGBD.h
+++ b/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,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
+			float depthScaleFactor = 1.0f,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRGBDImages();
@@ -291,14 +290,7 @@ protected:
 	virtual SensorData captureImage();

 private:
-	CameraImages * cameraRGB_;
-	CameraImages * cameraDepth_;
-	double depthScaleFactor_;
-	bool filenamesAreTimestamps_;
-	std::string timestampsPath_;
-	std::list<double> stamps_;
-	CameraModel cameraModel_;
-	std::string cameraName_;
+	CameraImages cameraDepth_;
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/CameraStereo.h
+++ b/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_;
 };


--- a/corelib/include/rtabmap/core/Features2d.h
+++ b/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,
--- a/corelib/include/rtabmap/core/Graph.h
+++ b/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,
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/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
--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/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_;
--- a/corelib/include/rtabmap/core/OdometryInfo.h
+++ b/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
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/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;
@@ -427,6 +433,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_;
 	
--- a/corelib/include/rtabmap/core/Registration.h
+++ b/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())
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/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;}
--- a/corelib/include/rtabmap/core/RtabmapEvent.h
+++ b/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
--- a/corelib/include/rtabmap/core/SensorData.h
+++ b/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_;
 };

 }
--- a/corelib/include/rtabmap/core/Transform.h
+++ b/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:
--- a/corelib/src/Camera.cpp
+++ b/corelib/src/Camera.cpp
@@ -55,10 +55,9 @@ Camera::Camera(float imageRate, const Transform & localTransform) :

 Camera::~Camera()
 {
-	if(_frameRateTimer)
-	{
-		delete _frameRateTimer;
-	}
+	UDEBUG("");
+	delete _frameRateTimer;
+	UDEBUG("");
 }

 SensorData Camera::takeImage(CameraInfo * info)
--- a/corelib/src/CameraRGB.cpp
+++ b/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),
-	_refreshDir(refreshDir),
-	_rectifyImages(rectifyImages),
-	_isDepth(isDepth),
+	_startAt(0),
+	_refreshDir(false),
+	_rectifyImages(false),
+	_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,
-					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)
+CameraImages::~CameraImages()
 {
+	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)
-	{
-		return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
-	}
-	return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, img, cv::Mat(), _model, this->getNextSeqID(), UTimer::now());
+	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:cv::Mat(), _model, this->getNextSeqID(), stamp);
+	data.setGroundTruth(groundTruthPose);
+	return data;
 }


--- a/corelib/src/CameraRGBD.cpp
+++ b/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,
-		bool filenamesAreTimestamps,
-		const std::string & timestampsPath,
+		float depthScaleFactor,
 		float imageRate,
 		const Transform & localTransform) :
-		Camera(imageRate, localTransform),
-		cameraRGB_(0),
-		cameraDepth_(0),
-		depthScaleFactor_(depthScaleFactor),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath)
+		CameraImages(pathRGBImages, imageRate, localTransform)
 {
 	UASSERT(depthScaleFactor >= 1.0);
-	cameraRGB_ = new CameraImages(pathRGBImages);
-	cameraDepth_ = new CameraImages(pathDepthImages, 1, false, false, true);
+	cameraDepth_.setPath(pathDepthImages);
+	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());
-		}
-	}
-
-	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;
-			}
+					this->imagesCount(), cameraDepth_.imagesCount());
 		}
 	}

@@ -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();
-			if(depthScaleFactor_ > 1.0)
-			{
-				depthScaled /= depthScaleFactor_;
-			}
-			data = SensorData(rgb.imageRaw(), depthScaled, cameraModel_, this->getNextSeqID(), stamp);
+			data = SensorData(rgb.imageRaw(), depth.depthRaw(), rgb.cameraModels(), rgb.id(), rgb.stamp());
+			data.setGroundTruth(rgb.groundTruth());
 		}
 	}
 	return data;
--- a/corelib/src/CameraStereo.cpp
+++ b/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),
-		camera_(0),
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
+		CameraImages(pathLeftImages, imageRate, localTransform),
+		camera2_(new CameraImages(pathRightImages))
 {
-	camera_ = new CameraImages(pathLeftImages);
-	camera2_ = new CameraImages(pathRightImages);
+	this->setImagesRectified(rectifyImages);
+	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),
-		camera_(0),
-		camera2_(0),
-		filenamesAreTimestamps_(filenamesAreTimestamps),
-		timestampsPath_(timestampsPath),
-		rectifyImages_(rectifyImages)
+		CameraImages("", imageRate, localTransform),
+		camera2_(0)
 {
 	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_)
-	{
-		delete camera_;
-	}
+	UDEBUG("");
 	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)
-	{
-		UERROR("Cannot initialize the camera.");
-	}
-	else if(camera_->init())
+	if(CameraImages::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(stamps_.size())
+		if(camera2_)
 		{
-			stamp = stamps_.front();
-			stamps_.pop_front();
+			right = camera2_->takeImage();
 		}
 		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 leftImage = left.imageRaw();
-				cv::Mat rightImage = right.imageRaw();
-				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);
+				cv::Mat tmp;
+				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
+				rightImage = tmp;
 			}
+			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;
--- a/corelib/src/CameraThread.cpp
+++ b/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;
--- a/corelib/src/Features2d.cpp
+++ b/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)
 	{
--- a/corelib/src/Graph.cpp
+++ b/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
--- a/corelib/src/Memory.cpp
+++ b/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(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
+			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
+			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
-					{
-						cv::KeyPoint::convert(keypoints, leftCorners);
-					}
+
+					UASSERT(keypoints.size() == leftCorners.size());

 					//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)
-						{
-							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(keypoints.size() == descriptors.rows);

+						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);
--- a/corelib/src/Odometry.cpp
+++ b/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;
--- a/corelib/src/Parameters.cpp
+++ b/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;
--- a/corelib/src/RegistrationVis.cpp
+++ b/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));
 		}
--- a/corelib/src/Rtabmap.cpp
+++ b/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;
 	}
--- a/corelib/src/RtabmapThread.cpp
+++ b/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)
 		{
--- a/corelib/src/Stereo.cpp
+++ b/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)
--- a/corelib/src/Transform.cpp
+++ b/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 (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz].
+ * 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
 */
 Transform Transform::fromString(const std::string & string)
 {
-	Transform t;
 	std::list<std::string> list = uSplit(string, ' ');
-	if(list.size() == 6 || list.size() == 9 || list.size() == 12)
-	{
-		std::vector<float> numbers(list.size());
-		int i = 0;
-		for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
-		{
-			numbers[i++] = uStr2Float(*iter);
-		}
+	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());

-		if(numbers.size() == 6)
-		{
-			t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
-		}
-		else if(numbers.size() == 9)
-		{
-			t = Transform(numbers[0], numbers[1], numbers[2], 0,
-						  numbers[3], numbers[4], numbers[5], 0,
-						  numbers[6], numbers[7], numbers[8], 0);
-		}
-		else if(numbers.size() == 12)
-		{
-			t = Transform(numbers[0], numbers[1], numbers[2], numbers[9],
-						  numbers[3], numbers[4], numbers[5], numbers[10],
-						  numbers[6], numbers[7], numbers[8], numbers[11]);
-		}
-	}
-	else
+	std::vector<float> numbers(list.size());
+	int i = 0;
+	for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
 	{
-		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;
 }
--- a/corelib/src/util2d.cpp
+++ b/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);