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-02 01:20:25 +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/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);