Updated local loop closure detection in space and path planning for Localization mode.

corelib/include/rtabmap/core/Rtabmap.h

@@ -155,7 +155,7 @@ private:
 			bool lookInDatabase,
 			std::multimap<int, Link> * constraints = 0) const;
 	void updateGoalIndex();
-	bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
+	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -226,6 +226,7 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _mapTransform; // for localization mode
+	Transform _lastLocalizationPose; // for localization mode
 
 	// Planning stuff
 	std::vector<std::pair<int,Transform> > _path;

corelib/src/Memory.cpp

@@ -330,6 +330,8 @@ Memory::~Memory()
 	UDEBUG("");
 	if(!_memoryChanged && !_linksChanged)
 	{
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
+
 		UDEBUG("");
 		if(_dbDriver)
 		{

corelib/src/Rtabmap.cpp

@@ -316,6 +316,7 @@ void Rtabmap::close()
 	_constraints.clear();
 	_mapCorrection.setIdentity();
 	_mapTransform.setIdentity();
+	_lastLocalizationPose.setNull();
 	this->clearPath();
 
 	flushStatisticLogs();
@@ -739,6 +740,7 @@ void Rtabmap::resetMemory()
 	_constraints.clear();
 	_mapCorrection.setIdentity();
 	_mapTransform.setIdentity();
+	_lastLocalizationPose.setNull();
 	this->clearPath();
 
 	if(_memory)
@@ -938,6 +940,7 @@ bool Rtabmap::process(const SensorData & data)
 
 		Transform newPose = _mapCorrection * signature->getPose();
 		_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
+		_lastLocalizationPose = newPose; // used in localization mode only (path planning)
 
 		//============================================================
 		// Scan matching
@@ -1361,7 +1364,8 @@ bool Rtabmap::process(const SensorData & data)
 		}
 
 		// immunize the path from the nearest local location to the current location
-		if(immunizedLocally < maxLocalLocationsImmunized)
+		if(immunizedLocally < maxLocalLocationsImmunized &&
+			_memory->isIncremental()) // Can only work in mapping mode
 		{
 			std::map<int ,Transform> poses;
 			// remove poses from STM
@@ -1644,8 +1648,11 @@ bool Rtabmap::process(const SensorData & data)
 		{
 			UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
 		}
-		else
+		else if(_memory->isIncremental() || _loopClosureHypothesis.first == 0)
 		{
+			// In localization mode, no need to check local loop
+			// closures if we are already localized by a global closure.
+
 			//============================================================
 			// LOCAL LOOP CLOSURE SPACE
 			//============================================================
@@ -1658,7 +1665,16 @@ bool Rtabmap::process(const SensorData & data)
 			{
 				r = _localPathFilteringRadius;
 			}
-			std::map<int, float> nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth);
+
+			std::map<int, float> nearestIds;
+			if(_memory->isIncremental())
+			{
+				nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth);
+			}
+			else
+			{
+				nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, r);
+			}
 			std::map<int, Transform> nearestPoses;
 			for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
 			{
@@ -1666,7 +1682,9 @@ bool Rtabmap::process(const SensorData & data)
 			}
 			// segment poses by paths, only one detection per path
 			std::list<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
-			for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin(); iter!=nearestPaths.end(); ++iter)
+			for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
+				iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
+				++iter)
 			{
 				std::map<int, Transform> & path = *iter;
 				UASSERT(path.size());
@@ -1753,12 +1771,15 @@ bool Rtabmap::process(const SensorData & data)
 								transform.prettyPrint().c_str());
 						_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance);
 
-						// Old map -> new map, used for localization correction on loop closure
-						const Signature * oldS = _memory->getSignature(nearestId);
-						UASSERT(oldS != 0);
-						_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
-						++localSpaceClosuresAddedVisually;
-						lastLocalSpaceClosureId = nearestId;
+						if(_loopClosureHypothesis.first == 0)
+						{
+							// Old map -> new map, used for localization correction on loop closure
+							const Signature * oldS = _memory->getSignature(nearestId);
+							UASSERT(oldS != 0);
+							_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
+							++localSpaceClosuresAddedVisually;
+							lastLocalSpaceClosureId = nearestId;
+						}
 					}
 				}
 			}
@@ -1766,8 +1787,13 @@ bool Rtabmap::process(const SensorData & data)
 			//
 			// 2) compare locally with nearest locations by scan matching
 			//
-			if( !signature->getLaserScanCompressed().empty())
+			if( !signature->getLaserScanCompressed().empty() &&
+				(_memory->isIncremental() || lastLocalSpaceClosureId == 0))
 			{
+				// In localization mode, no need to check local loop
+				// closures if we are already localized by at least one
+				// local visual closure above.
+
 				std::map<int, Transform> forwardPoses;
 				forwardPoses = this->getForwardWMPoses(
 						signature->id(),
@@ -1778,7 +1804,9 @@ bool Rtabmap::process(const SensorData & data)
 				std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
 				localSpacePaths = (int)forwardPaths.size();
 
-				for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin(); iter!=forwardPaths.end(); ++iter)
+				for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin();
+						iter!=forwardPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
+						++iter)
 				{
 					std::map<int, Transform> & path = *iter;
 					UASSERT(path.size());
@@ -1835,7 +1863,7 @@ bool Rtabmap::process(const SensorData & data)
 									++localSpaceClosuresAddedByICPOnly;
 
 									// no local loop closure added visually
-									if(localSpaceClosuresAddedVisually == 0)
+									if(localSpaceClosuresAddedVisually == 0 && _loopClosureHypothesis.first == 0)
 									{
 										// Old map -> new map, used for localization correction on loop closure
 										const Signature * oldS = _memory->getSignature(nearestId);
@@ -1873,6 +1901,7 @@ bool Rtabmap::process(const SensorData & data)
 			// Update map correction, it should be identify when optimizing from the last node
 			_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
 			_mapTransform.setIdentity(); // reset mapTransform (used for localization only)
+			_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update in case we switch to localization mode
 			if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
 			{
 				UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
@@ -1895,13 +1924,13 @@ bool Rtabmap::process(const SensorData & data)
 			UASSERT(oldS != 0);
 			Transform correction = _optimizedPoses.at(oldId) * oldS->getPose().inverse();
 			_mapCorrection = correction * _mapTransform;
+			_lastLocalizationPose = _mapCorrection * signature->getPose();
 		}
 		else
 		{
 			UERROR("Not supposed to be here!");
 		}
 	}
-	Transform currentRawOdomPose = signature->getPose();
 
 	timeMapOptimization = timer.ticks();
 	ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization);
@@ -2374,7 +2403,7 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		int fromId,
 		int maxNearestNeighbors,
 		float radius,
-		int maxDiffID // 0 means ignore
+		int maxGraphDepth // 0 means ignore
 		) const
 {
 	std::map<int, Transform> poses;
@@ -2392,10 +2421,15 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		const std::set<int> & stm = _memory->getStMem();
 		//get distances
 		std::map<int, float> foundIds;
-		if(maxDiffID > 0)
+		if(_memory->isIncremental())
 		{
-			foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxDiffID);
+			foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxGraphDepth);
 		}
+		else
+		{
+			foundIds = graph::getNodesInRadius(fromId, _optimizedPoses, radius);
+		}
+
 		float radiusSqrd = radius * radius;
 		for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 		{
@@ -2828,17 +2862,30 @@ void Rtabmap::clearPath()
 
 bool Rtabmap::computePath(
 		int targetNode,
-		const std::map<int, Transform> & nodes,
+		std::map<int, Transform> nodes,
 		const std::multimap<int, rtabmap::Link> & constraints)
 {
 	if(_memory)
 	{
-		if(!_memory->getLastWorkingSignature())
+		int currentNode;
+		if(_memory->isIncremental())
 		{
-			UWARN("Working memory is empty... cannot compute a path");
-			return false;
+			if(!_memory->getLastWorkingSignature())
+			{
+				UWARN("Working memory is empty... cannot compute a path");
+				return false;
+			}
+			currentNode = _memory->getLastWorkingSignature()->id();
+		}
+		else
+		{
+			if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0)
+			{
+				UWARN("Last localization pose is null... cannot compute a path");
+				return false;
+			}
+			currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
 		}
-		int currentNode = _memory->getLastWorkingSignature()->id();
 
 		if(!uContains(nodes, currentNode))
 		{
@@ -2852,6 +2899,19 @@ bool Rtabmap::computePath(
 			return false;
 		}
 
+		// transform nodes into current referential
+		if(_optimizedPoses.size())
+		{
+			if(uContains(nodes, currentNode) && uContains(_optimizedPoses, currentNode))
+			{
+				Transform t = _optimizedPoses.at(currentNode) * nodes.at(currentNode).inverse();
+				for(std::map<int, Transform>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+				{
+					iter->second = t * iter->second;
+				}
+			}
+		}
+
 		std::multimap<int, int> links;
 		for(std::multimap<int, rtabmap::Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 		{
@@ -3096,19 +3156,34 @@ void Rtabmap::updateGoalIndex()
 		}
 
 		UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
-		if(_memory->getLastWorkingSignature() == 0 ||
-		   !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
+		Transform currentPose;
+		if(_memory->isIncremental())
 		{
-			UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
-			this->clearPath();
-			return;
+			if(_memory->getLastWorkingSignature() == 0 ||
+			   !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
+			{
+				UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
+				this->clearPath();
+				return;
+			}
+			currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
+		}
+		else
+		{
+			if(_lastLocalizationPose.isNull())
+			{
+				UERROR("Last localization pose is null. Aborting the plan...");
+				this->clearPath();
+				return;
+			}
+			currentPose = _lastLocalizationPose;
 		}
 
 		int goalId = _path.back().first;
 		if(uContains(_optimizedPoses, goalId))
 		{
 			//use local position to know if the goal is reached
-			float d = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal);
+			float d = currentPose.getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal);
 			if(d < _goalReachedRadius)
 			{
 				UINFO("Goal %d reached!", goalId);
@@ -3127,7 +3202,7 @@ void Rtabmap::updateGoalIndex()
 				{
 					if(_localRadius > 0.0f)
 					{
-						distanceFromCurrentNode = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first));
+						distanceFromCurrentNode = currentPose.getDistance(_optimizedPoses.at(_path[i].first));
 					}
 
 					if(distanceFromCurrentNode <= _localRadius)
@@ -3155,7 +3230,6 @@ void Rtabmap::updateGoalIndex()
 			// update nearest pose in the path
 			unsigned int nearestNodeIndex = 0;
 			float distance = -1.0f;
-			const Transform & currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
 			UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0);
 			for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
 			{

corelib/src/VWDictionary.cpp

@@ -209,6 +209,10 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	{
 		_incrementalDictionary = false;
 	}
+	else if(_incrementalDictionary)
+	{
+		UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size());
+	}
 	_dictionaryPath = dictionaryPath;
 }
 

guilib/src/GraphViewer.cpp

@@ -75,6 +75,7 @@ public:
 		this->setBrush(b);
 	}
 
+	const Transform & pose() const {return _pose;}
 	void setPose(const Transform & pose) {this->setPos(-pose.y(),-pose.x()); _pose=pose;}
 
 protected:
@@ -497,27 +498,65 @@ void GraphViewer::setLocalRadius(float radius)
 
 void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 {
+	for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+
 	if(localPath.size() > 1)
 	{
 		for(unsigned int i=0; i<localPath.size()-1; ++i)
 		{
-			if(_linkItems.contains(localPath[i]))
+			int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
+			int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
+			if(_nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
 			{
-				int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
-				int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
-				QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(idFrom);
-				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
+				bool updated = false;
+				if(_localPathLinkItems.contains(idFrom))
 				{
-					if(itemIter.value()->to() == idTo)
+					QMultiMap<int, LinkItem*>::iterator itemIter = _localPathLinkItems.find(idFrom);
+					while(itemIter.key() == idFrom && itemIter != _localPathLinkItems.end())
 					{
-						itemIter.value()->setColor(_localPathColor);
-						break;
+						if(itemIter.value()->to() == idTo)
+						{
+							itemIter.value()->setPoses(_nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose());
+							itemIter.value()->show();
+							updated = true;
+							break;
+						}
+						++itemIter;
 					}
-					++itemIter;
+				}
+				if(!updated)
+				{
+					//create a link item
+					LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link::kUndef);
+					QPen p = item->pen();
+					p.setWidthF(_linkWidth);
+					item->setPen(p);
+					item->setColor(_localPathColor);
+					this->scene()->addItem(item);
+					item->setZValue(16); // just over the global path
+					item->setParentItem(_root);
+					_localPathLinkItems.insert(idFrom, item);
 				}
 			}
 		}
 	}
+
+	// remove not used links
+	for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _localPathLinkItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
 }
 
 void GraphViewer::clearGraph()
@@ -526,8 +565,11 @@ void GraphViewer::clearGraph()
 	_nodeItems.clear();
 	qDeleteAll(_linkItems);
 	_linkItems.clear();
+	qDeleteAll(_localPathLinkItems);
+	_localPathLinkItems.clear();
 	qDeleteAll(_globalPathLinkItems);
 	_globalPathLinkItems.clear();
+
 	_referential->resetTransform();
 	_localRadius->resetTransform();
 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents

guilib/src/GraphViewer.h

@@ -129,6 +129,7 @@ private:
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;
+	QMultiMap<int, LinkItem*> _localPathLinkItems;
 	QMultiMap<int, LinkItem*> _globalPathLinkItems;
 	float _nodeRadius;
 	float _linkWidth;