0.13.0 Database: Added velocity field to Node table, renamed Map_Node_Word table to Feature, added information_matrix field to Link table (replacing rot_variance and trans_variance). Added graph::calcKittiSequenceErrors().Added "Mem/IntermediateNodeDataKept" parameter (default false). Updated all interfaces using rotVariance and transVariance values with covariance matrix instead. g2o SBA: always use covariance in links. kitti-dataset tool: added --disp option and kitti statistics are shown at the end. StatsToolBox: units can have "/".

2026-09-11 22:10:21 +08:00 · 2017-05-11 15:02:18 -04:00
parent 414e3555a5
commit 104c1e6945
45 changed files with 1539 additions and 1148 deletions
@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 12)
-SET(RTABMAP_PATCH_VERSION 5)
+SET(RTABMAP_MINOR_VERSION 13)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -131,7 +131,7 @@ public:
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -197,7 +197,7 @@ private:

 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
@@ -58,6 +58,20 @@ bool RTABMAP_EXP importPoses(
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1

+/**
+ * Compute translation and rotation errors for KITTI datasets.
+ * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
+ * @param poses_gt, Ground Truth poses
+ * @param poses_result, Estimated poses
+ * @param t_err, Output translation error (%)
+ * @param r_err, Output rotation error (deg/m)
+ */
+void RTABMAP_EXP calcKittiSequenceErrors(
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err);
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
@@ -205,6 +219,10 @@ float RTABMAP_EXP computePathLength(
 		unsigned int fromIndex = 0,
 		unsigned int toIndex = 0);

+// assuming they are all linked in map order
+float RTABMAP_EXP computePathLength(
+		const std::map<int, Transform> & path);
+
 std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);
@@ -52,14 +52,7 @@ public:
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
-			const cv::Mat & userData = cv::Mat());
-	Link(int from,
-			int to,
-			Type type,
-			const Transform & transform,
-			double rotVariance,
-			double transVariance,
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
 			const cv::Mat & userData = cv::Mat());

 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -87,7 +80,6 @@ public:

 private:
 	void setInfMatrix(const cv::Mat & infMatrix);
-	void setVariance(double rotVariance, double transVariance);

 private:
 	int from_;
@@ -75,6 +75,7 @@ public:
 	bool update(const SensorData & data,
 			const Transform & pose,
 			const cv::Mat & covariance,
+			const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
 			Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
@@ -153,6 +154,7 @@ public:
 			std::string & label,
 			double & stamp,
 			Transform & groundTruth,
+			std::vector<float> & velocity,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
@@ -244,6 +246,7 @@ private:
 	bool _rawDescriptorsKept;
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
+	bool _saveIntermediateNodeData;
 	bool _incrementalMemory;
 	bool _reduceGraph;
 	int _maxStMemSize;
@@ -39,53 +39,29 @@ namespace rtabmap {
 class OdometryEvent : public UEvent
 {
 public:
-	static cv::Mat generateCovarianceMatrix(float rotVariance, float transVariance)
-	{
-		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
-		UASSERT(uIsFinite(transVariance) && transVariance>0);
-		cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-		covariance.at<double>(0,0) = transVariance;
-		covariance.at<double>(1,1) = transVariance;
-		covariance.at<double>(2,2) = transVariance;
-		covariance.at<double>(3,3) = rotVariance;
-		covariance.at<double>(4,4) = rotVariance;
-		covariance.at<double>(5,5) = rotVariance;
-		return covariance;
-	}
-public:
-	OdometryEvent() :
-		_covariance(cv::Mat::eye(6,6,CV_64FC1))
+	OdometryEvent()
 	{
+		_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 	OdometryEvent(
 		const SensorData & data,
 		const Transform & pose,
-		const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1),
 		const OdometryInfo & info = OdometryInfo()) :
 			_data(data),
 			_pose(pose),
 			_info(info)
 	{
-		UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
-		UASSERT_MSG(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(1,1)) && covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(5,5)) && covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		_covariance = covariance;
-	}
-	OdometryEvent(
-		const SensorData & data,
-		const Transform & pose,
-		double rotVariance = 1.0,
-		double transVariance = 1.0,
-		const OdometryInfo & info = OdometryInfo()) :
-			_data(data),
-			_pose(pose),
-			_covariance(generateCovarianceMatrix(rotVariance, transVariance)),
-			_info(info)
-	{
+		if(_info.covariance.empty())
+		{
+			_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+		}
+		UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
 	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
@@ -93,15 +69,29 @@ public:
 	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
 	const Transform & pose() const {return _pose;}
-	const cv::Mat & covariance() const {return _covariance;}
+	const cv::Mat & covariance() const {return _info.covariance;}
+	std::vector<float> velocity() const {
+		if(_info.interval>0.0)
+		{
+			std::vector<float> velocity(6,0);
+			float x,y,z,roll,pitch,yaw;
+			_info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+			velocity[0] = x/_info.interval;
+			velocity[1] = y/_info.interval;
+			velocity[2] = z/_info.interval;
+			velocity[3] = roll/_info.interval;
+			velocity[4] = pitch/_info.interval;
+			velocity[5] = yaw/_info.interval;
+		}
+		return std::vector<float>();
+	}
 	const OdometryInfo & info() const {return _info;}
-	double rotVariance() const {return uMax3(_covariance.at<double>(3,3), _covariance.at<double>(4,4), _covariance.at<double>(5,5));}
-	double transVariance() const {return uMax3(_covariance.at<double>(0,0), _covariance.at<double>(1,1), _covariance.at<double>(2,2));}
+	double rotVariance() const {return uMax3(_info.covariance.at<double>(3,3), _info.covariance.at<double>(4,4), _info.covariance.at<double>(5,5));}
+	double transVariance() const {return uMax3(_info.covariance.at<double>(0,0), _info.covariance.at<double>(1,1), _info.covariance.at<double>(2,2));}

 private:
 	SensorData _data;
 	Transform _pose;
-	cv::Mat _covariance;
 	OdometryInfo _info;
 };

@@ -42,8 +42,6 @@ public:
 		matches(0),
 		inliers(0),
 		icpInliersRatio(0.0f),
-		varianceLin(0.0f),
-		varianceAng(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
@@ -67,8 +65,7 @@ public:
 		output.matches = matches;
 		output.inliers = inliers;
 		output.icpInliersRatio = icpInliersRatio;
-		output.varianceLin = varianceLin;
-		output.varianceAng = varianceAng;
+		output.covariance = covariance.clone();
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
@@ -80,6 +77,7 @@ public:
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
+		output.interval = interval;
 		output.transform = transform;
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
@@ -92,8 +90,7 @@ public:
 	int matches;
 	int inliers;
 	float icpInliersRatio;
-	float varianceLin;
-	float varianceAng;
+	cv::Mat covariance;
 	int features;
 	int localMapSize;
 	int localScanMapSize;
@@ -197,6 +197,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, MapLabelsAdded, 		    bool, true, 	"Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
 	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, false, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
 	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
+	RTABMAP_PARAM(Mem, IntermediateNodeDataKept, bool, false, 	"Keep intermediate node data in db.");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory,       bool, true, 	"SLAM mode, otherwise it is Localization mode.");
 	RTABMAP_PARAM(Mem, ReduceGraph,             bool, false, 	"Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
@@ -35,8 +35,6 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
-		varianceLin(0),
-		varianceAng(0),
 		inliers(0),
 		matches(0),
 		icpInliersRatio(0),
@@ -45,8 +43,7 @@ public:
 	{
 	}

-	float varianceLin;
-	float varianceAng;
+	cv::Mat covariance;
 	std::string rejectedMsg;

 	// RegistrationVis
@@ -71,6 +71,7 @@ public:
 			const SensorData & data,
 			Transform odomPose,
 			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
+			const std::vector<float> & odomVelocity = std::vector<float>(),
 			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 	// for convenience
 	bool process(
@@ -78,6 +79,7 @@ public:
 			Transform odomPose,
 			float odomLinearVariance,
 			float odomAngularVariance,
+			const std::vector<float> & odomVelocity = std::vector<float>(),
 			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 	// for convenience, loop closure detection only
 	bool process(
@@ -125,8 +125,7 @@ private:
 	Rtabmap * _rtabmap;
 	bool _paused;
 	Transform lastPose_;
-	double _rotVariance;
-	double _transVariance;
+	cv::Mat covariance_;

 	cv::Mat _userData;
 	UMutex _userDataMutex;
@@ -115,11 +115,21 @@ public:
 	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
+	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
+		_velocity = std::vector<float>(6,0);
+		_velocity[0]=vx;
+		_velocity[1]=vy;
+		_velocity[2]=vz;
+		_velocity[3]=vroll;
+		_velocity[4]=vpitch;
+		_velocity[5]=vyaw;
+	}

 	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
+	const std::vector<float> & getVelocity() const {return _velocity;}

 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}
@@ -147,6 +157,7 @@ private:

 	Transform _pose;
 	Transform _groundTruthPose;
+	std::vector<float> _velocity;

 	SensorData _sensorData;
 };
@@ -50,7 +50,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
 			int pnpRefineIterations = 1,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
-			double * varianceOut = 0, // mean reproj error if words3B is not set
+			cv::Mat * covariance = 0, // mean reproj error if words3B is not set
 			std::vector<int> * matchesOut = 0,
 			std::vector<int> * inliersOut = 0);

@@ -61,7 +61,7 @@ Transform RTABMAP_EXP estimateMotion3DTo3D(
 			double inliersDistance = 0.1,
 			int iterations = 100,
 			int refineIterations = 5,
-			double * varianceOut = 0,
+			cv::Mat * covariance = 0,
 			std::vector<int> * matchesOut = 0,
 			std::vector<int> * inliersOut = 0);

@@ -57,7 +57,7 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		int refineModelIterations = 10,
 		double refineModelSigma = 3.0,
 		std::vector<int> * inliers = 0,
-		double * variance = 0);
+		cv::Mat * variance = 0);

 void RTABMAP_EXP computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
@@ -639,7 +639,8 @@ bool DBDriver::getNodeInfo(
 		int & weight,
 		std::string & label,
 		double & stamp,
-		Transform & groundTruthPose) const
+		Transform & groundTruthPose,
+		std::vector<float> & velocity) const
 {
 	bool found = false;
 	// look in the trash
@@ -659,7 +660,7 @@ bool DBDriver::getNodeInfo(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;
@@ -864,6 +864,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 		if(uStrNumCmp(_version, "0.11.10") >= 0)
 		{
 			std::stringstream fields;
+
 			if(images)
 			{
 				fields << "image, depth, calibration";
@@ -1442,7 +1443,8 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		int & weight,
 		std::string & label,
 		double & stamp,
-		Transform & groundTruthPose) const
+		Transform & groundTruthPose,
+		std::vector<float> & velocity) const
 {
 	bool found = false;
 	if(_ppDb && signatureId)
@@ -1451,7 +1453,14 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;

-		if(uStrNumCmp(_version, "0.11.1") >= 0)
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity "
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
+		else if(uStrNumCmp(_version, "0.11.1") >= 0)
 		{
 			query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose "
 					 "FROM Node "
@@ -1503,15 +1512,26 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 					label = reinterpret_cast<const char*>(p); // label
 				}
 				stamp = sqlite3_column_double(ppStmt, index++); // stamp
-			}

-			if(uStrNumCmp(_version, "0.11.1") >= 0)
-			{
-				data = sqlite3_column_blob(ppStmt, index); // ground_truh_pose
-				dataSize = sqlite3_column_bytes(ppStmt, index++);
-				if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
+				if(uStrNumCmp(_version, "0.11.1") >= 0)
 				{
-					memcpy(groundTruthPose.data(), data, dataSize);
+					data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
+					{
+						memcpy(groundTruthPose.data(), data, dataSize);
+					}
+
+					if(uStrNumCmp(_version, "0.13.0") >= 0)
+					{
+						velocity.resize(6,0);
+						data = sqlite3_column_blob(ppStmt, index); // velocity
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
+						{
+							memcpy(velocity.data(), data, dataSize);
+						}
+					}
 				}
 			}

@@ -1546,7 +1566,14 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 		}
 		if(ignoreBadSignatures)
 		{
-			query << "WHERE id in (select node_id from Map_Node_Word) ";
+			if(uStrNumCmp(_version, "0.13.0") >= 0)
+			{
+				query << "WHERE id in (select node_id from Feature) ";
+			}
+			else
+			{
+				query << "WHERE id in (select node_id from Map_Node_Word) ";
+			}
 		}
 		query  << "ORDER BY id";

@@ -1581,7 +1608,11 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;

-		if(uStrNumCmp(_version, "0.10.10") >= 0)
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link ORDER BY from_id, to_id";
+		}
+		else if(uStrNumCmp(_version, "0.10.10") >= 0)
 		{
 			query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ORDER BY from_id, to_id";
 		}
@@ -1604,8 +1635,6 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
 		int fromId = -1;
 		int toId = -1;
 		int type = Link::kUndef;
-		float rotVariance = 1.0f;
-		float transVariance = 1.0f;
 		const void * data = 0;
 		int dataSize = 0;

@@ -1634,10 +1663,28 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig

 			if(!ignoreNullLinks || !transform.isNull())
 			{
+				cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
 				if(uStrNumCmp(_version, "0.8.4") >= 0)
 				{
-					rotVariance = sqlite3_column_double(ppStmt, index++);
-					transVariance = sqlite3_column_double(ppStmt, index++);
+					if(uStrNumCmp(_version, "0.13.0") >= 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						UASSERT(dataSize==36*sizeof(double) && data);
+						informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
+					}
+					else
+					{
+						double rotVariance = sqlite3_column_double(ppStmt, index++);
+						double transVariance = sqlite3_column_double(ppStmt, index++);
+						UASSERT(rotVariance > 0.0 && transVariance>0.0);
+						informationMatrix.at<double>(0,0) = 1.0/transVariance;
+						informationMatrix.at<double>(1,1) = 1.0/transVariance;
+						informationMatrix.at<double>(2,2) = 1.0/transVariance;
+						informationMatrix.at<double>(3,3) = 1.0/rotVariance;
+						informationMatrix.at<double>(4,4) = 1.0/rotVariance;
+						informationMatrix.at<double>(5,5) = 1.0/rotVariance;
+					}

 					cv::Mat userDataCompressed;
 					if(uStrNumCmp(_version, "0.10.10") >= 0)
@@ -1651,17 +1698,19 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
 						}
 					}

-					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
+					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
 				}
 				else if(uStrNumCmp(_version, "0.7.4") >= 0)
 				{
-					rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
-					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+					double variance = sqlite3_column_double(ppStmt, index++);
+					UASSERT(variance>0.0);
+					informationMatrix *= 1.0/variance;
+					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix)));
 				}
 				else
 				{
 					// neighbor is 0, loop closures are 1 and 2 (child)
-					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
+					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
 				}
 			}

@@ -1726,14 +1775,18 @@ void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;

-		// Create a new entry in table Signature
-		query << "SELECT count(word_id) "
-			  << "FROM Map_Node_Word "
-			  << "WHERE node_id=" << nodeId << ";";
-
-		//query.append("COMMIT;");
-
-		//ULOGGER_DEBUG("DBDriverSqlite3::getSurfNi() Execute query : %s", query.toStdString().c_str());
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query << "SELECT count(word_id) "
+				  << "FROM Feature "
+				  << "WHERE node_id=" << nodeId << ";";
+		}
+		else
+		{
+			query << "SELECT count(word_id) "
+				  << "FROM Map_Node_Word "
+				  << "WHERE node_id=" << nodeId << ";";
+		}

 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1879,7 +1932,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 		unsigned int loaded = 0;

 		// Load nodes information
-		if(uStrNumCmp(_version, "0.11.1") >= 0)
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
+		else if(uStrNumCmp(_version, "0.11.1") >= 0)
 		{
 			query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
 				  << "FROM Node "
@@ -1914,6 +1973,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			int weight = 0;
 			Transform pose;
 			Transform groundTruthPose;
+			std::vector<float> velocity;
 			const void * data = 0;
 			int dataSize = 0;
 			std::string label;
@@ -1942,15 +2002,26 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					{
 						label = reinterpret_cast<const char*>(p);
 					}
-				}

-				if(uStrNumCmp(_version, "0.11.1") >= 0)
-				{
-					data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
-					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
+					if(uStrNumCmp(_version, "0.11.1") >= 0)
 					{
-						memcpy(groundTruthPose.data(), data, dataSize);
+						data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
+						{
+							memcpy(groundTruthPose.data(), data, dataSize);
+						}
+
+						if(uStrNumCmp(_version, "0.13.0") >= 0)
+						{
+							velocity.resize(6,0);
+							data = sqlite3_column_blob(ppStmt, index); // velocity
+							dataSize = sqlite3_column_bytes(ppStmt, index++);
+							if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
+							{
+								memcpy(velocity.data(), data, dataSize);
+							}
+						}
 					}
 				}

@@ -1970,6 +2041,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						label,
 						pose,
 						groundTruthPose);
+				if(velocity.size() == 6)
+				{
+					s->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
+				}
 				s->setSaved(true);
 				nodes.push_back(s);
 				++loaded;
@@ -1992,7 +2067,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<

 		// Prepare the query... Get the map from signature and visual words
 		std::stringstream query2;
-		if(uStrNumCmp(_version, "0.12.0") >= 0)
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
+					 "FROM Feature "
+					 "WHERE node_id = ? ";
+		}
+		else if(uStrNumCmp(_version, "0.12.0") >= 0)
 		{
 			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
 					 "FROM Map_Node_Word "
@@ -2485,7 +2566,11 @@ void DBDriverSqlite3::loadLinksQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;

-		if(uStrNumCmp(_version, "0.10.10") >= 0)
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query << "SELECT to_id, type, transform, information_matrix, user_data FROM Link ";
+		}
+		else if(uStrNumCmp(_version, "0.10.10") >= 0)
 		{
 			query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
 		}
@@ -2524,8 +2609,6 @@ void DBDriverSqlite3::loadLinksQuery(

 		int toId = -1;
 		int type = Link::kUndef;
-		float rotVariance = 1.0f;
-		float transVariance = 1.0f;
 		const void * data = 0;
 		int dataSize = 0;

@@ -2551,10 +2634,28 @@ void DBDriverSqlite3::loadLinksQuery(
 				UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
 			}

+			cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(uStrNumCmp(_version, "0.8.4") >= 0)
 			{
-				rotVariance = sqlite3_column_double(ppStmt, index++);
-				transVariance = sqlite3_column_double(ppStmt, index++);
+				if(uStrNumCmp(_version, "0.13.0") >= 0)
+				{
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					UASSERT(dataSize==36*sizeof(double) && data);
+					informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
+				}
+				else
+				{
+					double rotVariance = sqlite3_column_double(ppStmt, index++);
+					double transVariance = sqlite3_column_double(ppStmt, index++);
+					UASSERT(rotVariance > 0.0 && transVariance>0.0);
+					informationMatrix.at<double>(0,0) = 1.0/transVariance;
+					informationMatrix.at<double>(1,1) = 1.0/transVariance;
+					informationMatrix.at<double>(2,2) = 1.0/transVariance;
+					informationMatrix.at<double>(3,3) = 1.0/rotVariance;
+					informationMatrix.at<double>(4,4) = 1.0/rotVariance;
+					informationMatrix.at<double>(5,5) = 1.0/rotVariance;
+				}

 				cv::Mat userDataCompressed;
 				if(uStrNumCmp(_version, "0.10.10") >= 0)
@@ -2568,17 +2669,19 @@ void DBDriverSqlite3::loadLinksQuery(
 					}
 				}

-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
 			}
 			else if(uStrNumCmp(_version, "0.7.4") >= 0)
 			{
-				rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+				double variance = sqlite3_column_double(ppStmt, index++);
+				UASSERT(variance>0.0);
+				informationMatrix *= 1.0/variance;
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
 			}
 			else
 			{
 				// neighbor is 0, loop closures are 1 and 2 (child)
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
 			}

 			rc = sqlite3_step(ppStmt);
@@ -2608,7 +2711,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 		std::stringstream query;
 		int totalLinksLoaded = 0;

-		if(uStrNumCmp(_version, "0.10.10") >= 0)
+		if(uStrNumCmp(_version, "0.13.0") >= 0)
+		{
+			query << "SELECT to_id, type, information_matrix, user_data, transform FROM Link "
+				  << "WHERE from_id = ? "
+				  << "ORDER BY to_id";
+		}
+		else if(uStrNumCmp(_version, "0.10.10") >= 0)
 		{
 			query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
 				  << "WHERE from_id = ? "
@@ -2644,8 +2753,6 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const

 			int toId = -1;
 			int linkType = -1;
-			float rotVariance = 1.0f;
-			float transVariance = 1.0f;
 			std::list<Link> links;
 			const void * data = 0;
 			int dataSize = 0;
@@ -2659,10 +2766,28 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 				toId = sqlite3_column_int(ppStmt, index++);
 				linkType = sqlite3_column_int(ppStmt, index++);
 				cv::Mat userDataCompressed;
+				cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
 				if(uStrNumCmp(_version, "0.8.4") >= 0)
 				{
-					rotVariance = sqlite3_column_double(ppStmt, index++);
-					transVariance = sqlite3_column_double(ppStmt, index++);
+					if(uStrNumCmp(_version, "0.13.0") >= 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						UASSERT(dataSize==36*sizeof(double) && data);
+						informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
+					}
+					else
+					{
+						double rotVariance = sqlite3_column_double(ppStmt, index++);
+						double transVariance = sqlite3_column_double(ppStmt, index++);
+						UASSERT(rotVariance > 0.0 && transVariance>0.0);
+						informationMatrix.at<double>(0,0) = 1.0/transVariance;
+						informationMatrix.at<double>(1,1) = 1.0/transVariance;
+						informationMatrix.at<double>(2,2) = 1.0/transVariance;
+						informationMatrix.at<double>(3,3) = 1.0/rotVariance;
+						informationMatrix.at<double>(4,4) = 1.0/rotVariance;
+						informationMatrix.at<double>(5,5) = 1.0/rotVariance;
+					}

 					if(uStrNumCmp(_version, "0.10.10") >= 0)
 					{
@@ -2677,7 +2802,9 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 				}
 				else if(uStrNumCmp(_version, "0.7.4") >= 0)
 				{
-					rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
+					double variance = sqlite3_column_double(ppStmt, index++);
+					UASSERT(variance>0.0);
+					informationMatrix *= 1.0/variance;
 				}

 				//transform
@@ -2697,11 +2824,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 				{
 					if(uStrNumCmp(_version, "0.7.4") >= 0)
 					{
-						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance, userDataCompressed));
+						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
 					}
 					else // neighbor is 0, loop closures are 1 and 2 (child)
 					{
-						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance, userDataCompressed));
+						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
 					}
 				}
 				else
@@ -2960,7 +3087,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 		UDEBUG("Time=%fs", timer.ticks());


-		// Create new entries in table Map_Word_Node
+		// Create new entries in table Feature
 		query = queryStepKeypoint();
 		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -3296,7 +3423,11 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const

 std::string DBDriverSqlite3::queryStepNode() const
 {
-	if(uStrNumCmp(_version, "0.11.1") >= 0)
+	if(uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity) VALUES(?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.11.1") >= 0)
 	{
 		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
 	}
@@ -3353,6 +3484,26 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 	if(uStrNumCmp(_version, "0.10.1") >= 0)
 	{
 		// ignore user_data
+
+		if(uStrNumCmp(_version, "0.11.1") >= 0)
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			if(uStrNumCmp(_version, "0.13.0") >= 0)
+			{
+				if(s->getVelocity().empty())
+				{
+					rc = sqlite3_bind_null(ppStmt, index++);
+					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				}
+				else
+				{
+					rc = sqlite3_bind_blob(ppStmt, index++, s->getVelocity().data(), s->getVelocity().size()*sizeof(float), SQLITE_STATIC);
+					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				}
+			}
+		}
 	}
 	else if(uStrNumCmp(_version, "0.8.8") >= 0)
 	{
@@ -3367,13 +3518,6 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		}
 	}
-
-	if(uStrNumCmp(_version, "0.11.1") >= 0)
-	{
-		rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-	}
-

 	//step
 	rc=sqlite3_step(ppStmt);
@@ -3822,7 +3966,11 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,

 std::string DBDriverSqlite3::queryStepLinkUpdate() const
 {
-	if(uStrNumCmp(_version, "0.10.10") >= 0)
+	if(uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		return "UPDATE Link SET type=?, information_matrix=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
+	}
+	else if(uStrNumCmp(_version, "0.10.10") >= 0)
 	{
 		return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
 	}
@@ -3842,7 +3990,11 @@ std::string DBDriverSqlite3::queryStepLinkUpdate() const
 std::string DBDriverSqlite3::queryStepLink() const
 {
 	// from_id, to_id are at the end to match the update query above
-	if(uStrNumCmp(_version, "0.10.10") >= 0)
+	if(uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		return "INSERT INTO Link(type, information_matrix, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.10.10") >= 0)
 	{
 		return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
 	}
@@ -3881,7 +4033,13 @@ void DBDriverSqlite3::stepLink(
 	rc = sqlite3_bind_int(ppStmt, index++, link.type());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

-	if(uStrNumCmp(_version, "0.8.4") >= 0)
+	if(uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		// information_matrix
+		rc = sqlite3_bind_blob(ppStmt, index++, link.infMatrix().data, (int)link.infMatrix().total()*sizeof(double), SQLITE_STATIC);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+	else if(uStrNumCmp(_version, "0.8.4") >= 0)
 	{
 		rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -3925,7 +4083,14 @@ void DBDriverSqlite3::stepLink(

 std::string DBDriverSqlite3::queryStepWordsChanged() const
 {
-	return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
+	if(uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		return "UPDATE Feature SET word_id = ? WHERE word_id = ? AND node_id = ?;";
+	}
+	else
+	{
+		return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
+	}
 }
 void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldWordId, int newWordId) const
 {
@@ -3951,7 +4116,11 @@ void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int ol

 std::string DBDriverSqlite3::queryStepKeypoint() const
 {
-	if(uStrNumCmp(_version, "0.12.0") >= 0)
+	if(uStrNumCmp(_version, "0.13.0") >= 0)
+	{
+		return "INSERT INTO Feature(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.12.0") >= 0)
 	{
 		return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -99,7 +99,7 @@ private:

 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -267,7 +267,8 @@ SensorData DBReader::captureImage(CameraInfo * info)
 						double stamp;
 						int mapId;
 						Transform localTransform, pose, groundTruth;
-						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
+						std::vector<float> velocity;
+						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
 						if(previousStamp && stamp && stamp > previousStamp)
 						{
 							delay = stamp - previousStamp;
@@ -320,7 +321,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			std::string label;
 			double stamp;
 			Transform groundTruth;
-			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
+			std::vector<float> velocity;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);

 			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
@@ -425,6 +425,124 @@ bool importPoses(
 	return false;
 }

+// KITTI evaluation
+float lengths[] = {100,200,300,400,500,600,700,800};
+int32_t num_lengths = 8;
+
+struct errors {
+	int32_t first_frame;
+	float   r_err;
+	float   t_err;
+	float   len;
+	float   speed;
+	errors (int32_t first_frame,float r_err,float t_err,float len,float speed) :
+		first_frame(first_frame),r_err(r_err),t_err(t_err),len(len),speed(speed) {}
+};
+
+std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) {
+	std::vector<float> dist;
+	dist.push_back(0);
+	for (int32_t i=1; i<poses.size(); i++) {
+		Transform P1 = poses[i-1];
+		Transform P2 = poses[i];
+		float dx = P1.x()-P2.x();
+		float dy = P1.y()-P2.y();
+		float dz = P1.z()-P2.z();
+		dist.push_back(dist[i-1]+sqrt(dx*dx+dy*dy+dz*dz));
+	}
+	return dist;
+}
+
+int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,float len) {
+	for (int32_t i=first_frame; i<dist.size(); i++)
+		if (dist[i]>dist[first_frame]+len)
+			return i;
+	return -1;
+}
+
+inline float rotationError(const Transform &pose_error) {
+	float a = pose_error(0,0);
+	float b = pose_error(1,1);
+	float c = pose_error(2,2);
+	float d = 0.5*(a+b+c-1.0);
+	return std::acos(std::max(std::min(d,1.0f),-1.0f));
+}
+
+inline float translationError(const Transform &pose_error) {
+	float dx = pose_error.x();
+	float dy = pose_error.y();
+	float dz = pose_error.z();
+	return sqrt(dx*dx+dy*dy+dz*dz);
+}
+
+void calcKittiSequenceErrors (
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err) {
+
+	UASSERT(poses_gt.size() == poses_result.size());
+
+	// error vector
+	std::vector<errors> err;
+
+	// parameters
+	int32_t step_size = 10; // every second
+
+	// pre-compute distances (from ground truth as reference)
+	std::vector<float> dist = trajectoryDistances(poses_gt);
+
+	// for all start positions do
+	for (int32_t first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
+
+		// for all segment lengths do
+		for (int32_t i=0; i<num_lengths; i++) {
+
+			// current length
+			float len = lengths[i];
+
+			// compute last frame
+			int32_t last_frame = lastFrameFromSegmentLength(dist,first_frame,len);
+
+			// continue, if sequence not long enough
+			if (last_frame==-1)
+				continue;
+
+			// compute rotational and translational errors
+			Transform pose_delta_gt     = poses_gt[first_frame].inverse()*poses_gt[last_frame];
+			Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
+			Transform pose_error        = pose_delta_result.inverse()*pose_delta_gt;
+			float r_err = rotationError(pose_error);
+			float t_err = translationError(pose_error);
+
+			// compute speed
+			float num_frames = (float)(last_frame-first_frame+1);
+			float speed = len/(0.1*num_frames);
+
+			// write to file
+			err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
+		}
+	}
+
+	t_err = 0;
+	r_err = 0;
+
+	// for all errors do => compute sum of t_err, r_err
+	for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
+	{
+		t_err += it->t_err;
+		r_err += it->r_err;
+	}
+
+	// save errors
+	float num = err.size();
+	t_err /= num;
+	r_err /= num;
+	t_err *= 100.0f;    // Translation error (%)
+	r_err *= 180/CV_PI; // Rotation error (deg/m)
+}
+// KITTI evaluation end
+

 ////////////////////////////////////////////
 // Graph utilities
@@ -1636,6 +1754,29 @@ float computePathLength(
 	return length;
 }

+float computePathLength(
+		const std::map<int, Transform> & path)
+{
+	float length = 0.0f;
+	if(path.size() > 1)
+	{
+		float x=0, y=0, z=0;
+		std::map<int, Transform>::const_iterator iter=path.begin();
+		Transform previousPose = iter->second;
+		++iter;
+		for(; iter!=path.end(); ++iter)
+		{
+			const Transform & currentPose = iter->second;
+			x += fabs(previousPose.x() - currentPose.x());
+			y += fabs(previousPose.y() - currentPose.y());
+			z += fabs(previousPose.z() - currentPose.z());
+			previousPose = currentPose;
+		}
+		length = sqrt(x*x + y*y + z*z);
+	}
+	return length;
+}
+
 // return all paths linked only by neighbor links
 std::list<std::map<int, Transform> > getPaths(
 		std::map<int, Transform> poses,
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Link.h"
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/Compression.h>

 namespace rtabmap {
@@ -61,29 +62,6 @@ Link::Link(int from,
 		_userDataRaw = userData;
 	}
 }
-Link::Link(int from,
-		int to,
-		Type type,
-		const Transform & transform,
-		double rotVariance,
-		double transVariance,
-		const cv::Mat & userData) :
-	from_(from),
-	to_(to),
-	transform_(transform),
-	type_(type)
-{
-	setVariance(rotVariance, transVariance);
-
-	if(userData.type() == CV_8UC1) // Bytes
-	{
-		_userDataCompressed = userData; // assume compressed
-	}
-	else
-	{
-		_userDataRaw = userData;
-	}
-}

 double Link::rotVariance() const
 {
@@ -100,25 +78,14 @@ double Link::transVariance() const

 void Link::setInfMatrix(const cv::Mat & infMatrix) {
 	UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(0,0)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(1,1)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(2,2)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(3,3)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(4,4)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(5,5)).c_str());
 	infMatrix_ = infMatrix;
 }
-void Link::setVariance(double rotVariance, double transVariance) {
-	UASSERT(uIsFinite(rotVariance) && rotVariance>0);
-	UASSERT(uIsFinite(transVariance) && transVariance>0);
-	infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
-	infMatrix_.at<double>(0,0) = 1.0/transVariance;
-	infMatrix_.at<double>(1,1) = 1.0/transVariance;
-	infMatrix_.at<double>(2,2) = 1.0/transVariance;
-	infMatrix_.at<double>(3,3) = 1.0/rotVariance;
-	infMatrix_.at<double>(4,4) = 1.0/rotVariance;
-	infMatrix_.at<double>(5,5) = 1.0/rotVariance;
-}

 void Link::uncompressUserData()
 {
@@ -74,6 +74,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
 	_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
+	_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
@@ -425,6 +426,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
 	Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
 	Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
+	Parameters::parse(parameters, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -580,13 +582,14 @@ bool Memory::update(
 		const SensorData & data,
 		Statistics * stats)
 {
-	return update(data, Transform(), cv::Mat(), stats);
+	return update(data, Transform(), cv::Mat(), std::vector<float>(), stats);
 }

 bool Memory::update(
 		const SensorData & data,
 		const Transform & pose,
 		const cv::Mat & covariance,
+		const std::vector<float> & velocity,
 		Statistics * stats)
 {
 	UDEBUG("");
@@ -613,6 +616,10 @@ bool Memory::update(
 		UERROR("Failed to create a signature...");
 		return false;
 	}
+	if(velocity.size()==6)
+	{
+		signature->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
+	}

 	t=timer.ticks()*1000;
 	if(stats) stats->addStatistic(Statistics::kTimingMemSignature_creation(), t);
@@ -2265,10 +2272,11 @@ Transform Memory::computeTransform(
 			else if(info && !transform.isIdentity())
 			{
 				//normalize variance
-				info->varianceLin *= transform.getNorm();
-				info->varianceAng *= transform.getAngle();
-				info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
-				info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
+				info->covariance *= transform.getNorm();
+				if(info->covariance.at<double>(0,0) < 0.0001)
+				{
+					info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
+				}
 			}
 		}
 	}
@@ -2321,10 +2329,11 @@ Transform Memory::computeIcpTransform(
 		if(!t.isNull() && !t.isIdentity() && info)
 		{
 			// normalize variance
-			info->varianceLin *= t.getNorm();
-			info->varianceAng *= t.getAngle();
-			info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
-			info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
+			info->covariance *= t.getNorm();
+			if(info->covariance.at<double>(0,0)<=0.0)
+			{
+				info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
+			}
 		}
 	}
 	else
@@ -2875,7 +2884,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				newS->setLabel(oldS->getLabel());
 				oldS->setLabel("");
 				oldS->removeLinks(); // remove all links
-				oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location
+				oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location

 				// Set old image to new signature
 				this->copyData(oldS, newS);
@@ -2890,7 +2899,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 			}
 			else
 			{
-				newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location
+				newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location

 				// update weight
 				oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
@@ -2949,7 +2958,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
 	int mapId, weight;
 	std::string label;
 	double stamp;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
+	std::vector<float> velocity;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
 	return pose;
 }

@@ -2959,7 +2969,8 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
 	int mapId, weight;
 	std::string label;
 	double stamp;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
+	std::vector<float> velocity;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
 	return groundTruth;
 }

@@ -2970,6 +2981,7 @@ bool Memory::getNodeInfo(int signatureId,
 		std::string & label,
 		double & stamp,
 		Transform & groundTruth,
+		std::vector<float> & velocity,
 		bool lookInDatabase) const
 {
 	const Signature * s = this->getSignature(signatureId);
@@ -2981,11 +2993,12 @@ bool Memory::getNodeInfo(int signatureId,
 		label = s->getLabel();
 		stamp = s->getStamp();
 		groundTruth = s->getGroundTruthPose();
+		velocity = s->getVelocity();
 		return true;
 	}
 	else if(lookInDatabase && _dbDriver)
 	{
-		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth);
+		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity);
 	}
 	return false;
 }
@@ -3669,7 +3682,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}

 	Signature * s;
-	if(this->isBinDataKept() && !isIntermediateNode)
+	if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
 	{
 		UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
 				image.empty()?0:1,
@@ -529,10 +529,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			info->distanceTravelled = distanceTravelled_;
 		}

-		info->varianceLin *= t.getNorm();
-		info->varianceAng *= t.getAngle();
-		info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
-		info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
+		info->covariance *= t.getNorm();
+		if(info->covariance.at<double>(0,0)<=0.0)
+		{
+			info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
+		}

 		return _pose *= t; // update
 	}
@@ -168,8 +168,7 @@ Transform OdometryF2F::computeTransform(
 		//return Identity
 		output = Transform::getIdentity();
 		// a very high variance tells that the new pose is not linked with the previous one
-		regInfo.varianceLin = 9999;
-		regInfo.varianceAng = 9999;
+		regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
 	}

 	if(!output.isNull())
@@ -245,8 +244,7 @@ Transform OdometryF2F::computeTransform(
 	if(info)
 	{
 		info->type = 1;
-		info->varianceLin = regInfo.varianceLin;
-		info->varianceAng = regInfo.varianceAng;
+		info->covariance = regInfo.covariance;
 		info->inliers = regInfo.inliers;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;
@@ -250,7 +250,8 @@ Transform OdometryF2M::computeTransform(
 						UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
 								uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());

-						bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.varianceAng, regInfo.varianceLin)));
+						regInfo.covariance *= transform.getNorm();
+						bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
 						bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));

 						UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
@@ -735,8 +736,7 @@ Transform OdometryF2M::computeTransform(
 			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());

 			// a very high variance tells that the new pose is not linked with the previous one
-			regInfo.varianceLin = 9999;
-			regInfo.varianceAng = 9999;
+			regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;

 			bool frameValid = false;
 			Transform newFramePose = this->getPose(); // initial pose may be not identity...
@@ -874,8 +874,7 @@ Transform OdometryF2M::computeTransform(

 	if(info)
 	{
-		info->varianceLin = regInfo.varianceLin;
-		info->varianceAng = regInfo.varianceAng;
+		info->covariance = regInfo.covariance;
 		info->inliers = regInfo.inliers;
 		info->matches = regInfo.matches;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
@@ -899,8 +898,8 @@ Transform OdometryF2M::computeTransform(
 			nFeatures,
 			regInfo.inliers,
 			regInfo.matches,
-			regInfo.varianceLin,
-			regInfo.varianceAng,
+			regInfo.covariance.at<double>(0,0),
+			regInfo.covariance.at<double>(5,5),
 			regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
 			regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().cols:0);

@@ -976,8 +976,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		if(info)
 		{
 			// a very high variance tells that the new pose is not linked with the previous one
-			info->varianceLin = 9999;
-			info->varianceAng = 9999;
+			info->covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
 		}

 			// generate kpts
@@ -102,10 +102,8 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
 		// a null pose notify that odometry could not be computed
-		double varianceLin = info.varianceLin>0?info.varianceLin:1;
-		double varianceAng = info.varianceAng>0?info.varianceAng:1;
 		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
-		this->post(new OdometryEvent(data, pose, varianceAng, varianceLin, info));
+		this->post(new OdometryEvent(data, pose, info));
 	}
 }

@@ -665,10 +665,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				UASSERT(!iter->second.transform().isNull());

 				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
-				if(!isCovarianceIgnored())
-				{
-					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
-				}
+				memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));

 				// between cameras, not base_link
 				Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
@@ -777,7 +774,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 		UASSERT(optimizer.verifyInformationMatrices());

-		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%f)", iterations(), this->epsilon(), robustKernelDelta_);
+		UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);

 		int it = 0;
 		UTimer timer;
@@ -446,22 +446,20 @@ bool OptimizerTORO::loadGraph(
 				float roll = uStr2Float(strList[6]);
 				float pitch = uStr2Float(strList[7]);
 				float yaw = uStr2Float(strList[8]);
-				float infR = uStr2Float(strList[9]);
-				float infP = uStr2Float(strList[15]);
-				float infW = uStr2Float(strList[20]);
-				UASSERT_MSG(infR > 0 && infP > 0 && infW > 0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
-				float rotVariance = infR<=infP && infR<=infW?infR:infP<=infW?infP:infW; // maximum variance
-				float infX = uStr2Float(strList[24]);
-				float infY = uStr2Float(strList[27]);
-				float infZ = uStr2Float(strList[29]);
-				UASSERT_MSG(infX > 0 && infY > 0 && infZ > 0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
-				float transVariance = 1.0f/(infX<=infY && infX<=infZ?infX:infY<=infW?infY:infZ); // maximum variance
-				UINFO("id=%d rotV=%f transV=%f", idFrom, rotVariance, transVariance);
+				cv::Mat informationMatrix(6,6,CV_64FC1);
+				informationMatrix.at<double>(3,3) = uStr2Float(strList[9]);
+				informationMatrix.at<double>(4,4) = uStr2Float(strList[15]);
+				informationMatrix.at<double>(5,5) = uStr2Float(strList[20]);
+				UASSERT_MSG(informationMatrix.at<double>(3,3) > 0.0 && informationMatrix.at<double>(4,4) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
+				informationMatrix.at<double>(0,0) = uStr2Float(strList[24]);
+				informationMatrix.at<double>(1,1) = uStr2Float(strList[27]);
+				informationMatrix.at<double>(2,2) = uStr2Float(strList[29]);
+				UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(2,2) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
 				Transform transform(x, y, z, roll, pitch, yaw);
 				if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
 				{
 					//Link type is unknown
-					Link link(idFrom, idTo, Link::kUndef, transform, rotVariance, transVariance);
+					Link link(idFrom, idTo, Link::kUndef, transform, informationMatrix);
 					edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
 				}
 				else
@@ -740,16 +740,19 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 					addParameter = oldIter->second.first;
 					if(addParameter)
 					{
-						key = oldIter->second.second;
-						UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
-								oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second, Parameters::getDefaultParameters().at(oldIter->second.second).c_str());
+						if(parameters.find(oldIter->second.second) == parameters.end())
+						{
+							key = oldIter->second.second;
+							UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
+									oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second, Parameters::getDefaultParameters().at(oldIter->second.second).c_str());
+						}
 					}
 					else if(oldIter->second.second.empty())
 					{
 						UWARN("Parameter \"%s\" doesn't exist anymore.",
 									oldIter->first.c_str());
 					}
-					else
+					else if(parameters.find(oldIter->second.second) == parameters.end())
 					{
 						UWARN("Parameter \"%s\" (value=%s) doesn't exist anymore, you may want to use this similar parameter \"%s (default=%s): %s\".",
 									oldIter->first.c_str(), iter->second, oldIter->second.second.c_str(), Parameters::getDefaultParameters().at(oldIter->second.second).c_str(), Parameters::getDescription(oldIter->second.second).c_str());
@@ -197,16 +197,16 @@ Transform Registration::computeTransformationMod(
 	{
 		if(info.icpInliersRatio)
 		{
-			info.varianceLin = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
-			info.varianceAng = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
+			info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0);
 		}
 		else
 		{
-			info.varianceLin = info.inliers > 0?1.0f/float(info.inliers):1.0f;
-			info.varianceAng = info.inliers > 0?1.0f/float(info.inliers):1.0f;
+			info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.inliers > 0?1.0/double(info.inliers):1.0);
+		}
+		if(info.covariance.at<double>(0,0)<0.0001)
+		{
+			info.covariance =cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
 		}
-		info.varianceLin = info.varianceLin>0.0f?info.varianceLin:0.0001f; // epsilon if exact transform
-		info.varianceAng = info.varianceAng>0.0f?info.varianceAng:0.0001f; // epsilon if exact transform
 	}

 	if(child_)
@@ -360,8 +360,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);

-					info.varianceLin = variance>0.0f?variance:0.0001; // epsilon if exact transform
-					info.varianceAng = variance>0.0f?variance:0.0001; // epsilon if exact transform
+					info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(variance>0.0001?variance:0.0001); // epsilon if exact transform
 					info.icpInliersRatio = correspondencesRatio;

 					if(correspondencesRatio < _correspondenceRatio)
@@ -1045,7 +1045,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	// Motion estimation
 	/////////////////////
 	Transform transform;
-	float variance = 1.0f;
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
 	int inliersCount = 0;
 	int matchesCount = 0;
 	if(toSignature.getWords().size())
@@ -1053,7 +1053,9 @@ Transform RegistrationVis::computeTransformationImpl(
 		Transform transforms[2];
 		std::vector<int> inliers[2];
 		std::vector<int> matches[2];
-		double variances[2] = {1.0f};
+		cv::Mat covariances[2];
+		covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
+		covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
 		for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
 		{
 			// A to B
@@ -1086,6 +1088,7 @@ Transform RegistrationVis::computeTransformationImpl(

 					// we only need the camera transform, send guess words3 for scale estimation
 					Transform cameraTransform;
+					double variance = 1.0f;
 					std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
 							uMultimapToMapUnique(signatureA->getWords()),
 							uMultimapToMapUnique(signatureB->getWords()),
@@ -1098,21 +1101,21 @@ Transform RegistrationVis::computeTransformationImpl(
 							1.0f,
 							0.99f,
 							uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
-							&variances[dir]);
-
+							&variance);
+					covariances[dir] *= variance;
 					inliers[dir] = uKeys(inliers3D);

 					if(!cameraTransform.isNull())
 					{
 						if((int)inliers3D.size() >= _minInliers)
 						{
-							if(variances[dir] <= _epipolarGeometryVar)
+							if(variance <= _epipolarGeometryVar)
 							{
 								transforms[dir] = cameraTransform;
 							}
 							else
 							{
-								msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variances[dir]);
+								msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
 								UINFO(msg.c_str());
 							}
 						}
@@ -1175,7 +1178,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 								uMultimapToMapUnique(signatureB->getWords3()),
-								varianceFromInliersCount()?0:&variances[dir],
+								varianceFromInliersCount()?0:&covariances[dir],
 								&matchesV,
 								&inliersV);
 						inliers[dir] = inliersV;
@@ -1212,7 +1215,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							_inlierDistance,
 							_iterations,
 							_refineIterations,
-							&variances[dir],
+							&covariances[dir],
 							&matchesV,
 							&inliersV);
 					inliers[dir] = inliersV;
@@ -1288,10 +1291,10 @@ Transform RegistrationVis::computeTransformationImpl(
 			poses.insert(std::make_pair(1, Transform::getIdentity()));
 			poses.insert(std::make_pair(2, transforms[0]));

-			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], variances[0], variances[0])));
+			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], (covariances[0]*transforms[0].getNorm()).inv())));
 			if(!transforms[1].isNull() && inliers[1].size())
 			{
-				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], variances[1], variances[1])));
+				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], (covariances[1]*transforms[1].getNorm()).inv())));
 			}

 			std::map<int, Transform> optimizedPoses;
@@ -1422,18 +1425,18 @@ Transform RegistrationVis::computeTransformationImpl(
 			if(transforms[0].isNull())
 			{
 				transform = transforms[1];
-				variance = variances[1];
+				covariance = covariances[1];
 			}
 			else
 			{
 				transform = transforms[0].interpolate(0.5f, transforms[1]);
-				variance = (variances[0]+variances[1])/2.0f;
+				covariance = (covariances[0]+covariances[1])/2.0f;
 			}
 		}
 		else
 		{
 			transform = transforms[0];
-			variance = variances[0];
+			covariance = covariances[0];
 		}
 	}
 	else if(toSignature.sensorData().isValid())
@@ -1447,8 +1450,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.inliers = inliersCount;
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
-	info.varianceLin = variance>0.0f?variance:0.0001f; // epsilon if exact transform
-	info.varianceAng = info.varianceLin;
+	info.covariance = covariance.at<double>(0,0)>0.0001?covariance:cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform

 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;
@@ -768,7 +768,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 				int m, w;
 				std::string l;
 				double stamp = 0.0;
-				_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, true);
+				std::vector<float> v;
+				_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, true);
 				stamps.insert(std::make_pair(iter->first, stamp));
 			}
 		}
@@ -827,6 +828,7 @@ bool Rtabmap::process(
 			Transform odomPose,
 			float odomLinearVariance,
 			float odomAngularVariance,
+			const std::vector<float> & odomVelocity,
 			const std::map<std::string, float> & externalStats)
 {
 	if(!odomPose.isNull())
@@ -841,12 +843,13 @@ bool Rtabmap::process(
 	covariance.at<double>(3,3) = odomAngularVariance;
 	covariance.at<double>(4,4) = odomAngularVariance;
 	covariance.at<double>(5,5) = odomAngularVariance;
-	return process(data, odomPose, covariance, externalStats);
+	return process(data, odomPose, covariance, odomVelocity, externalStats);
 }
 bool Rtabmap::process(
 		const SensorData & data,
 		Transform odomPose,
 		const cv::Mat & odomCovariance,
+		const std::vector<float> & odomVelocity,
 		const std::map<std::string, float> & externalStats)
 {
 	UDEBUG("");
@@ -985,14 +988,14 @@ bool Rtabmap::process(
 	ULOGGER_INFO("Updating memory...");
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->update(data, odomPose, odomCovariance, &statistics_))
+		if(!_memory->update(data, odomPose, odomCovariance, odomVelocity, &statistics_))
 		{
 			return false;
 		}
 	}
 	else
 	{
-		if(!_memory->update(data, Transform(), cv::Mat(), &statistics_))
+		if(!_memory->update(data, Transform(), cv::Mat(), std::vector<float>(), &statistics_))
 		{
 			return false;
 		}
@@ -1076,7 +1079,7 @@ bool Rtabmap::process(
 					{
 						// set small variance
 						UDEBUG("Set small variance. The robot is not moving.");
-						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
+						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, cv::Mat::eye(6,6,CV_64FC1)*1000));
 					}
 				}
 				else
@@ -1094,12 +1097,12 @@ bool Rtabmap::process(
 							UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
 									oldId,
 									signature->id(),
-									info.varianceLin,
-									info.varianceAng,
+									info.covariance.at<double>(0,0),
+									info.covariance.at<double>(5,5),
 									guess.prettyPrint().c_str(),
 									t.prettyPrint().c_str());
-							UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
-							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.varianceAng, info.varianceLin));
+							UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));

 							if(_optimizeFromGraphEnd)
 							{
@@ -1120,9 +1123,9 @@ bool Rtabmap::process(
 						else
 						{
 							UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
-							if(info.varianceLin > 0 && info.varianceAng > 0)
+							if(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0)
 							{
-								_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrt(info.varianceAng), sqrt(info.varianceLin)));
+								_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 							}
 						}
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1265,8 +1268,8 @@ bool Rtabmap::process(
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
-						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.varianceAng, info.varianceLin)))
+						UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.covariance.inv())))
 						{
 							++proximityDetectionsInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1822,7 +1825,7 @@ bool Rtabmap::process(
 		//Compute transform if metric data are present
 		Transform transform;
 		RegistrationInfo info;
-		info.varianceLin = info.varianceAng = 1.0f;
+		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
 		if(_rgbdSlamMode)
 		{
 			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
@@ -1842,8 +1845,8 @@ bool Rtabmap::process(
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
-			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.varianceAng, info.varianceLin));
+			UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.covariance.inv()));
 			if(!rejectedHypothesis)
 			{
 				loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1948,8 +1951,8 @@ bool Rtabmap::process(
 											signature->id(),
 											nearestId,
 											transform.prettyPrint().c_str());
-									UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
-									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.varianceAng, info.varianceLin));
+									UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.covariance.inv()));
 									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

 									if(loopClosureVisualInliers == 0)
@@ -2077,8 +2080,8 @@ bool Rtabmap::process(
 											}

 											// set Identify covariance for laser scan matching only
-											UASSERT(info.varianceLin>0.0 && info.varianceAng>0.0);
-											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrt(info.varianceAng), sqrt(info.varianceLin), scanMatchingIds));
+											UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, (info.covariance*100.0).inv(), scanMatchingIds));
 											loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

 											++proximityDetectionsAddedByICPOnly;
@@ -2130,7 +2133,7 @@ bool Rtabmap::process(

 			if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
 			{
-				_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // set high variance
+				_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
 			}
 			else
 			{
@@ -2649,8 +2652,8 @@ bool Rtabmap::process(
 			std::string label;
 			double stamp = 0;
 			Transform groundTruth;
-			std::vector<unsigned char> userData;
-			_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, false);
+			std::vector<float> velocity;
+			_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, false);
 			signatures.insert(std::make_pair(iter->first,
 					Signature(iter->first,
 							mapId,
@@ -2659,6 +2662,10 @@ bool Rtabmap::process(
 							label,
 							odomPoseLocal,
 							groundTruth)));
+			if(!velocity.empty())
+			{
+				signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
+			}
 		}
 		localGraphSize = (int)poses.size();
 		if(!lastSignatureLocalizedPose.isNull())
@@ -3327,7 +3334,8 @@ void Rtabmap::get3DMap(
 			std::string label;
 			double stamp = 0;
 			Transform groundTruth;
-			_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, true);
+			std::vector<float> velocity;
+			_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, true);
 			SensorData data = _memory->getNodeData(*iter);
 			data.setId(*iter);
 			std::multimap<int, cv::KeyPoint> words;
@@ -3346,6 +3354,10 @@ void Rtabmap::get3DMap(
 			signatures.at(*iter).setWords(words);
 			signatures.at(*iter).setWords3(words3);
 			signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
+			if(!velocity.empty())
+			{
+				signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
+			}
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3397,7 +3409,8 @@ void Rtabmap::getGraph(
 				std::string label;
 				double stamp = 0;
 				Transform groundTruth;
-				_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, global);
+				std::vector<float> velocity;
+				_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, global);
 				signatures->insert(std::make_pair(iter->first,
 						Signature(iter->first,
 							mapId,
@@ -3420,6 +3433,11 @@ void Rtabmap::getGraph(
 				_memory->getNodeCalibration(iter->first, models, stereoModel);
 				signatures->at(iter->first).sensorData().setCameraModels(models);
 				signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel);
+
+				if(!velocity.empty())
+				{
+					signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
+				}
 			}
 		}
 	}
@@ -3534,7 +3552,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 								}
 							}
 							std::multimap<int, Link> linksIn = links;
-							linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin)));
+							linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.covariance.inv())));
 							const Link * maxLinearLink = 0;
 							const Link * maxAngularLink = 0;
 							float maxLinearError = 0.0f;
@@ -3627,8 +3645,9 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 							UINFO("Added new loop closure between %d and %d.", from, to);
 							addedLinks.insert(from);
 							addedLinks.insert(to);
-							links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin)));
-							loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin));
+							cv::Mat inf = info.covariance.inv();
+							links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
+							loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
 							UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
 						}
 					}
@@ -3712,7 +3731,7 @@ int Rtabmap::refineLinks()

 		if(!t.isNull())
 		{
-			linksRefined.push_back(Link(from, to, iter->second.type(), t, info.varianceAng, info.varianceLin));
+			linksRefined.push_back(Link(from, to, iter->second.type(), t, info.covariance.inv()));
 			UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
 		}
 	}
@@ -4104,7 +4123,7 @@ void Rtabmap::updateGoalIndex()
 							if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
 							{
 								Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-								_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // on the optimized path
+								_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
 								UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 							}
 						}
@@ -51,9 +51,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_previousStamp(0.0),
 		_rtabmap(rtabmap),
 		_paused(false),
-		lastPose_(Transform::getIdentity()),
-		_rotVariance(0),
-		_transVariance(0)
+		lastPose_(Transform::getIdentity())

 {
 	UASSERT(rtabmap != 0);
@@ -88,8 +86,7 @@ void RtabmapThread::clearBufferedData()
 	{
 		_dataBuffer.clear();
 		lastPose_.setIdentity();
-		_rotVariance = 0;
-		_transVariance = 0;
+		covariance_ = cv::Mat();
 		_previousStamp = 0;
 	}
 	_dataMutex.unlock();
@@ -338,7 +335,9 @@ bool RtabmapThread::handleEvent(UEvent* event)
 				{
 					if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
 					{
-						this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
+						OdometryInfo infoCov;
+						infoCov.covariance = e->info().odomCovariance;
+						this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 					}
 					else
 					{
@@ -347,7 +346,9 @@ bool RtabmapThread::handleEvent(UEvent* event)
 				}
 				else
 				{ 
-					this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
+					OdometryInfo infoCov;
+					infoCov.covariance = e->info().odomCovariance;
+					this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 				}
 				
 			}
@@ -531,7 +532,7 @@ void RtabmapThread::process()
 		if(_rtabmap->getMemory())
 		{
 			bool wasPlanning = _rtabmap->getPath().size()>0;
-			if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
+			if(_rtabmap->process(data.data(), data.pose(), data.covariance(), data.velocity()))
 			{
 				Statistics stats = _rtabmap->getStatistics();
 				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
@@ -569,8 +570,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		if(!lastPose_.isIdentity() &&
 						(odomEvent.pose().isIdentity() ||
-						odomEvent.info().varianceLin>=9999 ||
-						odomEvent.info().varianceAng>=9999 ||
 						odomEvent.rotVariance()>=9999 ||
 						odomEvent.transVariance()>=9999))
 		{
@@ -580,25 +579,21 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			}
 			else
 			{
-				UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!",
-						odomEvent.info().varianceLin>odomEvent.transVariance()?odomEvent.info().varianceLin:odomEvent.transVariance(),
-						odomEvent.info().varianceAng>odomEvent.rotVariance()?odomEvent.info().varianceAng:odomEvent.rotVariance());
+				UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!", odomEvent.transVariance(), odomEvent.rotVariance());
 			}
 			pushNewState(kStateTriggeringMap);
-			_rotVariance = 0;
-			_transVariance = 0;
+			covariance_ = cv::Mat();
 		}

 		double maxRotVar = odomEvent.rotVariance();
 		double maxTransVar = odomEvent.transVariance();
-		// FIXME: should merge the transformations/variances like Link::merge();
-		if(maxRotVar != 1.0f)
+		if(maxRotVar != 1.0f && maxTransVar != 1.0f && !covariance_.empty())
 		{
-			_rotVariance += maxRotVar;
+			covariance_ = (covariance_.inv() + odomEvent.covariance().inv()).inv();
 		}
-		if(maxTransVar != 1.0f)
+		else
 		{
-			_transVariance += maxTransVar;
+			covariance_ = odomEvent.covariance();
 		}

 		if(ignoreFrame && !_createIntermediateNodes)
@@ -614,31 +609,28 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		lastPose_ = odomEvent.pose();

 		bool notify = true;
-		
-		if(_rotVariance <= 0)
+
+		if(covariance_.empty())
 		{
-			_rotVariance = 1.0;
-		}
-		if(_transVariance <= 0)
-		{
-			_transVariance = 1.0;
+			covariance_ = cv::Mat::eye(6,6,CV_64FC1);
 		}
+		OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
+		odomInfo.covariance = covariance_;
 		if(ignoreFrame)
 		{
 			// set negative id so rtabmap will detect it as an intermediate node
 			SensorData tmp = odomEvent.data();
 			tmp.setId(-1);
 			tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
-			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
+			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
 		}
 		else
 		{
-			_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
+			_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), odomInfo));
 		}
-		UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
+		UINFO("Added data %d", odomEvent.data().id());

-		_rotVariance = 0;
-		_transVariance = 0;
+		covariance_ = cv::Mat();
 		while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
 		{
 			if(_rate > 0.0f)
@@ -18,8 +18,9 @@ CREATE TABLE Node (
 	map_id INTEGER NOT NULL,
 	weight INTEGER,
 	stamp FLOAT,
-	pose BLOB,
-	ground_truth_pose BLOB,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
 	label TEXT,
 	
 	time_enter DATE,
@@ -51,9 +52,8 @@ CREATE TABLE Link (
 	from_id INTEGER NOT NULL,
 	to_id INTEGER NOT NULL,
 	type INTEGER NOT NULL,    -- neighbor=0, loop=1, child=2
-	rot_variance FLOAT NOT NULL,
-	trans_variance FLOAT NOT NULL,
-	transform BLOB,
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
 	user_data BLOB,           -- compressed data (User data)
 	FOREIGN KEY (from_id) REFERENCES Node(id),
 	FOREIGN KEY (to_id) REFERENCES Node(id)
@@ -68,7 +68,7 @@ CREATE TABLE Word (
 	PRIMARY KEY (id)
 );

-CREATE TABLE Map_Node_Word (
+CREATE TABLE Feature (
 	node_id INTEGER NOT NULL,
 	word_id INTEGER NOT NULL,
 	pos_x FLOAT NOT NULL,
@@ -111,10 +111,10 @@ CREATE TABLE Admin (
 -- *******************************************************************
 -- TRIGGERS
 -- *******************************************************************
-CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word 
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
 WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
 BEGIN
- SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
 END;

 --   Creating a trigger for time_enter
@@ -137,7 +137,7 @@ END;
 -- INDEXES
 -- *******************************************************************
 CREATE UNIQUE INDEX IDX_Node_id on Node (id);
-CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
 CREATE INDEX IDX_Link_from_id on Link (from_id);
 CREATE UNIQUE INDEX IDX_node_label on Node (label);
 CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
@@ -56,7 +56,7 @@ Transform estimateMotion3DTo2D(
 			int refineIterations,
 			const Transform & guess,
 			const std::map<int, cv::Point3f> & words3B,
-			double * varianceOut,
+			cv::Mat * covariance,
 			std::vector<int> * matchesOut,
 			std::vector<int> * inliersOut)
 {
@@ -65,9 +65,9 @@ Transform estimateMotion3DTo2D(
 	Transform transform;
 	std::vector<int> matches, inliers;

-	if(varianceOut)
+	if(covariance)
 	{
-		*varianceOut = 1.0;
+		*covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}

 	// find correspondences
@@ -138,7 +138,7 @@ Transform estimateMotion3DTo2D(
 			transform = (cameraModel.localTransform() * pnp).inverse();

 			// compute variance (like in PCL computeVariance() method of sac_model.h)
-			if(varianceOut && words3B.size())
+			if(covariance && words3B.size())
 			{
 				std::vector<float> errorSqrdDists(inliers.size());
 				oi = 0;
@@ -162,10 +162,10 @@ Transform estimateMotion3DTo2D(
 				{
 					std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
 					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-					*varianceOut = 2.1981 * median_error_sqr;
+					*covariance *= 2.1981 * median_error_sqr;
 				}
 			}
-			else if(varianceOut)
+			else if(covariance)
 			{
 				// compute variance, which is the rms of reprojection errors
 				std::vector<cv::Point2f> imagePointsReproj;
@@ -175,7 +175,7 @@ Transform estimateMotion3DTo2D(
 				{
 					err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
 				}
-				*varianceOut = std::sqrt(err/float(inliers.size()));
+				*covariance *= std::sqrt(err/float(inliers.size()));
 			}
 		}
 	}
@@ -203,7 +203,7 @@ Transform estimateMotion3DTo3D(
 			double inliersDistance,
 			int iterations,
 			int refineIterations,
-			double * varianceOut,
+			cv::Mat * covariance,
 			std::vector<int> * matchesOut,
 			std::vector<int> * inliersOut)
 {
@@ -222,9 +222,9 @@ Transform estimateMotion3DTo3D(
 	UASSERT(inliers1.size() == inliers2.size());
 	UDEBUG("Unique correspondences = %d", (int)inliers1.size());

-	if(varianceOut)
+	if(covariance)
 	{
-		*varianceOut = 1.0;
+		*covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}

 	std::vector<int> inliers;
@@ -251,7 +251,7 @@ Transform estimateMotion3DTo3D(
 				refineIterations,
 				3.0,
 				&inliers,
-				varianceOut);
+				covariance);

 		if(!t.isNull() && (int)inliers.size() >= minInliers)
 		{
@@ -66,15 +66,15 @@ Transform transformFromXYZCorrespondences(
 		int refineIterations,
 		double refineSigma,
 		std::vector<int> * inliersOut,
-		double * varianceOut)
+		cv::Mat * covariance)
 {
 	//NOTE: this method is a mix of two methods:
 	//  - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
 	//  - refineModel() in pcl/sample_consensus/sac.h

-	if(varianceOut)
+	if(covariance)
 	{
-		*varianceOut = 1.0;
+		*covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 	Transform transform;
 	if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
@@ -199,9 +199,9 @@ Transform transformFromXYZCorrespondences(
 				{
 					*inliersOut = inliers;
 				}
-				if(varianceOut)
+				if(covariance)
 				{
-					*varianceOut = model->computeVariance();
+					*covariance *= model->computeVariance();
 				}

 				// get best transformation
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>

 #include <rtabmap/core/Link.h>
+#include <rtabmap/core/Signature.h>

 class Ui_DatabaseViewer;
 class QGraphicsScene;
@@ -88,9 +89,12 @@ private slots:
 	void generateGraph();
 	void exportDatabase();
 	void extractImages();
+	void exportPosesRaw();
+	void exportPosesRGBDSLAM();
+	void exportPosesKITTI();
+	void exportPosesTORO();
+	void exportPosesG2O();
 	void generateLocalGraph();
-	void generateTOROGraph();
-	void generateG2OGraph();
 	void regenerateLocalMaps();
 	void regenerateCurrentLocalMaps();
 	void view3DMap();
@@ -145,10 +149,8 @@ private:
 	void updateConstraintView(
 			const rtabmap::Link & link,
 			bool updateImageSliders = true,
-			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-			const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-			const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+			const Signature & signatureFrom = Signature(0),
+			const Signature & signatureTo = Signature(0));
 	void updateConstraintButtons();
 	Link findActiveLink(int from, int to);
 	bool containsLink(
@@ -160,6 +162,7 @@ private:
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
 	void refineConstraint(int from, int to,  bool silent);
 	bool addConstraint(int from, int to, bool silent);
+	void exportPoses(int format);

 private:
 	Ui_DatabaseViewer * ui_;
@@ -811,10 +811,12 @@ bool MainWindow::handleEvent(UEvent* anEvent)
 			emit cameraInfoReceived(cameraEvent->info());
 			if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
 			{
+				OdometryInfo odomInfo;
+				odomInfo.covariance = cameraEvent->info().odomCovariance;
 				if (!_processingOdometry && !_processingStatistics)
 				{
 					_processingOdometry = true; // if we receive too many odometry events!
-					OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
+					OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, odomInfo);
 					emit odometryReceived(tmp, false);
 				}
 				else
@@ -824,7 +826,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
 					data.setCameraModels(cameraEvent->data().cameraModels());
 					data.setStereoCameraModel(cameraEvent->data().stereoCameraModel());
 					data.setGroundTruth(cameraEvent->data().groundTruth());
-					OdometryEvent tmp(data, cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
+					OdometryEvent tmp(data, cameraEvent->info().odomPose, odomInfo);
 					emit odometryReceived(tmp, true);
 				}
 			}
@@ -845,7 +847,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
 			data.setCameraModels(odomEvent->data().cameraModels());
 			data.setStereoCameraModel(odomEvent->data().stereoCameraModel());
 			data.setGroundTruth(odomEvent->data().groundTruth());
-			OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info().copyWithoutData());
+			OdometryEvent tmp(data, odomEvent->pose(), odomEvent->info().copyWithoutData());
 			emit odometryReceived(tmp, true);
 		}
 	}
@@ -1302,10 +1304,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().varianceLin), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().varianceLin, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().varianceAng), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().varianceAng, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
@@ -5068,10 +5070,11 @@ void MainWindow::postProcessing()
 									if(!transform.isIdentity())
 									{
 										// normalize variance
-										info.varianceLin *= transform.getNorm();
-										info.varianceAng *= transform.getAngle();
-										info.varianceLin = info.varianceLin>0.0f?info.varianceLin:0.0001f; // epsilon if exact transform
-										info.varianceAng = info.varianceAng>0.0f?info.varianceAng:0.0001f; // epsilon if exact transform
+										info.covariance *= transform.getNorm();
+										if(info.covariance.at<double>(0,0)<=0.0)
+										{
+											info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
+										}
 									}

 									//optimize the graph to see if the new constraint is globally valid
@@ -5090,7 +5093,7 @@ void MainWindow::postProcessing()
 											}
 										}
 										std::multimap<int, Link> linksIn = _currentLinksMap;
-										linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.varianceAng, info.varianceLin)));
+										linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
 										const Link * maxLinearLink = 0;
 										const Link * maxAngularLink = 0;
 										float maxLinearError = 0.0f;
@@ -5186,7 +5189,7 @@ void MainWindow::postProcessing()
 										addedLinks.insert(from);
 										addedLinks.insert(to);

-										_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.varianceAng, info.varianceLin)));
+										_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
 										++loopClosuresAdded;
 										_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
 										QApplication::processEvents();
@@ -5282,7 +5285,7 @@ void MainWindow::postProcessing()

 						if(!transform.isNull())
 						{
-							Link newLink(from, to, iter->second.type(), transform, info.varianceAng, info.varianceLin);
+							Link newLink(from, to, iter->second.type(), transform, info.covariance.inv());
 							iter->second = newLink;
 						}
 						else
@@ -613,6 +613,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// Memory
 	_ui->general_checkBox_keepRawData->setObjectName(Parameters::kMemImageKept().c_str());
 	_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
+	_ui->general_checkBox_saveIntermediateNodeData->setObjectName(Parameters::kMemIntermediateNodeDataKept().c_str());
 	_ui->general_checkBox_keepDescriptors->setObjectName(Parameters::kMemRawDescriptorsKept().c_str());
 	_ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str());
 	_ui->general_checkBox_compressionParallelized->setObjectName(Parameters::kMemCompressionParallelized().c_str());
@@ -319,6 +319,10 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<fl
 			grp = list.at(0);
 			name = list.at(1);
 			unit = list.at(2);
+			for(int i=3; i<list.size(); ++i)
+			{
+				unit += "/" + list.at(i);
+			}
 		}
 		else if(list.size() == 2)
 		{
@@ -52,8 +52,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>381</width>
-           <height>196</height>
+           <width>408</width>
+           <height>208</height>
          </rect>
         </property>
         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -210,8 +210,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>381</width>
-           <height>196</height>
+           <width>408</width>
+           <height>208</height>
          </rect>
         </property>
         <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -484,13 +484,23 @@
     <x>0</x>
     <y>0</y>
     <width>1547</width>
-     <height>25</height>
+     <height>22</height>
    </rect>
   </property>
   <widget class="QMenu" name="menuFile">
    <property name="title">
     <string>File</string>
    </property>
+    <widget class="QMenu" name="menuExport_poses">
+     <property name="title">
+      <string>Export poses...</string>
+     </property>
+     <addaction name="actionRaw_format_txt"/>
+     <addaction name="actionRGBD_SLAM_format_txt"/>
+     <addaction name="actionKITTI_format_txt"/>
+     <addaction name="actionTORO_graph"/>
+     <addaction name="actionG2o_g2o"/>
+    </widget>
    <addaction name="actionOpen_database"/>
    <addaction name="separator"/>
    <addaction name="actionSave_config"/>
@@ -499,6 +509,7 @@
    <addaction name="actionExport_3D_laser_scans_ply_pcd"/>
    <addaction name="actionExport"/>
    <addaction name="actionExtract_images"/>
+    <addaction name="menuExport_poses"/>
    <addaction name="separator"/>
    <addaction name="actionQuit"/>
   </widget>
@@ -508,8 +519,6 @@
    </property>
    <addaction name="actionGenerate_graph_dot"/>
    <addaction name="actionGenerate_local_graph_dot"/>
-    <addaction name="actionGenerate_TORO_graph_graph"/>
-    <addaction name="actionGenerate_g2o_graph_g2o"/>
    <addaction name="separator"/>
    <addaction name="actionDetect_more_loop_closures"/>
    <addaction name="actionRefine_all_neighbor_links"/>
@@ -1010,8 +1019,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>339</width>
-          <height>186</height>
+          <width>324</width>
+          <height>188</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1146,8 +1155,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>326</width>
-          <height>609</height>
+          <width>280</width>
+          <height>666</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1509,8 +1518,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>339</width>
-          <height>173</height>
+          <width>201</width>
+          <height>126</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1609,8 +1618,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>326</width>
-          <height>249</height>
+          <width>186</width>
+          <height>496</height>
         </rect>
        </property>
        <attribute name="label">
@@ -2155,11 +2164,6 @@
    <string>View 3D map...</string>
   </property>
  </action>
-  <action name="actionGenerate_TORO_graph_graph">
-   <property name="text">
-    <string>Generate TORO graph (*.graph)...</string>
-   </property>
-  </action>
  <action name="actionRefine_all_neighbor_links">
   <property name="text">
    <string>Refine all neighbor links...</string>
@@ -2191,11 +2195,6 @@
    <string>Reset link and grid map changes</string>
   </property>
  </action>
-  <action name="actionGenerate_g2o_graph_g2o">
-   <property name="text">
-    <string>Generate g2o graph (*.g2o)...</string>
-   </property>
-  </action>
  <action name="actionView_3D_laser_scans">
   <property name="text">
    <string>View 2D-3D laser scans...</string>
@@ -2237,17 +2236,48 @@
    <string>Restore default GUI settings</string>
   </property>
  </action>
+  <action name="actionRaw_format_txt">
+   <property name="text">
+    <string>Raw format (*.txt)</string>
+   </property>
+  </action>
+  <action name="actionRGBD_SLAM_format_txt">
+   <property name="text">
+    <string>RGBD-SLAM format (*.txt)</string>
+   </property>
+  </action>
+  <action name="actionKITTI_format_txt">
+   <property name="text">
+    <string>KITTI format (*.txt)</string>
+   </property>
+  </action>
+  <action name="actionTORO_graph">
+   <property name="text">
+    <string>TORO (*.graph)</string>
+   </property>
+  </action>
+  <action name="actionG2o_g2o">
+   <property name="text">
+    <string>g2o (*.g2o)</string>
+   </property>
+  </action>
 </widget>
 <customwidgets>
  <customwidget>
   <class>rtabmap::ImageView</class>
   <extends>QWidget</extends>
-   <header>rtabmap/gui/ImageView.h</header>
+   <header>../include/rtabmap/gui/ImageView.h</header>
+  </customwidget>
+  <customwidget>
+   <class>rtabmap::StatsToolBox</class>
+   <extends>QWidget</extends>
+   <header>../include/rtabmap/gui/StatsToolBox.h</header>
+   <container>1</container>
  </customwidget>
  <customwidget>
   <class>rtabmap::GraphViewer</class>
   <extends>QGraphicsView</extends>
-   <header>rtabmap/gui/GraphViewer.h</header>
+   <header>../include/rtabmap/gui/GraphViewer.h</header>
  </customwidget>
  <customwidget>
   <class>rtabmap::ParametersToolBox</class>
@@ -2255,12 +2285,6 @@
   <header>ParametersToolBox.h</header>
   <container>1</container>
  </customwidget>
-  <customwidget>
-   <class>rtabmap::StatsToolBox</class>
-   <extends>QWidget</extends>
-   <header>rtabmap/gui/StatsToolBox.h</header>
-   <container>1</container>
-  </customwidget>
 </customwidgets>
 <resources/>
 <connections/>
@@ -63,9 +63,9 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>0</y>
+            <y>-667</y>
            <width>673</width>
-            <height>2718</height>
+            <height>2747</height>
           </rect>
          </property>
          <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>13</number>
+                <number>18</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -6124,7 +6124,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="15" column="0">
+                      <item row="16" column="0">
                       <widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
                        <property name="minimumSize">
                         <size>
@@ -6140,7 +6140,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="10" column="1">
+                      <item row="11" column="1">
                       <widget class="QLabel" name="label_retrieved_12">
                        <property name="text">
                         <string>Raw descriptors kept in memory.</string>
@@ -6153,7 +6153,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="10" column="0">
+                      <item row="11" column="0">
                       <widget class="QCheckBox" name="general_checkBox_keepDescriptors">
                        <property name="text">
                         <string/>
@@ -6163,7 +6163,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="13" column="1">
+                      <item row="14" column="1">
                       <widget class="QLabel" name="label_retrieved_13">
                        <property name="text">
                         <string>Image pre decimation. This option can be used to reduce image size before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).</string>
@@ -6222,7 +6222,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="11" column="1">
+                      <item row="12" column="1">
                       <widget class="QLabel" name="label_retrieved_9">
                        <property name="text">
                         <string>Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).</string>
@@ -6235,7 +6235,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="14" column="0">
+                      <item row="15" column="0">
                       <widget class="QSpinBox" name="spinBox_imagePostDecimation">
                        <property name="minimum">
                         <number>-16</number>
@@ -6281,7 +6281,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="14" column="1">
+                      <item row="15" column="1">
                       <widget class="QLabel" name="label_retrieved_6">
                        <property name="text">
                         <string>Image post decimation. This option can be used to save images in lower resolution (size/decimation). It is done on the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).</string>
@@ -6307,7 +6307,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="15" column="1">
+                      <item row="16" column="1">
                       <widget class="QLabel" name="label_retrieved_8">
                        <property name="text">
                         <string>If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
@@ -6320,7 +6320,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="11" column="0">
+                      <item row="12" column="0">
                       <widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
                        <property name="text">
                         <string/>
@@ -6330,7 +6330,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="13" column="0">
+                      <item row="14" column="0">
                       <widget class="QSpinBox" name="spinBox_imagePreDecimation">
                        <property name="minimum">
                         <number>-16</number>
@@ -6340,7 +6340,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="16" column="0">
+                      <item row="17" column="0">
                       <widget class="QSpinBox" name="general_spinBox_laserScanNormalK">
                        <property name="minimumSize">
                         <size>
@@ -6359,7 +6359,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="16" column="1">
+                      <item row="17" column="1">
                       <widget class="QLabel" name="label_retrieved_14">
                        <property name="text">
                         <string>If &gt; 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.</string>
@@ -6372,7 +6372,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="12" column="1">
+                      <item row="13" column="1">
                       <widget class="QLabel" name="label_retrieved_15">
                        <property name="text">
                         <string>Multi-threaded compression.</string>
@@ -6385,7 +6385,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="12" column="0">
+                      <item row="13" column="0">
                       <widget class="QCheckBox" name="general_checkBox_compressionParallelized">
                        <property name="text">
                         <string/>
@@ -6395,6 +6395,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
+                      <item row="10" column="1">
+                       <widget class="QLabel" name="label_retrieved_16">
+                        <property name="text">
+                         <string>Save intermediate node data.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_saveIntermediateNodeData">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                     </layout>
                    </item>
                    <item>
@@ -10058,7 +10081,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                             <item row="4" column="1">
                              <widget class="QLabel" name="label_357">
                               <property name="text">
-                                <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work with Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
+                                <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -12299,153 +12322,166 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                      <property name="title">
                       <string>g2o</string>
                      </property>
-                      <layout class="QGridLayout" name="gridLayout_70" columnstretch="0,1">
-                       <item row="1" column="0">
-                        <widget class="QComboBox" name="comboBox_g2o_optimizer">
-                         <item>
-                          <property name="text">
-                           <string>Levenberg</string>
-                          </property>
+                      <layout class="QVBoxLayout" name="verticalLayout_117">
+                       <item>
+                        <layout class="QGridLayout" name="gridLayout_70" columnstretch="0,1">
+                         <item row="0" column="0">
+                          <widget class="QComboBox" name="comboBox_g2o_solver">
+                           <item>
+                            <property name="text">
+                             <string>CSparse</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>PCG</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Cholmod</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Eigen</string>
+                            </property>
+                           </item>
+                          </widget>
                         </item>
-                         <item>
-                          <property name="text">
-                           <string>Gauss Newton</string>
-                          </property>
+                         <item row="0" column="1">
+                          <widget class="QLabel" name="label_107">
+                           <property name="text">
+                            <string>Solver.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
                         </item>
-                        </widget>
-                       </item>
-                       <item row="0" column="0">
-                        <widget class="QComboBox" name="comboBox_g2o_solver">
-                         <item>
-                          <property name="text">
-                           <string>CSparse</string>
-                          </property>
+                         <item row="1" column="0">
+                          <widget class="QComboBox" name="comboBox_g2o_optimizer">
+                           <item>
+                            <property name="text">
+                             <string>Levenberg</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Gauss Newton</string>
+                            </property>
+                           </item>
+                          </widget>
                         </item>
-                         <item>
-                          <property name="text">
-                           <string>PCG</string>
-                          </property>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_138">
+                           <property name="text">
+                            <string>Optimization algorithm.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
                         </item>
-                         <item>
-                          <property name="text">
-                           <string>Cholmod</string>
-                          </property>
-                         </item>
-                         <item>
-                          <property name="text">
-                           <string>Eigen</string>
-                          </property>
-                         </item>
-                        </widget>
+                        </layout>
                       </item>
-                       <item row="2" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_pixelVariance">
-                         <property name="decimals">
-                          <number>2</number>
-                         </property>
-                         <property name="minimum">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="maximum">
-                          <double>999.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.001000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.010000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="0" column="1">
-                        <widget class="QLabel" name="label_107">
-                         <property name="text">
-                          <string>Solver.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="1" column="1">
-                        <widget class="QLabel" name="label_138">
-                         <property name="text">
-                          <string>Optimization algorithm.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_148">
-                         <property name="text">
-                          <string>Pixel variance used for bundle adjustment.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="3" column="1">
-                        <widget class="QLabel" name="label_350">
-                         <property name="text">
-                          <string>Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="3" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_robustKernelDelta">
-                         <property name="decimals">
-                          <number>3</number>
-                         </property>
-                         <property name="minimum">
-                          <double>0.000000000000000</double>
-                         </property>
-                         <property name="maximum">
-                          <double>999.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>1.000000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>5.991000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_baseline">
-                         <property name="suffix">
-                          <string> m</string>
-                         </property>
-                         <property name="decimals">
-                          <number>4</number>
-                         </property>
-                         <property name="minimum">
-                          <double>0.000000000000000</double>
-                         </property>
-                         <property name="maximum">
-                          <double>9.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.075000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="1">
-                        <widget class="QLabel" name="label_352">
-                         <property name="text">
-                          <string>When doing bundle adjustment with RGB-D data, we can set a fake baseline to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_20">
+                         <property name="title">
+                          <string>Bundle Adjustement</string>
                         </property>
+                         <layout class="QGridLayout" name="gridLayout_84" columnstretch="0,1">
+                          <item row="0" column="0">
+                           <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_pixelVariance">
+                            <property name="decimals">
+                             <number>2</number>
+                            </property>
+                            <property name="minimum">
+                             <double>0.010000000000000</double>
+                            </property>
+                            <property name="maximum">
+                             <double>999.000000000000000</double>
+                            </property>
+                            <property name="singleStep">
+                             <double>0.001000000000000</double>
+                            </property>
+                            <property name="value">
+                             <double>0.010000000000000</double>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="1">
+                           <widget class="QLabel" name="label_148">
+                            <property name="text">
+                             <string>Pixel variance used for bundle adjustment.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="0">
+                           <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_robustKernelDelta">
+                            <property name="decimals">
+                             <number>3</number>
+                            </property>
+                            <property name="minimum">
+                             <double>0.000000000000000</double>
+                            </property>
+                            <property name="maximum">
+                             <double>999.000000000000000</double>
+                            </property>
+                            <property name="singleStep">
+                             <double>1.000000000000000</double>
+                            </property>
+                            <property name="value">
+                             <double>5.991000000000000</double>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="1">
+                           <widget class="QLabel" name="label_350">
+                            <property name="text">
+                             <string>Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="2" column="0">
+                           <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_baseline">
+                            <property name="suffix">
+                             <string> m</string>
+                            </property>
+                            <property name="decimals">
+                             <number>4</number>
+                            </property>
+                            <property name="minimum">
+                             <double>0.000000000000000</double>
+                            </property>
+                            <property name="maximum">
+                             <double>9.000000000000000</double>
+                            </property>
+                            <property name="singleStep">
+                             <double>0.010000000000000</double>
+                            </property>
+                            <property name="value">
+                             <double>0.075000000000000</double>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="2" column="1">
+                           <widget class="QLabel" name="label_352">
+                            <property name="text">
+                             <string>When doing bundle adjustment with RGB-D data, we can set a fake baseline to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                         </layout>
                        </widget>
                       </item>
                      </layout>
@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UDirectory.h"
@@ -53,6 +55,7 @@ void showUsage()
 			"  --output           Output directory. By default, results are saved in \"path\".\n"
 			"  --gt \"path\"        Ground truth path (e.g., ~/KITTI/devkit/cpp/data/odometry/poses/07.txt)\n"
 			"  --color            Use color images for stereo (image_2 and image_3 folders).\n"
+			"  --disp             Generate full disparity.\n"
 			"  --scan             Include velodyne scan in node's data.\n"
 			"  --scan_step #      Scan downsample step (default=10).\n"
 			"  --scan_voxel #.#   Scan voxel size (default 0.3 m).\n"
@@ -97,6 +100,7 @@ int main(int argc, char * argv[])
 	int mapUpdate = 10;
 	bool color = false;
 	bool scan = false;
+	bool disp = false;
 	int scanStep = 10;
 	float scanVoxel = 0.3f;
 	int scanNormalK = 20;
@@ -159,7 +163,11 @@ int main(int argc, char * argv[])
 			}
 			else if(std::strcmp(argv[i], "--scan") == 0)
 			{
-				color = true;
+				scan = true;
+			}
+			else if(std::strcmp(argv[i], "--disp") == 0)
+			{
+				disp = true;
 			}
 		}
 		parameters = Parameters::parseArguments(argc, argv);
@@ -173,6 +181,7 @@ int main(int argc, char * argv[])
 		else
 		{
 			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
 		}
 	}

@@ -214,6 +223,10 @@ int main(int argc, char * argv[])
 			return -1;
 		}
 	}
+	if(disp)
+	{
+		printf("   Disparity:         %s\n", disp?"true":"false");
+	}
 	if(scan)
 	{
 		pathScan = path+"/velodyne";
@@ -291,6 +304,10 @@ int main(int argc, char * argv[])
 				0.0f,
 				opticalRotation), parameters);
 	((CameraStereoImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
+	if(disp)
+	{
+		cameraThread.setStereoToDepth(true);
+	}
 	if(!gtPath.empty())
 	{
 		((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(gtPath, 2);
@@ -327,6 +344,7 @@ int main(int argc, char * argv[])
 		/////////////////////////////
 		// Processing dataset begin
 		/////////////////////////////
+		cv::Mat covariance;
 		while(data.isValid() && g_forever)
 		{
 			std::map<std::string, float> externalStats;
@@ -347,8 +365,10 @@ int main(int argc, char * argv[])
 			Transform pose = odom.process(data, &odomInfo);
 			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
 			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
-			float speed = odomInfo.transform.x()/odomInfo.interval*3.6;
-			externalStats.insert(std::make_pair("Odometry/Speed/ms", speed));
+			float speed = 0.0f;
+			if(odomInfo.interval>0.0)
+				speed = odomInfo.transform.x()/odomInfo.interval*3.6;
+			externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
 			externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
 			externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));

@@ -360,11 +380,21 @@ int main(int argc, char * argv[])
 				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 				processData = intermediateNodes;
 			}
+			if(covariance.empty())
+			{
+				covariance = odomInfo.covariance;
+			}
+			else
+			{
+				covariance = (covariance.inv() + odomInfo.covariance.inv()).inv();
+			}

 			timer.restart();
 			if(processData)
 			{
-				rtabmap.process(data, pose, odomInfo.varianceLin, odomInfo.varianceAng, externalStats);
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
+				covariance = cv::Mat();
 			}
 			double slamTime = timer.ticks();

@@ -400,161 +430,157 @@ int main(int argc, char * argv[])
 		{
 			// Log ground truth statistics (in TUM's RGBD-SLAM format)
 			std::map<int, Transform> groundTruth;
-			graph::importPoses(gtPath, 2, groundTruth);
-			if(poses.size() == groundTruth.size())
+
+			//align with ground truth for more meaningful results
+			pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+			cloud1.resize(poses.size());
+			cloud2.resize(poses.size());
+			int oi = 0;
+			int idFirst = 0;
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
-				//align with ground truth for more meaningful results
-				pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
-				cloud1.resize(poses.size());
-				cloud2.resize(poses.size());
-				int oi = 0;
-				int idFirst = 0;
-				for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, true);
+				if(!gtPose.isNull())
 				{
-					std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
-					if(iter2!=poses.end())
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+					if(oi==0)
 					{
-						if(oi==0)
-						{
-							idFirst = iter->first;
-						}
-						cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-						cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
+						idFirst = iter->first;
 					}
-				}
-
-				Transform t = Transform::getIdentity();
-				if(oi>5)
-				{
-					cloud1.resize(oi);
-					cloud2.resize(oi);
-
-					t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
-				}
-				else if(idFirst)
-				{
-					t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
-				}
-				if(!t.isIdentity())
-				{
-					for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-					{
-						iter->second = t * iter->second;
-					}
-				}
-
-				std::vector<float> translationalErrors(poses.size());
-				std::vector<float> rotationalErrors(poses.size());
-				float sumTranslationalErrors = 0.0f;
-				float sumRotationalErrors = 0.0f;
-				float sumSqrdTranslationalErrors = 0.0f;
-				float sumSqrdRotationalErrors = 0.0f;
-				float radToDegree = 180.0f / M_PI;
-				float translational_min = 0.0f;
-				float translational_max = 0.0f;
-				float rotational_min = 0.0f;
-				float rotational_max = 0.0f;
-				oi=0;
-				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
-					if(jter!=groundTruth.end())
-					{
-						Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
-						Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
-						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
-						rotationalErrors[oi] = a*radToDegree;
-						translationalErrors[oi] = iter->second.getDistance(jter->second);
-
-						sumTranslationalErrors+=translationalErrors[oi];
-						sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
-						sumRotationalErrors+=rotationalErrors[oi];
-						sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
-
-						if(oi == 0)
-						{
-							translational_min = translational_max = translationalErrors[oi];
-							rotational_min = rotational_max = rotationalErrors[oi];
-						}
-						else
-						{
-							if(translationalErrors[oi] < translational_min)
-							{
-								translational_min = translationalErrors[oi];
-							}
-							else if(translationalErrors[oi] > translational_max)
-							{
-								translational_max = translationalErrors[oi];
-							}
-
-							if(rotationalErrors[oi] < rotational_min)
-							{
-								rotational_min = rotationalErrors[oi];
-							}
-							else if(rotationalErrors[oi] > rotational_max)
-							{
-								rotational_max = rotationalErrors[oi];
-							}
-						}
-
-						++oi;
-					}
-				}
-				translationalErrors.resize(oi);
-				rotationalErrors.resize(oi);
-				if(oi)
-				{
-					float total = float(oi);
-					float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
-					float translational_mean = sumTranslationalErrors/total;
-					float translational_median = translationalErrors[oi/2];
-					float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
-
-					float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
-					float rotational_mean = sumRotationalErrors/total;
-					float rotational_median = rotationalErrors[oi/2];
-					float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
-
-					printf("Ground truth comparison:\n");
-					printf("  translational_rmse=   %f\n", translational_rmse);
-					printf("  translational_mean=   %f\n", translational_mean);
-					printf("  translational_median= %f\n", translational_median);
-					printf("  translational_std=    %f\n", translational_std);
-					printf("  translational_min=    %f\n", translational_min);
-					printf("  translational_max=    %f\n", translational_max);
-					printf("  rotational_rmse=      %f\n", rotational_rmse);
-					printf("  rotational_mean=      %f\n", rotational_mean);
-					printf("  rotational_median=    %f\n", rotational_median);
-					printf("  rotational_std=       %f\n", rotational_std);
-					printf("  rotational_min=       %f\n", rotational_min);
-					printf("  rotational_max=       %f\n", rotational_max);
-
-					pFile = 0;
-					std::string pathErrors = output+"/rtabmap_rmse"+seq+".txt";
-					pFile = fopen(pathErrors.c_str(),"w");
-					if(!pFile)
-					{
-						UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
-					}
-					fprintf(pFile, "Ground truth comparison:\n");
-					fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
-					fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
-					fprintf(pFile, "  translational_median= %f\n", translational_median);
-					fprintf(pFile, "  translational_std=    %f\n", translational_std);
-					fprintf(pFile, "  translational_min=    %f\n", translational_min);
-					fprintf(pFile, "  translational_max=    %f\n", translational_max);
-					fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
-					fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
-					fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
-					fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
-					fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
-					fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
-					fclose(pFile);
+					cloud1[oi] = pcl::PointXYZ(gtPose.x(), gtPose.y(), gtPose.z());
+					cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 				}
 			}
-			else
+
+			// compute KITTI statistics before aligning the poses
+			float t_err = 0.0f;
+			float r_err = 0.0f;
+			graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), t_err, r_err);
+			printf("Ground truth comparison:\n");
+			printf("   KITTI t_err = %f %%\n", t_err);
+			printf("   KITTI r_err = %f deg/m\n", r_err);
+
+			Transform t = Transform::getIdentity();
+			if(oi>5)
 			{
-				UWARN("Cannot compute ground truth statistics, the computed poses (%d) are not the same size as the ground truth (%d). Make sure to use option \"--Rtabmap/CreateIntermediateNodes true\".");
+				cloud1.resize(oi);
+				cloud2.resize(oi);
+
+				t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+			}
+			else if(idFirst)
+			{
+				t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
+			}
+			if(!t.isIdentity())
+			{
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					iter->second = t * iter->second;
+				}
+			}
+
+			std::vector<float> translationalErrors(poses.size());
+			std::vector<float> rotationalErrors(poses.size());
+			float sumTranslationalErrors = 0.0f;
+			float sumRotationalErrors = 0.0f;
+			float sumSqrdTranslationalErrors = 0.0f;
+			float sumSqrdRotationalErrors = 0.0f;
+			float radToDegree = 180.0f / M_PI;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			oi=0;
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
+				if(jter!=groundTruth.end())
+				{
+					Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+					rotationalErrors[oi] = a*radToDegree;
+					translationalErrors[oi] = iter->second.getDistance(jter->second);
+
+					sumTranslationalErrors+=translationalErrors[oi];
+					sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
+					sumRotationalErrors+=rotationalErrors[oi];
+					sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
+
+					if(oi == 0)
+					{
+						translational_min = translational_max = translationalErrors[oi];
+						rotational_min = rotational_max = rotationalErrors[oi];
+					}
+					else
+					{
+						if(translationalErrors[oi] < translational_min)
+						{
+							translational_min = translationalErrors[oi];
+						}
+						else if(translationalErrors[oi] > translational_max)
+						{
+							translational_max = translationalErrors[oi];
+						}
+
+						if(rotationalErrors[oi] < rotational_min)
+						{
+							rotational_min = rotationalErrors[oi];
+						}
+						else if(rotationalErrors[oi] > rotational_max)
+						{
+							rotational_max = rotationalErrors[oi];
+						}
+					}
+
+					++oi;
+				}
+			}
+			translationalErrors.resize(oi);
+			rotationalErrors.resize(oi);
+			if(oi)
+			{
+				float total = float(oi);
+				float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
+				float translational_mean = sumTranslationalErrors/total;
+				float translational_median = translationalErrors[oi/2];
+				float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
+
+				float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
+				float rotational_mean = sumRotationalErrors/total;
+				float rotational_median = rotationalErrors[oi/2];
+				float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
+
+				printf("  translational_rmse=   %f\n", translational_rmse);
+				printf("  rotational_rmse=      %f\n", rotational_rmse);
+
+				pFile = 0;
+				std::string pathErrors = output+"/rtabmap_rmse"+seq+".txt";
+				pFile = fopen(pathErrors.c_str(),"w");
+				if(!pFile)
+				{
+					UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+				}
+				fprintf(pFile, "Ground truth comparison:\n");
+				fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+				fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+				fprintf(pFile, "  translational_median= %f\n", translational_median);
+				fprintf(pFile, "  translational_std=    %f\n", translational_std);
+				fprintf(pFile, "  translational_min=    %f\n", translational_min);
+				fprintf(pFile, "  translational_max=    %f\n", translational_max);
+				fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+				fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+				fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+				fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+				fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+				fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+				fclose(pFile);
 			}
 		}
 	}