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();

guilib/include/rtabmap/gui/StatsToolBox.h

@@ -48,7 +48,8 @@ class RTABMAPGUI_EXP StatItem : public QWidget
 public:
 	StatItem(const QString & name, const std::vector<float> & x, const std::vector<float> & y, const QString & unit = QString(), const QMenu * menu = 0, QGridLayout * grid = 0, QWidget * parent = 0);
 	virtual ~StatItem();
-	void setValue(float x, float y);
+	void addValue(float y);
+	void addValue(float x, float y);
 	void setValues(const std::vector<float> & x, const std::vector<float> & y);
 	QString value() const;
 
@@ -56,7 +57,8 @@ public slots:
 	void updateMenu(const QMenu * menu);
 
 signals:
-	void valueChanged(float, float);
+	void valueAdded(float);
+	void valueAdded(float, float);
 	void valuesChanged(const std::vector<float> &, const std::vector<float> &);
 	void plotRequested(const StatItem *, const QString &);
 
@@ -91,6 +93,7 @@ public:
 	void closeFigures();
 
 public slots:
+	void updateStat(const QString & statFullName, float y);
 	void updateStat(const QString & statFullName, float x, float y);
 	void updateStat(const QString & statFullName, const std::vector<float> & x, const std::vector<float> & y);
 

guilib/src/DatabaseViewer.cpp

@@ -1092,14 +1092,25 @@ void DatabaseViewer::updateIds()
 		bool linksInserted = false;
 		for(std::map<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
 		{
+			std::map<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
 			if(	jter->second.isValid() && // null transform means a rehearsed location
 				ids.find(jter->second.from()) != ids.end() &&
 				ids.find(jter->second.to()) != ids.end() &&
 				graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
 				graph::findLink(links, jter->second.from(), jter->second.to(), false) != links.end() &&
-				graph::findLink(links, jter->second.to(), jter->second.from(), false) != links.end())
+				invertedLinkIter != links.end())
 			{
-				links_.insert(std::make_pair(ids_[i], jter->second));
+				// check if user_data is set in opposite direction
+				if(jter->second.userDataCompressed().cols == 0 &&
+				   invertedLinkIter->second.userDataCompressed().cols != 0)
+				{
+					links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
+				}
+				else
+				{
+					links_.insert(std::make_pair(ids_[i], jter->second));
+				}
+
 				linksInserted = true;
 			}
 		}
@@ -2611,7 +2622,8 @@ void DatabaseViewer::updateConstraintView(
 
 				ui_->constraintsViewer->removeCloud("scan2");
 				ui_->constraintsViewer->removeGraph("scan2graph");
-				if(link.type() == Link::kLocalSpaceClosure && !link.userDataCompressed().empty())
+				if(link.type() == Link::kLocalSpaceClosure &&
+				   !link.userDataCompressed().empty())
 				{
 					std::vector<int> ids;
 					cv::Mat userData = link.uncompressUserDataConst();

guilib/src/MainWindow.cpp

@@ -470,6 +470,12 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 			_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), 0, (*iter).second);
 		}
 	}
+	// Specific MainWindow
+	_ui->statsToolBox->updateStat("Planning/From/", 0.0f);
+	_ui->statsToolBox->updateStat("Planning/Time/ms", 0.0f);
+	_ui->statsToolBox->updateStat("Planning/Goal/", 0.0f);
+	_ui->statsToolBox->updateStat("Planning/Poses/", 0.0f);
+	_ui->statsToolBox->updateStat("Planning/Length/m", 0.0f);
 	this->loadFigures();
 	connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
 
@@ -1302,22 +1308,26 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			}
 		}
 
-		// update posterior on the graph view
-		if(_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible() && stat.posterior().size())
+		if( _ui->graphicsView_graphView->isVisible())
 		{
-			_ui->graphicsView_graphView->updatePosterior(stat.posterior());
-		}
-		// update local path on the graph view
-		_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
-		if(stat.localPath().size() == 0)
-		{
-			// clear the global path if set (goal reached)
-			_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
-		}
-		// update current goal id
-		if(stat.currentGoalId() > 0)
-		{
-			_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
+			// update posterior on the graph view
+			if(_preferencesDialog->isPosteriorGraphView() &&
+			   stat.posterior().size())
+			{
+				_ui->graphicsView_graphView->updatePosterior(stat.posterior());
+			}
+			// update local path on the graph view
+			_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
+			if(stat.localPath().size() == 0)
+			{
+				// clear the global path if set (goal reached)
+				_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
+			}
+			// update current goal id
+			if(stat.currentGoalId() > 0)
+			{
+				_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
+			}
 		}
 
 		UDEBUG("");
@@ -2193,6 +2203,12 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
 		_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
 	}
 
+	_ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0));
+	_ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0));
+	_ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()));
+	_ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()));
+	_ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())));
+
 	if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
 	{
 		// use MessageBox

guilib/src/StatsToolBox.cpp

@@ -71,10 +71,16 @@ StatItem::~StatItem()
 
 }
 
-void StatItem::setValue(float x, float y)
+void StatItem::addValue(float y)
 {
 	_value->setText(QString::number(y, 'g', 3));
-	emit valueChanged(x,y);
+	emit valueAdded(y);
+}
+
+void StatItem::addValue(float x, float y)
+{
+	_value->setText(QString::number(y, 'g', 3));
+	emit valueAdded(x,y);
 }
 
 void StatItem::setValues(const std::vector<float> & x, const std::vector<float> & y)
@@ -184,6 +190,13 @@ void StatsToolBox::closeFigures()
 	}
 }
 
+void StatsToolBox::updateStat(const QString & statFullName, float y)
+{
+	std::vector<float> vx,vy(1);
+	vy[0] = y;
+	updateStat(statFullName, vx, vy);
+}
+
 void StatsToolBox::updateStat(const QString & statFullName, float x, float y)
 {
 	std::vector<float> vx(1),vy(1);
@@ -203,7 +216,11 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<fl
 	{
 		if(y.size() == 1 && x.size() == 1)
 		{
-			item->setValue(x[0], y[0]);
+			item->addValue(x[0], y[0]);
+		}
+		else if(y.size() == 1 && x.size() == 0)
+		{
+			item->addValue(y[0]);
 		}
 		else
 		{
@@ -303,7 +320,8 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 		{
 			UPlotCurve * curve = new UPlotCurve(stat->objectName(), plot);
 			curve->setPen(plot->getRandomPenColored());
-			connect(stat, SIGNAL(valueChanged(float, float)), curve, SLOT(addValue(float, float)));
+			connect(stat, SIGNAL(valueAdded(float)), curve, SLOT(addValue(float)));
+			connect(stat, SIGNAL(valueAdded(float, float)), curve, SLOT(addValue(float, float)));
 			connect(stat, SIGNAL(valuesChanged(const std::vector<float> &, const std::vector<float> &)), curve, SLOT(setData(const std::vector<float> &, const std::vector<float> &)));
 			if(stat->value().compare("*") == 0)
 			{
@@ -351,7 +369,8 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 		//Add a new curve linked to the statBox
 		UPlotCurve * curve = new UPlotCurve(stat->objectName(), newPlot);
 		curve->setPen(newPlot->getRandomPenColored());
-		connect(stat, SIGNAL(valueChanged(float, float)), curve, SLOT(addValue(float, float)));
+		connect(stat, SIGNAL(valueAdded(float)), curve, SLOT(addValue(float)));
+		connect(stat, SIGNAL(valueAdded(float, float)), curve, SLOT(addValue(float, float)));
 		connect(stat, SIGNAL(valuesChanged(const std::vector<float> &, const std::vector<float> &)), curve, SLOT(setData(const std::vector<float> &, const std::vector<float> &)));
 		if(stat->value().compare("*") == 0)
 		{