0.11.1: Added "ground_truth_pose" field to database in Node table. MainWindow: alignment of the map to ground truth (if ground truth is present).

corelib/include/rtabmap/core/DBDriver.h

@@ -119,7 +119,7 @@ public:
 	// Specific queries...
 	void loadNodeData(std::list<Signature *> & signatures) const;
 	void getNodeData(int signatureId, SensorData & data) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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;
@@ -169,7 +169,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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;

corelib/include/rtabmap/core/Memory.h

@@ -137,12 +137,14 @@ public:
 	std::string getDatabaseVersion() const;
 	double getDbSavingTime() const;
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
+	Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
 	bool getNodeInfo(int signatureId,
 			Transform & odomPose,
 			int & mapId,
 			int & weight,
 			std::string & label,
 			double & stamp,
+			Transform & groundTruth,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);

corelib/include/rtabmap/core/Signature.h

@@ -56,6 +56,7 @@ public:
 			double stamp = 0.0,
 			const std::string & label = std::string(),
 			const Transform & pose = Transform(),
+			const Transform & groundTruthPose = Transform(),
 			const SensorData & sensorData = SensorData());
 	Signature(const SensorData & data);
 	virtual ~Signature();
@@ -110,10 +111,12 @@ public:
 	//metric stuff
 	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;}
 
 	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;}
 
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}
@@ -138,6 +141,7 @@ private:
 	bool _enabled;
 
 	Transform _pose;
+	Transform _groundTruthPose;
 
 	SensorData _sensorData;
 };

corelib/src/CameraRGB.cpp

@@ -295,7 +295,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 				}
 				std::vector<double> values = uValues(stamps);
 
-				Transform firstPoseInv;
 				for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
 				{
 					Transform pose; // null transform
@@ -323,17 +322,9 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 							}
 						}
 					}
-					if(!pose.isNull())
+					if(pose.isNull())
 					{
-						if(firstPoseInv.isNull())
-						{
-							firstPoseInv = pose.inverse();
-							pose.setIdentity();
-						}
-						else
-						{
-							pose = firstPoseInv * pose;
-						}
+						UWARN("Ground truth pose not found for stamp %f", *ster);
 					}
 					groundTruth_.push_back(pose);
 				}

corelib/src/DBDriver.cpp

@@ -552,7 +552,8 @@ bool DBDriver::getNodeInfo(
 		int & mapId,
 		int & weight,
 		std::string & label,
-		double & stamp) const
+		double & stamp,
+		Transform & groundTruthPose) const
 {
 	bool found = false;
 	// look in the trash
@@ -564,6 +565,7 @@ bool DBDriver::getNodeInfo(
 		weight = _trashSignatures.at(signatureId)->getWeight();
 		label = _trashSignatures.at(signatureId)->getLabel();
 		stamp = _trashSignatures.at(signatureId)->getStamp();
+		groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -571,7 +573,7 @@ bool DBDriver::getNodeInfo(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;

corelib/src/DBDriverSqlite3.cpp

@@ -994,7 +994,8 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		int & mapId,
 		int & weight,
 		std::string & label,
-		double & stamp) const
+		double & stamp,
+		Transform & groundTruthPose) const
 {
 	bool found = false;
 	if(_ppDb && signatureId)
@@ -1003,7 +1004,14 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.8.5") >= 0)
+		if(uStrNumCmp(_version, "0.11.1") >= 0)
+		{
+			query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose "
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
 		{
 			query << "SELECT pose, map_id, weight, label, stamp "
 					 "FROM Node "
@@ -1050,6 +1058,16 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 				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)
+				{
+					memcpy(groundTruthPose.data(), data, dataSize);
+				}
+			}
+
 			rc = sqlite3_step(ppStmt); // next result...
 		}
 		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1414,7 +1432,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 		unsigned int loaded = 0;
 
 		// Load nodes information
-		if(uStrNumCmp(_version, "0.8.5") >= 0)
+		if(uStrNumCmp(_version, "0.11.1") >= 0)
+		{
+			query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
 		{
 			query << "SELECT id, map_id, weight, pose, stamp, label "
 				  << "FROM Node "
@@ -1442,6 +1466,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			double stamp = 0.0;
 			int weight = 0;
 			Transform pose;
+			Transform groundTruthPose;
 			const void * data = 0;
 			int dataSize = 0;
 			std::string label;
@@ -1472,6 +1497,16 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					}
 				}
 
+				if(uStrNumCmp(_version, "0.11.1") >= 0)
+				{
+					data = sqlite3_column_blob(ppStmt, index); // pose
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
+					{
+						memcpy(groundTruthPose.data(), data, dataSize);
+					}
+				}
+
 				rc = sqlite3_step(ppStmt);
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1486,7 +1521,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						weight,
 						stamp,
 						label,
-						pose);
+						pose,
+						groundTruthPose);
 				s->setSaved(true);
 				nodes.push_back(s);
 				++loaded;
@@ -2507,7 +2543,11 @@ void DBDriverSqlite3::updateLinkQuery(const Link & link) const
 
 std::string DBDriverSqlite3::queryStepNode() const
 {
-	if(uStrNumCmp(_version, "0.10.1") >= 0)
+	if(uStrNumCmp(_version, "0.11.1") >= 0)
+	{
+		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.10.1") >= 0)
 	{
 		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
 	}
@@ -2575,6 +2615,12 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 		}
 	}
 
+	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", sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBDriverSqlite3.h

@@ -83,7 +83,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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;

corelib/src/DBReader.cpp

@@ -211,8 +211,8 @@ void DBReader::mainLoop()
 				std::string label;
 				double stamp;
 				int mapId;
-				Transform localTransform, pose;
-				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
+				Transform localTransform, pose, groundTruth;
+				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
 				if(previousStamp && stamp && stamp > previousStamp)
 				{
 					delay = stamp - previousStamp;
@@ -285,7 +285,8 @@ OdometryEvent DBReader::getNextData()
 			int weight;
 			std::string label;
 			double stamp;
-			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
+			Transform groundTruth;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
 
 			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
@@ -369,6 +370,7 @@ OdometryEvent DBReader::getNextData()
 				data.uncompressData();
 				data.setId(seq);
 				data.setStamp(stamp);
+				data.setGroundTruth(groundTruth);
 				UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
 						data.laserScanRaw().empty()?0:1,
 						data.imageRaw().empty()?0:1,

corelib/src/Graph.cpp

@@ -220,6 +220,10 @@ bool importPoses(
 								-1, 0, 0, 0,
 								 0,-1, 0, 0);
 					pose = t * pose * t.inverse();
+					t = Transform( 0, 0, 1, 0,
+								   0, -1, 0, 0,
+								   1, 0, 0, 0);
+					pose = t*pose;
 					poses.insert(std::make_pair(id, pose));
 				}
 				else

corelib/src/Memory.cpp

@@ -2727,20 +2727,31 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 
 Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
 {
-	Transform pose;
+	Transform pose, groundTruth;
 	int mapId, weight;
 	std::string label;
 	double stamp;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, lookInDatabase);
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
 	return pose;
 }
 
+Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
+{
+	Transform pose, groundTruth;
+	int mapId, weight;
+	std::string label;
+	double stamp;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
+	return groundTruth;
+}
+
 bool Memory::getNodeInfo(int signatureId,
 		Transform & odomPose,
 		int & mapId,
 		int & weight,
 		std::string & label,
 		double & stamp,
+		Transform & groundTruth,
 		bool lookInDatabase) const
 {
 	const Signature * s = this->getSignature(signatureId);
@@ -2751,11 +2762,12 @@ bool Memory::getNodeInfo(int signatureId,
 		weight = s->getWeight();
 		label = s->getLabel();
 		stamp = s->getStamp();
+		groundTruth = s->getGroundTruthPose();
 		return true;
 	}
 	else if(lookInDatabase && _dbDriver)
 	{
-		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp);
+		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth);
 	}
 	return false;
 }
@@ -3289,6 +3301,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			data.stamp(),
 			"",
 			pose,
+			data.groundTruth(),
 			stereoCameraModel.isValid()?
 				SensorData(
 						ctDepth2d.getCompressedData(),
@@ -3319,6 +3332,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			data.stamp(),
 			"",
 			pose,
+			data.groundTruth(),
 			stereoCameraModel.isValid()?
 				SensorData(
 						cv::Mat(),
@@ -3339,6 +3353,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						id,
 						0));
 	}
+
 	s->setWords(words);
 	s->setWords3(words3D);
 	if(this->isRawDataKept())

corelib/src/OdometryF2F.cpp

@@ -179,6 +179,7 @@ Transform OdometryF2F::computeTransform(
 		info->type = 1;
 		info->variance = regInfo.variance;
 		info->inliers = regInfo.inliers;
+		info->matches = regInfo.matches;
 	}
 
 	UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",

corelib/src/Rtabmap.cpp

@@ -743,11 +743,11 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
-				Transform o;
+				Transform o,g;
 				int m, w;
 				std::string l;
 				double stamp = 0.0;
-				_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
+				_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, true);
 				stamps.insert(std::make_pair(iter->first, stamp));
 			}
 		}
@@ -2471,15 +2471,17 @@ bool Rtabmap::process(
 			int mapId = -1;
 			std::string label;
 			double stamp = 0;
+			Transform groundTruth;
 			std::vector<unsigned char> userData;
-			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, false);
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, false);
 			signatures.insert(std::make_pair(iter->first,
 					Signature(iter->first,
 							mapId,
 							weight,
 							stamp,
 							label,
-							odomPose)));
+							odomPose,
+							groundTruth)));
 		}
 		statistics_.setPoses(poses);
 		statistics_.setConstraints(constraints);
@@ -3090,7 +3092,8 @@ void Rtabmap::get3DMap(
 			int mapId = -1;
 			std::string label;
 			double stamp = 0;
-			_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, true);
+			Transform groundTruth;
+			_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, groundTruth, true);
 			SensorData data = _memory->getNodeData(*iter);
 			data.setId(*iter);
 			std::multimap<int, cv::KeyPoint> words;
@@ -3103,6 +3106,7 @@ void Rtabmap::get3DMap(
 							stamp,
 							label,
 							odomPose,
+							groundTruth,
 							data)));
 			signatures.at(*iter).setWords(words);
 			signatures.at(*iter).setWords3(words3);
@@ -3156,14 +3160,16 @@ void Rtabmap::getGraph(
 				int mapId = -1;
 				std::string label;
 				double stamp = 0;
-				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, global);
+				Transform groundTruth;
+				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, global);
 				signatures->insert(std::make_pair(iter->first,
 						Signature(iter->first,
 							mapId,
 							weight,
 							stamp,
 							label,
-							odomPose)));
+							odomPose,
+							groundTruth)));
 			}
 		}
 	}

corelib/src/Signature.cpp

@@ -55,6 +55,7 @@ Signature::Signature(
 		double stamp,
 		const std::string & label,
 		const Transform & pose,
+		const Transform & groundTruthPose,
 		const SensorData & sensorData):
 	_id(id),
 	_mapId(mapId),
@@ -66,6 +67,7 @@ Signature::Signature(
 	_linksModified(true),
 	_enabled(false),
 	_pose(pose),
+	_groundTruthPose(groundTruthPose),
 	_sensorData(sensorData)
 {
 	if(_sensorData.id() == 0)
@@ -86,6 +88,7 @@ Signature::Signature(const SensorData & data) :
 	_linksModified(true),
 	_enabled(false),
 	_pose(Transform::getIdentity()),
+	_groundTruthPose(data.groundTruth()),
 	_sensorData(data)
 {
 

corelib/src/resources/DatabaseSchema.sql.in

@@ -19,6 +19,7 @@ CREATE TABLE Node (
 	weight INTEGER,
 	stamp FLOAT,
 	pose BLOB,
+	ground_truth_pose BLOB,
 	label TEXT,
 	time_enter DATE,
 	PRIMARY KEY (id)