Some fixes:

-Memory: Fixed wrongly rejected scan matching transform because of too large correction
-DBViewer: Fixed not shown proximity scans
-DBDriver: Fixed not loaded links' user_data on getAllLinks() method
-MainWindow: Added planning statistics
-DBReader: set a maximum of 10 sec sleep if the map ID has changed

corelib/include/rtabmap/core/DBReader.h

@@ -79,6 +79,7 @@ private:
 	std::set<int> _ids;
 	std::set<int>::iterator _currentId;
 	double _previousStamp;
+	int _previousMapID;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -198,24 +198,36 @@ class RtabmapGlobalPathEvent : public UEvent
 {
 public:
 	RtabmapGlobalPathEvent():
-		UEvent(0) {}
-	RtabmapGlobalPathEvent(int goalId, const std::vector<std::pair<int, Transform> > & poses) :
-		UEvent(goalId),
-		_poses(poses) {}
-	RtabmapGlobalPathEvent(int goalId, const std::string & goalLabel, const std::vector<std::pair<int, Transform> > & poses) :
-			UEvent(goalId),
-			_goalLabel(goalLabel),
-			_poses(poses) {}
+		UEvent(0),
+		_planningTime(0.0) {}
+	RtabmapGlobalPathEvent(
+			int goalId,
+			const std::vector<std::pair<int, Transform> > & poses,
+			double planningTime) :
+				UEvent(goalId),
+				_poses(poses),
+				_planningTime(planningTime) {}
+	RtabmapGlobalPathEvent(
+			int goalId,
+			const std::string & goalLabel,
+			const std::vector<std::pair<int, Transform> > & poses,
+			double planningTime) :
+				UEvent(goalId),
+				_goalLabel(goalLabel),
+				_poses(poses),
+				_planningTime(planningTime) {}
 
 	virtual ~RtabmapGlobalPathEvent() {}
 	int getGoal() const {return this->getCode();}
 	const std::string & getGoalLabel() const {return _goalLabel;}
+	double getPlanningTime() const {return _planningTime;}
 	const std::vector<std::pair<int, Transform> > & getPoses() const {return _poses;}
 	virtual std::string getClassName() const {return std::string("RtabmapGlobalPathEvent");}
 
 private:
 	std::string _goalLabel;
 	std::vector<std::pair<int, Transform> > _poses;
+	double _planningTime;
 };
 
 class RtabmapLabelErrorEvent : public UEvent

corelib/src/DBDriverSqlite3.cpp

@@ -1118,7 +1118,11 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		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";
+		}
+		else if(uStrNumCmp(_version, "0.8.4") >= 0)
 		{
 			query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id";
 		}
@@ -1171,7 +1175,20 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
 				{
 					rotVariance = sqlite3_column_double(ppStmt, index++);
 					transVariance = sqlite3_column_double(ppStmt, index++);
-					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+
+					cv::Mat userDataCompressed;
+					if(uStrNumCmp(_version, "0.10.10") >= 0)
+					{
+						const void * data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						//Create the userData
+						if(dataSize>4 && data)
+						{
+							userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
+						}
+					}
+
+					links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
 				}
 				else if(uStrNumCmp(_version, "0.7.4") >= 0)
 				{
@@ -2946,7 +2963,7 @@ void DBDriverSqlite3::stepLink(
 		}
 		else
 		{
-			rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+			rc = sqlite3_bind_null(ppStmt, index++);
 		}
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}

corelib/src/DBReader.cpp

@@ -54,7 +54,8 @@ DBReader::DBReader(const std::string & databasePath,
 	_goalsIgnored(goalsIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
-	_previousStamp(0)
+	_previousStamp(0),
+	_previousMapID(0)
 {
 }
 
@@ -70,7 +71,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_goalsIgnored(goalsIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
-	_previousStamp(0)
+	_previousStamp(0),
+	_previousMapID(0)
 {
 }
 
@@ -94,6 +96,7 @@ bool DBReader::init(int startIndex)
 	_ids.clear();
 	_currentId=_ids.end();
 	_previousStamp = 0;
+	_previousMapID = 0;
 
 	if(_paths.size() == 0)
 	{
@@ -315,9 +318,15 @@ OdometryEvent DBReader::getNextData()
 					UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
 					this->kill();
 				}
-				else if(_previousStamp > 0)
+				else if(_previousMapID == mapId && _previousStamp > 0)
 				{
 					int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
+					if(sleepTime > 10000)
+					{
+						UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
+								sleepTime/1000, _previousStamp, stamp);
+						sleepTime = 10000;
+					}
 					if(sleepTime > 2)
 					{
 						uSleep(sleepTime-2);
@@ -334,6 +343,7 @@ OdometryEvent DBReader::getNextData()
 					UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
 				}
 				_previousStamp = stamp;
+				_previousMapID = mapId;
 			}
 			else if(_frameRate>0.0f)
 			{

corelib/src/Memory.cpp

@@ -1992,6 +1992,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 		if(s)
 		{
 			s->setLabel(label);
+			UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 			return true;
 		}
 		else if(_dbDriver)
@@ -2003,6 +2004,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 			if(signatures.size())
 			{
 				signatures.front()->setLabel(label);
+				UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 				_dbDriver->asyncSave(signatures.front()); // move it again to trash
 				return true;
 			}
@@ -2818,6 +2820,8 @@ Transform Memory::computeScanMatchingTransform(
 	UASSERT(uContains(poses, newId) && uContains(_signatures, newId));
 	UASSERT(uContains(poses, oldId) && uContains(_signatures, oldId));
 
+	UDEBUG("Guess=%s", (poses.at(newId).inverse() * poses.at(oldId)).prettyPrint().c_str());
+
 	// make sure that all depth2D are loaded
 	std::list<Signature*> depthToLoad;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -2880,7 +2884,7 @@ Transform Memory::computeScanMatchingTransform(
 		int correspondences = 0;
 		bool hasConverged = false;
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
-		Transform icpT = util3d::icp2D(
+		Transform icpGlobal = util3d::icp2D(
 				newCloudVoxelized,
 				assembledOldClouds,
 			   _icp2MaxCorrespondenceDistance,
@@ -2888,22 +2892,26 @@ Transform Memory::computeScanMatchingTransform(
 			   hasConverged,
 			   *newCloudRegistered);
 
-		UDEBUG("icpT=%s", icpT.prettyPrint().c_str());
+		// in global Referential
+		UDEBUG("icpGlobal=%s", icpGlobal.prettyPrint().c_str());
 
 		//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
 		//pcl::io::savePCDFile("new.pcd", *newCloud, true);
 		//UWARN("local scan matching old.pcd, new.pcd saved!");
-		//if(!icpT.isNull())
+		//if(!icpGlobal.isNull())
 		//{
-		//	newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
-		//	pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
+		//	pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTmp = util3d::transformPointCloud(newCloud, icpGlobal);
+		//	pcl::io::savePCDFile("newFinal.pcd", *newCloudTmp, true);
 		//	UWARN("local scan matching newFinal.pcd saved!");
 		//}
 
-		if(!icpT.isNull() && hasConverged)
+		if(!icpGlobal.isNull() && hasConverged)
 		{
+			Transform icpLocal = poses.at(newId).inverse()*icpGlobal*poses.at(newId);
+			UDEBUG("icpLocal=%s", icpLocal.prettyPrint().c_str());
+
 			float ix,iy,iz, iroll,ipitch,iyaw;
-			icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
+			icpLocal.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
 			if((_icpMaxTranslation>0.0f &&
 				(fabs(ix) > _icpMaxTranslation ||
 				 fabs(iy) > _icpMaxTranslation ||
@@ -2921,7 +2929,7 @@ Transform Memory::computeScanMatchingTransform(
 			{
 				if(_icp2VoxelSize <= _laserScanVoxelSize)
 				{
-					newCloud = util3d::transformPointCloud(newCloud, icpT);
+					newCloud = util3d::transformPointCloud(newCloud, icpGlobal);
 				}
 				else
 				{
@@ -2967,7 +2975,7 @@ Transform Memory::computeScanMatchingTransform(
 
 				if(correspondencesRatio >= _icp2CorrespondenceRatio)
 				{
-					transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
+					transform = poses.at(newId).inverse()*icpGlobal.inverse() * poses.at(oldId);
 				}
 				else
 				{
@@ -3019,7 +3027,7 @@ bool Memory::addLink(const Link & link)
 
 		UDEBUG("Add link between %d and %d", toS->id(), fromS->id());
 
-		toS->addLink(Link(link.to(), link.from(), link.type(), link.transform().inverse(), link.infMatrix()));
+		toS->addLink(link.inverse());
 		fromS->addLink(link);
 
 		if(_incrementalMemory)

corelib/src/Rtabmap.cpp

@@ -664,7 +664,6 @@ int Rtabmap::triggerNewMap()
 		_optimizedPoses.clear();
 		_constraints.clear();
 		_lastLocalizationNodeId = 0;
-		_distanceTravelled = 0.0f;
 
 		//Verify if there are nodes that were merged through graph reduction
 		if(reducedIds.size() && _path.size())
@@ -1099,7 +1098,8 @@ bool Rtabmap::process(
 		// Update Poses and Constraints
 		_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
 		_lastLocalizationPose = newPose; // used in localization mode only (path planning)
-		if(signature->getLinks().size() == 1)
+		if(signature->getLinks().size() == 1 &&
+		   signature->getLinks().begin()->second.type() == Link::kNeighbor)
 		{
 			// link should be old to new
 			UASSERT_MSG(signature->id() > signature->getLinks().begin()->second.to(),

corelib/src/RtabmapThread.cpp

@@ -154,7 +154,6 @@ void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
 void RtabmapThread::mainLoopKill()
 {
 	this->clearBufferedData();
-
 	// this will post the newData semaphore
 	_dataAdded.release();
 }
@@ -178,6 +177,7 @@ void RtabmapThread::mainLoop()
 
 	int id = 0;
 	cv::Mat userData;
+	UTimer timer;
 	switch(state)
 	{
 	case kStateDetecting:
@@ -266,11 +266,16 @@ void RtabmapThread::mainLoop()
 		{
 			UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
 		}
+		timer.start();
 		if(id > 0 && !_rtabmap->computePath(id, true))
 		{
 			UERROR("Failed to compute a path to goal %d.", id);
 		}
-		this->post(new RtabmapGlobalPathEvent(id, parameters.at("label"), _rtabmap->getPath()));
+		this->post(new RtabmapGlobalPathEvent(
+				id,
+				parameters.at("label"),
+				_rtabmap->getPath(),
+				timer.elapsed()));
 		break;
 	case kStateCancellingGoal:
 		_rtabmap->clearPath(0);

corelib/src/SensorData.cpp

@@ -445,7 +445,11 @@ void SensorData::setUserData(const cv::Mat & userData)
 {
 	if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
 	{
-		UWARN("Writing new user data over existing user data. This may result in data loss.");
+		UWARN("Writing new user data (%d bytes) over existing user "
+			  "data (%d bytes, %d compressed). This may result in data loss.",
+			  int(userData.total()*userData.elemSize()),
+			  int(_userDataRaw.total()*_userDataRaw.elemSize()),
+			  _userDataCompressed.cols);
 	}
 	_userDataRaw = cv::Mat();
 	_userDataCompressed = cv::Mat();