0.15: statistics are now compressed in database. Eigen issue: using linear() instead of rotation() to get rotation from affine3f. Increased loading speed of statistics in DatabaseViewer. Added graph:calcRMSE().

2026-09-01 17:10:26 +08:00 · 2017-11-09 12:03:01 -05:00
parent 8cdd138143
commit 71d9816f4b
22 changed files with 685 additions and 602 deletions
--- a/corelib/src/Compression.cpp
+++ b/corelib/src/Compression.cpp
@@ -270,4 +270,21 @@ cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
 	return data;
 }

+cv::Mat compressString(const std::string & str)
+{
+	// +1 to include null character
+	return compressData2(cv::Mat(1, str.size()+1, CV_8SC1, (void *)str.data()));
+}
+
+std::string uncompressString(const cv::Mat & bytes)
+{
+	cv::Mat strMat = uncompressData(bytes);
+	if(!strMat.empty())
+	{
+		UASSERT(strMat.type() == CV_8SC1 && strMat.rows == 1);
+		return (const char*)strMat.data;
+	}
+	return "";
+}
+
 } /* namespace rtabmap */
--- a/corelib/src/DBDriver.cpp
+++ b/corelib/src/DBDriver.cpp
@@ -254,6 +254,15 @@ std::map<std::string, float> DBDriver::getStatistics(int nodeId, double & stamp)
 	return statistics;
 }

+std::map<int, std::pair<std::map<std::string, float>, double> > DBDriver::getAllStatistics() const
+{
+	std::map<int, std::pair<std::map<std::string, float>, double> > statistics;
+	_dbSafeAccessMutex.lock();
+	statistics = getAllStatisticsQuery();
+	_dbSafeAccessMutex.unlock();
+	return statistics;
+}
+
 std::string DBDriver::getDatabaseVersion() const
 {
 	std::string version = "0.0.0";
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -1080,8 +1080,26 @@ std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, dou
 			rc = sqlite3_step(ppStmt);
 			if(rc == SQLITE_ROW)
 			{
-				stamp = sqlite3_column_double(ppStmt, 0);
-				std::string text((const char *)sqlite3_column_text(ppStmt, 1));
+				int index = 0;
+				stamp = sqlite3_column_double(ppStmt, index++);
+
+				std::string text;
+				if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
+				{
+					const void * dataPtr = 0;
+					int dataSize = 0;
+					dataPtr = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize>0 && dataPtr)
+					{
+						text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
+					}
+				}
+				else
+				{
+					text = (const char *)sqlite3_column_text(ppStmt, index++);
+				}
+
 				if(text.size())
 				{
 					data = Statistics::deserializeData(text);
@@ -1097,6 +1115,63 @@ std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, dou
 	return data;
 }

+std::map<int, std::pair<std::map<std::string, float>, double> > DBDriverSqlite3::getAllStatisticsQuery() const
+{
+	UDEBUG("");
+	std::map<int, std::pair<std::map<std::string, float>, double> > data;
+	if(_ppDb)
+	{
+		if(uStrNumCmp(_version, "0.11.11") >= 0)
+		{
+			std::stringstream query;
+
+			query << "SELECT id, stamp, data "
+				  << "FROM Statistics;";
+
+			int rc = SQLITE_OK;
+			sqlite3_stmt * ppStmt = 0;
+			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());
+			rc = sqlite3_step(ppStmt);
+			while(rc == SQLITE_ROW)
+			{
+				int index = 0;
+				int id = sqlite3_column_int(ppStmt, index++);
+				double stamp = sqlite3_column_double(ppStmt, index++);
+
+				std::string text;
+				if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
+				{
+					const void * dataPtr = 0;
+					int dataSize = 0;
+					dataPtr = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize>0 && dataPtr)
+					{
+						text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
+					}
+				}
+				else
+				{
+					text = (const char *)sqlite3_column_text(ppStmt, index++);
+				}
+
+				if(text.size())
+				{
+					data.insert(std::make_pair(id, std::make_pair(Statistics::deserializeData(text), stamp)));
+				}
+
+				rc = sqlite3_step(ppStmt);
+			}
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+	UDEBUG("");
+	return data;
+}
+
 void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	UDEBUG("load data for %d signatures", (int)signatures.size());
@@ -3767,8 +3842,19 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 				rc = sqlite3_bind_double(ppStmt, index++, statistics.stamp());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				rc = sqlite3_bind_text(ppStmt, index++, param.c_str(), -1, SQLITE_STATIC);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				cv::Mat compressedParam;
+				if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
+				{
+					compressedParam = compressString(param);
+					rc = sqlite3_bind_blob(ppStmt, index++, compressedParam.data, compressedParam.cols, SQLITE_STATIC);
+					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+				}
+				else
+				{
+					rc = sqlite3_bind_text(ppStmt, index++, param.c_str(), -1, 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);
--- a/corelib/src/DBDriverSqlite3.h
+++ b/corelib/src/DBDriverSqlite3.h
@@ -71,6 +71,7 @@ private:
 	virtual int getTotalDictionarySizeQuery() const;
 	virtual ParametersMap getLastParametersQuery() const;
 	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const;

 	virtual void executeNoResultQuery(const std::string & sql) const;

--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_registration.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/common.h>
@@ -642,6 +643,143 @@ void calcKittiSequenceErrors (
 }
 // KITTI evaluation end

+Transform calcRMSE (
+		const std::map<int, Transform> & groundTruth,
+		const std::map<int, Transform> & poses,
+		float & translational_rmse,
+		float & translational_mean,
+		float & translational_median,
+		float & translational_std,
+		float & translational_min,
+		float & translational_max,
+		float & rotational_rmse,
+		float & rotational_mean,
+		float & rotational_median,
+		float & rotational_std,
+		float & rotational_min,
+		float & rotational_max)
+{
+
+	translational_rmse = 0.0f;
+	translational_mean = 0.0f;
+	translational_median = 0.0f;
+	translational_std = 0.0f;
+	translational_min = 0.0f;
+	translational_max = 0.0f;
+
+	rotational_rmse = 0.0f;
+	rotational_mean = 0.0f;
+	rotational_median = 0.0f;
+	rotational_std = 0.0f;
+	rotational_min = 0.0f;
+	rotational_max = 0.0f;
+
+	//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)
+	{
+		std::map<int, Transform>::const_iterator jter=groundTruth.find(iter->first);
+		if(jter != groundTruth.end())
+		{
+			if(oi==0)
+			{
+				idFirst = iter->first;
+			}
+			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
+			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		}
+	}
+
+	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();
+	}
+
+	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;
+	oi=0;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
+		if(jter!=groundTruth.end())
+		{
+			Transform pose = t * iter->second;
+			Eigen::Vector3f xAxis(1,0,0);
+			Eigen::Vector3f vA = pose.toEigen3f().linear()*xAxis;
+			Eigen::Vector3f vB = jter->second.toEigen3f().linear()*xAxis;
+			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] = pose.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);
+		translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
+		translational_mean = sumTranslationalErrors/total;
+		translational_median = translationalErrors[oi/2];
+		translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
+
+		rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
+		rotational_mean = sumRotationalErrors/total;
+		rotational_median = rotationalErrors[oi/2];
+		rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
+	}
+	return t;
+}
+

 ////////////////////////////////////////////
 // Graph utilities
@@ -888,7 +1026,7 @@ std::map<int, Transform> radiusPosesFiltering(

 				std::set<int> cloudIndices;
 				const Transform & currentT = transforms.at(i);
-				Eigen::Vector3f vA = currentT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+				Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 				for(unsigned int j=0; j<kIndices.size(); ++j)
 				{
 					if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
@@ -897,7 +1035,7 @@ std::map<int, Transform> radiusPosesFiltering(
 						{
 							const Transform & checkT = transforms.at(kIndices[j]);
 							// same orientation?
-							Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+							Eigen::Vector3f vB = checkT.toEigen3f().linear()*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));
 							if(a <= angle)
 							{
@@ -993,7 +1131,7 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p

 			std::set<int> cloudIndices;
 			const Transform & currentT = transforms.at(i);
-			Eigen::Vector3f vA = currentT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+			Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 			for(unsigned int j=0; j<kIndices.size(); ++j)
 			{
 				if((int)i != kIndices[j])
@@ -1002,7 +1140,7 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
 					{
 						const Transform & checkT = transforms.at(kIndices[j]);
 						// same orientation?
-						Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+						Eigen::Vector3f vB = checkT.toEigen3f().linear()*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));
 						if(a <= angle)
 						{
@@ -1644,7 +1782,7 @@ std::list<std::pair<int, Transform> > computePath(
 					{
 						//Transform nextPose = iter->second;
 						//Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-previousIter->second.x(), nextPose.y()-previousIter->second.y(), nextPose.z()-previousIter->second.z(), 1.0f);
-						//Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
+						//Eigen::Vector4f v2 = nextPose.linear().toEigen4f()*Eigen::Vector4f(1,0,0,1);
 						//float angle = pcl::getAngle3D(v1, v2);
 						//float cost = angle ;
 						//UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost);
@@ -1803,7 +1941,7 @@ std::map<int, Transform> getPosesInRadius(
 		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);

-		Eigen::Vector3f vA = fromT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+		Eigen::Vector3f vA = fromT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);

 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
@@ -1813,7 +1951,7 @@ std::map<int, Transform> getPosesInRadius(
 				{
 					const Transform & checkT = nodes.at(ids[ind[i]]);
 					// same orientation?
-					Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					Eigen::Vector3f vB = checkT.toEigen3f().linear()*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));
 					if(a <= angle)
 					{
--- a/corelib/src/OptimizerG2O.cpp
+++ b/corelib/src/OptimizerG2O.cpp
@@ -268,7 +268,7 @@ std::map<int, Transform> OptimizerG2O::optimize(

 				Eigen::Affine3d a = iter->second.toEigen3d();
 				Eigen::Isometry3d pose;
-				pose = a.rotation();
+				pose = a.linear();
 				pose.translation() = a.translation();
 				v3->setEstimate(pose);
 				if(iter->first == rootId)
@@ -326,7 +326,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 						priorEdge->setVertex(0, v1);
 						Eigen::Affine3d a = iter->second.transform().toEigen3d();
 						Eigen::Isometry3d pose;
-						pose = a.rotation();
+						pose = a.linear();
 						pose.translation() = a.translation();
 						priorEdge->setMeasurement(pose);
 						priorEdge->setParameterId(0, PARAM_OFFSET);
@@ -430,7 +430,7 @@ std::map<int, Transform> OptimizerG2O::optimize(

 					Eigen::Affine3d a = iter->second.transform().toEigen3d();
 					Eigen::Isometry3d constraint;
-					constraint = a.rotation();
+					constraint = a.linear();
 					constraint.translation() = a.translation();

 #ifdef RTABMAP_VERTIGO
@@ -503,7 +503,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 								{
 									float roll, pitch, yaw;
 									iter->second.getEulerAngles(roll, pitch, yaw);
-									Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
+									Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().linear().angle());
 									tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
 									UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
 								}
@@ -599,7 +599,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				{
 					float roll, pitch, yaw;
 					iter->second.getEulerAngles(roll, pitch, yaw);
-					Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
+					Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().linear().angle());
 					optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
 					UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
 				}
@@ -729,9 +729,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			Eigen::Affine3d a = camPose.toEigen3d();
 #ifdef RTABMAP_ORB_SLAM2
 			a = a.inverse();
-			vCam->setEstimate(g2o::SE3Quat(a.rotation(), a.translation()));
+			vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
 #else
-			g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
+			g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
 			cam.setKcam(
 					iterModel->second.fx(),
 					iterModel->second.fy(),
@@ -794,7 +794,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					UASSERT(v2 != 0);
 					e->setVertex(0, v1);
 					e->setVertex(1, v2);
-					e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
+					e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
 					e->setInformation(information);

 					if (!optimizer.addEdge(e))
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -79,6 +79,7 @@ Rtabmap::Rtabmap() :
 	_publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
 	_publishPdf(Parameters::defaultRtabmapPublishPdf()),
 	_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
+	_computeRMSE(Parameters::defaultRtabmapComputeRMSE()),
 	_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
 	_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
 	_loopThr(Parameters::defaultRtabmapLoopThr()),
@@ -400,6 +401,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
+	Parameters::parse(parameters, Parameters::kRtabmapComputeRMSE(), _computeRMSE);
 	Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
 	Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
@@ -2672,6 +2674,7 @@ bool Rtabmap::process(
 			constraints = _constraints;
 		}
 		UDEBUG("Get all node infos...");
+		std::map<int, Transform> groundTruths;
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
 			Transform odomPoseLocal;
@@ -2696,6 +2699,10 @@ bool Rtabmap::process(
 				signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
 			}
 			signatures.at(iter->first).sensorData().setGPS(gps);
+			if(_computeRMSE && !groundTruth.isNull())
+			{
+				groundTruths.insert(std::make_pair(iter->first, groundTruth));
+			}
 		}
 		localGraphSize = (int)poses.size();
 		if(!lastSignatureLocalizedPose.isNull())
@@ -2706,6 +2713,52 @@ bool Rtabmap::process(
 		statistics_.setConstraints(constraints);
 		statistics_.setSignatures(signatures);
 		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
+
+		if(_computeRMSE && groundTruths.size())
+		{
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+
+			graph::calcRMSE(
+					groundTruths,
+					poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+
+			statistics_.addStatistic(Statistics::kGtTranslational_rmse(), translational_rmse);
+			statistics_.addStatistic(Statistics::kGtTranslational_mean(), translational_mean);
+			statistics_.addStatistic(Statistics::kGtTranslational_median(), translational_median);
+			statistics_.addStatistic(Statistics::kGtTranslational_std(), translational_std);
+			statistics_.addStatistic(Statistics::kGtTranslational_min(), translational_min);
+			statistics_.addStatistic(Statistics::kGtTranslational_max(), translational_max);
+			statistics_.addStatistic(Statistics::kGtRotational_rmse(), rotational_rmse);
+			statistics_.addStatistic(Statistics::kGtRotational_mean(), rotational_mean);
+			statistics_.addStatistic(Statistics::kGtRotational_median(), rotational_median);
+			statistics_.addStatistic(Statistics::kGtRotational_std(), rotational_std);
+			statistics_.addStatistic(Statistics::kGtRotational_min(), rotational_min);
+			statistics_.addStatistic(Statistics::kGtRotational_max(), rotational_max);
+
+		}
 		UDEBUG("");
 	}

@@ -3627,8 +3680,8 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 												fabs(iter->second.transform().x() - t.x()),
 												fabs(iter->second.transform().y() - t.y()),
 												fabs(iter->second.transform().z() - t.z()));
-										Eigen::Vector3f vA = t1.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
-										Eigen::Vector3f vB = t2.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+										Eigen::Vector3f vA = t1.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
+										Eigen::Vector3f vB = t2.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 										float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 										if(linearError > maxLinearError)
 										{
--- a/corelib/src/Transform.cpp
+++ b/corelib/src/Transform.cpp
@@ -212,7 +212,7 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 float Transform::getAngle(float x, float y, float z) const
 {
 	Eigen::Vector3f vA(x,y,z);
-	Eigen::Vector3f vB = this->toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+	Eigen::Vector3f vB = this->toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 	return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 }

@@ -328,12 +328,12 @@ Eigen::Affine3d Transform::toEigen3d() const

 Eigen::Quaternionf Transform::getQuaternionf() const
 {
-	return Eigen::Quaternionf(this->toEigen3f().rotation()).normalized();
+	return Eigen::Quaternionf(this->toEigen3f().linear()).normalized();
 }

 Eigen::Quaterniond Transform::getQuaterniond() const
 {
-	return Eigen::Quaterniond(this->toEigen3d().rotation()).normalized();
+	return Eigen::Quaterniond(this->toEigen3d().linear()).normalized();
 }

 Transform Transform::getIdentity()
--- a/corelib/src/resources/DatabaseSchema.sql.in
+++ b/corelib/src/resources/DatabaseSchema.sql.in
@@ -99,7 +99,7 @@ CREATE TABLE Info (
 CREATE TABLE Statistics (
 	id INTEGER NOT NULL,
 	stamp FLOAT,
-	data TEXT,	
+	data BLOB,	
 	FOREIGN KEY (id) REFERENCES Node(id)
 );