Merge branch 'master' of github.com:introlab/rtabmap into opencv5_support

app/android/jni/RTABMapApp.cpp

@@ -204,7 +204,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "0"));      // deactivate global retrieval
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0"));    // deactivate local retrieval
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false"));    // don't create map labels
-        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMemoryThr(), "2"));         // keep the WM empty
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMemoryThr(), "1"));         // keep the WM empty
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));               // STM=1 -->
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), "false"));
         uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), "0"));

corelib/include/rtabmap/core/Memory.h

@@ -153,6 +153,9 @@ public:
 
 	//getters
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
+	size_t getWorkingMemSize(bool ignoreIntermediateNodes = false) const;
+	int getWorkingMemIntermediateNodesCount() const {return _workingMemIntermediateNodesCount;}
+	int getStMemIntermediateNodesCount() const {return _stMemIntermediateNodesCount;}
 	const std::set<int> & getStMem() const {return _stMem;}
 	int getMaxStMemSize() const {return _maxStMemSize;}
 	std::multimap<int, Link> getNeighborLinks(int signatureId,
@@ -364,6 +367,8 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
+	int _workingMemIntermediateNodesCount; // number of nodes with weight==-1 currently in _workingMem
+	int _stMemIntermediateNodesCount; // number of nodes with weight==-1 currently in _stMem
 	bool _allNodesInWM;
 	bool _receivingOdometryFeatures;
 	GPS _gpsOrigin;

corelib/include/rtabmap/core/Statistics.h

@@ -141,7 +141,9 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(NeighborLinkRefining, Pts,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
+	RTABMAP_STATS(Memory, Working_memory_inter_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
+	RTABMAP_STATS(Memory, Short_time_memory_inter_size,);
 	RTABMAP_STATS(Memory, Database_memory_used, MB);
 	RTABMAP_STATS(Memory, Signatures_removed,);
 	RTABMAP_STATS(Memory, Immunized_globally,);

corelib/src/Memory.cpp

@@ -133,6 +133,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_memoryChanged(false),
 	_linksChanged(false),
 	_signaturesAdded(0),
+	_workingMemIntermediateNodesCount(0),
+	_stMemIntermediateNodesCount(0),
 	_allNodesInWM(true),
 	_receivingOdometryFeatures(false),
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
@@ -269,6 +271,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 				//       global loop closures.
 				_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
 				_workingMem.insert(std::make_pair((*iter)->id(), UTimer::now()));
+				if((*iter)->getWeight() == -1)
+				{
+					++_workingMemIntermediateNodesCount;
+				}
 				if(!(*iter)->getGroundTruthPose().isNull()) {
 					_groundTruths.insert(std::make_pair((*iter)->id(), (*iter)->getGroundTruthPose()));
 				}
@@ -1081,7 +1087,7 @@ bool Memory::update(
 	}
 	else
 	{
-		if(_workingMem.size() <= 1)
+		if(this->getWorkingMemSize(true) == 0)
 		{
 			UWARN("The working memory is empty and the memory is not "
 				  "incremental (Mem/IncrementalMemory=False), no loop closure "
@@ -1257,6 +1263,10 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 
 		_signatures.insert(_signatures.end(), std::pair<int, Signature *>(signature->id(), signature));
 		_stMem.insert(_stMem.end(), signature->id());
+		if(signature->getWeight() == -1)
+		{
+			++_stMemIntermediateNodesCount;
+		}
 		if(!signature->getGroundTruthPose().isNull()) {
 			_groundTruths.insert(std::make_pair(signature->id(), signature->getGroundTruthPose()));
 		}
@@ -1278,6 +1288,10 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
 	{
 		UDEBUG("Inserting node %d in WM...", signature->id());
 		_workingMem.insert(std::make_pair(signature->id(), UTimer::now()));
+		if(signature->getWeight() == -1)
+		{
+			++_workingMemIntermediateNodesCount;
+		}
 		_signatures.insert(std::pair<int, Signature*>(signature->id(), signature));
 		if(!signature->getGroundTruthPose().isNull()) {
 			_groundTruths.insert(std::make_pair(signature->id(), signature->getGroundTruthPose()));
@@ -1468,6 +1482,11 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
 	if(reducedId == 0)
 	{
 		_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
+		if(this->_getSignature(*_stMem.begin())->getWeight() == -1)
+		{
+			++_workingMemIntermediateNodesCount;
+			--_stMemIntermediateNodesCount;
+		}
 		_stMem.erase(*_stMem.begin());
 	}
 	// else already removed from STM/WM in reduceNode()
@@ -1488,6 +1507,19 @@ const VWDictionary * Memory::getVWDictionary() const
 	return _vwd;
 }
 
+size_t Memory::getWorkingMemSize(bool ignoreIntermediateNodes) const
+{
+	// -1 removes the virtual place
+	if(!ignoreIntermediateNodes)
+	{
+		return _workingMem.size() - 1;
+	}
+	else
+	{
+		return _workingMem.size() - 1 - _workingMemIntermediateNodesCount;
+	}
+}
+
 std::multimap<int, Link> Memory::getNeighborLinks(
 		int signatureId,
 		bool lookInDatabase) const
@@ -2024,6 +2056,7 @@ void Memory::clear()
 		ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
 	}
 	_stMem.clear();
+	_stMemIntermediateNodesCount = 0;
 
 	this->cleanUnusedWords();
 
@@ -2040,18 +2073,15 @@ void Memory::clear()
 	}
 
 	// Save some stats to the db, save only when the mem is not empty
-	if(_dbDriver && (_stMem.size() || _workingMem.size()))
+	size_t workingMemSize = this->getWorkingMemSize(false);
+	if(_dbDriver && (_stMem.size() || workingMemSize))
 	{
-		unsigned int memSize = (unsigned int)(_workingMem.size() + _stMem.size());
-		if(_workingMem.size() && _workingMem.begin()->first < 0)
-		{
-			--memSize;
-		}
+		unsigned int memSize = workingMemSize + _stMem.size();
 
 		// this is only a safe check...not supposed to occur.
 		UASSERT_MSG(memSize == _signatures.size(),
 				uFormat("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d",
-						_workingMem.size(), _stMem.size(), _signatures.size()).c_str());
+						workingMemSize, _stMem.size(), _signatures.size()).c_str());
 
 		UDEBUG("Adding statistics after run...");
 		if(_memoryChanged)
@@ -2098,6 +2128,7 @@ void Memory::clear()
 		ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
 	}
 	_workingMem.clear();
+	_workingMemIntermediateNodesCount = 0;
 	if(_signatures.size()!=0)
 	{
 		ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size());
@@ -2484,7 +2515,7 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 				  "poses to force re-update. If you want to use the "
 				  "saved optimized poses, set %s to true",
 				  (int)poses.size(),
-				  (int)_workingMem.size()-1, // less virtual place
+				  (int)this->getWorkingMemSize(false),
 				  Parameters::kMemInitWMWithAllNodes().c_str());
 			return std::map<int, Transform>();
 		}
@@ -2592,18 +2623,21 @@ public:
 	}
 	int weight, age, id;
 };
+
 std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
 {
 	//UDEBUG("");
 	std::list<Signature *> removableSignatures;
 	std::map<WeightAgeIdKey, Signature *> weightAgeIdMap;
 
-	// Find the last index to check...
-	UDEBUG("mem.size()=%d, ignoredIds.size()=%d", (int)_workingMem.size(), (int)ignoredIds.size());
+	size_t workingMemSize = this->getWorkingMemSize(true);
 
-	if(_workingMem.size())
+	// Find the last index to check...
+	UDEBUG("mem.size()=%d, ignoredIds.size()=%d", (int)workingMemSize, (int)ignoredIds.size());
+
+	if(workingMemSize > 0)
 	{
-		int recentWmMaxSize = _recentWmRatio * float(_workingMem.size());
+		int recentWmMaxSize = _recentWmRatio * float(workingMemSize);
 		bool recentWmImmunized = false;
 		// look for the position of the lastLoopClosureId in WM
 		int currentRecentWmSize = 0;
@@ -2620,7 +2654,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 			{
 				recentWmImmunized = true;
 			}
-			else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
+			else if(currentRecentWmSize == 0)
 			{
 				UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureId);
 			}
@@ -2729,6 +2763,20 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 	//UDEBUG("id=%d", s?s->id():0);
 	if(s)
 	{
+		// Keep the WM/STM intermediate-node counters in sync now, before the weight
+		// may be set to -9 below.
+		if(s->getWeight() == -1)
+		{
+			if(this->isInWM(s->id()))
+			{
+				--_workingMemIntermediateNodesCount;
+			}
+			else if(this->isInSTM(s->id()))
+			{
+				--_stMemIntermediateNodesCount;
+			}
+		}
+
 		// Cleanup landmark indexes
 		if(!s->getLandmarks().empty())
 		{
@@ -3056,6 +3104,19 @@ void Memory::convertToIntermediate(int locationId)
 	Signature * location = _getSignature(locationId);
 	if(location)
 	{
+		// Keep the WM/STM intermediate-node counters in sync if the node is
+		// converted while already resident in memory.
+		if(location->getWeight() >= 0)
+		{
+			if(this->isInWM(locationId))
+			{
+				++_workingMemIntermediateNodesCount;
+			}
+			else if(this->isInSTM(locationId))
+			{
+				++_stMemIntermediateNodesCount;
+			}
+		}
 		location->setWeight(-1);
 		location->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		this->disableWordsRef(locationId); // won't be used for loop closure detection anymore
@@ -5196,11 +5257,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("time rectification = %fs", t);
 	}
 
-	int treeSize= int(_workingMem.size() + _stMem.size());
-	int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
-	if(meanWordsPerLocation==0 && treeSize > 1)
+	int notIntermediateNodesCount = 0;
+	for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
 	{
-		meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
+		const Signature * s = this->getSignature(*iter);
+		UASSERT(s != 0);
+		if(s->getWeight() >= 0)
+		{
+			++notIntermediateNodesCount;
+		}
+	}
+	int treeSize= int(this->getWorkingMemSize(true) + notIntermediateNodesCount);
+	int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
+	if(meanWordsPerLocation==0 && treeSize > 0)
+	{
+		meanWordsPerLocation = _vwd->getTotalActiveReferences() / treeSize;
 	}
 	else if(_useOdometryFeatures) {
 		// To not detect first image as bad signature if odometry 

corelib/src/Rtabmap.cpp

@@ -378,9 +378,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
 	if(!_memory->isIncremental())
 	{
-		if(_optimizedPoses.empty() &&
-			_memory->getWorkingMem().size()>1 &&
-			_memory->getWorkingMem().lower_bound(1)!=_memory->getWorkingMem().end())
+		if(_optimizedPoses.empty() && _memory->getWorkingMemSize(true) > 0)
 		{
 			cv::Mat cov;
 			this->optimizeCurrentMap(
@@ -810,7 +808,7 @@ int Rtabmap::getWMSize() const
 {
 	if(_memory)
 	{
-		return (int)_memory->getWorkingMem().size()-1; // remove virtual place
+		return (int)_memory->getWorkingMemSize(false);
 	}
 	return 0;
 }
@@ -1998,7 +1996,7 @@ bool Rtabmap::process(
 		// If the working memory is empty, don't do the detection. It happens when it
 		// is the first time the detector is started (there needs some images to
 		// fill the short-time memory before a signature is added to the working memory).
-		if(_memory->getWorkingMem().size())
+		if(_memory->getWorkingMemSize(true))
 		{
 			//============================================================
 			// Likelihood computation
@@ -2174,7 +2172,7 @@ bool Rtabmap::process(
 				}
 				if(	(( _memory->isIncremental() && !uContains(_optimizedPoses, _highestHypothesis.first)) || // not linked to previous map of that hypothesis
 					 (!_memory->isIncremental() && !hasLoopClosureConstraints)) && // not yet localized to any previous sessions
-					_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
+					_memory->getWorkingMemSize(true)>0 && // should have an old map
 					_rgbdSlamMode &&
 					loopThr > _aggressiveLoopThr)
 				{
@@ -2227,7 +2225,7 @@ bool Rtabmap::process(
 				hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
 			}
 		} // if(_memory->getWorkingMemSize())
-	}// !isBadSignature
+	} // !isBadSignature
 	else if(!signature->isBadSignature() && signature->getWeight()>=0 && (smallDisplacement || tooFastMovement))
 	{
 		_highestHypothesis = lastHighestHypothesis;
@@ -2259,12 +2257,12 @@ bool Rtabmap::process(
 	if(_maxTimeAllowed != 0 || _maxMemoryAllowed != 0)
 	{
 		// with memory management, we have to immunize some nodes
-		maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size());
+		maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMemSize(true));
 	}
 	// no need to do retrieval or immunization of locations if memory management
 	// is disabled and all nodes are in WM.
 	// Also skip memory mangement on intermediate nodes
-	if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0) && signature->getWeight()>=0)
+	if(!((_memory->allNodesInWM() || _maxRetrieved==0) && maxLocalLocationsImmunized==0) && signature->getWeight()>=0)
 	{
 		if(retrievalId > 0)
 		{
@@ -2320,7 +2318,13 @@ bool Rtabmap::process(
 							//immunized locations in the neighborhood from being transferred
 							if(immunizedLocations.insert(iter->first).second)
 							{
-								++immunizedGlobally;
+								// Count only non-intermediate nodes (intermediate nodes are still
+								// immunized but don't consume the immunization budget/statistic).
+								const Signature * sImmunized = _memory->getSignature(iter->first);
+								if(sImmunized == 0 || sImmunized->getWeight() >= 0)
+								{
+									++immunizedGlobally;
+								}
 							}
 
 							//UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
@@ -2423,9 +2427,12 @@ bool Rtabmap::process(
 					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
 				}
 
-				if(_memory->getSignature(_path[i].first) != 0)
+				const Signature * sPath = _memory->getSignature(_path[i].first);
+				if(sPath != 0)
 				{
-					if(immunizedLocations.insert(_path[i].first).second)
+					// Count only non-intermediate nodes (intermediate nodes are still
+					// immunized but don't consume the immunization budget/statistic).
+					if(immunizedLocations.insert(_path[i].first).second && sPath->getWeight() >= 0)
 					{
 						++immunizedLocally;
 					}
@@ -2447,7 +2454,7 @@ bool Rtabmap::process(
 			}
 		}
 
-		if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
+		if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized==0))
 		{
 			// immunize the path from the nearest local location to the current location
 			if(immunizedLocally < maxLocalLocationsImmunized &&
@@ -2498,15 +2505,16 @@ bool Rtabmap::process(
 									{
 										UWARN("Could not immunize the whole local path (%d) between "
 											  "%d and %d (max location immunized=%d). You may want "
-											  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
+											  "to increase %s (current=%f (%d of WM=%d)) "
 											  "to be able to immunize longer paths.",
 												(int)path.size(),
 												nearestId,
 												signature->id(),
 												maxLocalLocationsImmunized,
+												Parameters::kRGBDLocalImmunizationRatio().c_str(),
 												_localImmunizationRatio,
 												maxLocalLocationsImmunized,
-												(int)_memory->getWorkingMem().size());
+												(int)_memory->getWorkingMemSize(true));
 									}
 									break;
 								}
@@ -2514,7 +2522,13 @@ bool Rtabmap::process(
 								{
 									if(immunizedLocations.insert(iter->first).second)
 									{
-										++immunizedLocally;
+										// Count only non-intermediate nodes (intermediate nodes are still
+										// immunized but don't consume the immunization budget/statistic).
+										const Signature * sLocal = _memory->getSignature(iter->first);
+										if(sLocal == 0 || sLocal->getWeight() >= 0)
+										{
+											++immunizedLocally;
+										}
 									}
 									//UDEBUG("local node %d on path immunized=1", iter->first);
 								}
@@ -2538,7 +2552,7 @@ bool Rtabmap::process(
 					maxLocalLocationsImmunized,
 					immunizedLocally,
 					_localImmunizationRatio,
-					(int)_memory->getWorkingMem().size());
+					(int)_memory->getWorkingMemSize(true));
 			std::list<int> retrievalLocalIdsIntermediate;
 			for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
 				iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
@@ -2574,7 +2588,9 @@ bool Rtabmap::process(
 					}
 					if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
 					{
-						if(immunizedLocations.insert(s->id()).second)
+						// Count only non-intermediate nodes (intermediate nodes are still
+						// immunized but don't consume the immunization budget/statistic).
+						if(immunizedLocations.insert(s->id()).second && s->getWeight() >= 0)
 						{
 							++immunizedLocally;
 						}
@@ -2685,7 +2701,7 @@ bool Rtabmap::process(
 	   signature->getWeight() >= 0) // not an intermediate node
 	{
 		if(_startNewMapOnLoopClosure &&
-			_memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place)
+			_memory->getWorkingMemSize(true)>0 && // must have an old map
 			_localizationCovariance.empty() && // if we didn't localize yet
 			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session
 		{
@@ -4451,7 +4467,7 @@ bool Rtabmap::process(
 			_memory->isIncremental() &&              // only in mapping mode
 			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 &&      // alone in the current map
 			(landmarksDetected.empty() || rejectedLoopClosure) &&      // if we re not seeing a landmark from a previous map
-			_memory->getWorkingMem().size()>=2)       // The working memory should not be empty (beside virtual signature)
+			_memory->getWorkingMemSize(true)>0)       // The working memory should not be empty
 		{
 			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
 					signature->id());
@@ -4538,26 +4554,29 @@ bool Rtabmap::process(
 	//============================================================
 	double totalTime = timerTotal.ticks();
 	ULOGGER_INFO("Total time processing = %fs...", totalTime);
-	if(!lastSignatureWasIntermediateNode && // skip memory management on intermediate nodes
-		((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
-		 (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed)))
+	if(!lastSignatureWasIntermediateNode) // skip memory management on intermediate nodes
 	{
-		if(_maxTimeAllowed!=0 && totalTime*1000>_maxTimeAllowed)
+		size_t workingMemSize = _memory->getWorkingMemSize(true);
+		if((_maxTimeAllowed != 0 && totalTime*1000 > _maxTimeAllowed) ||
+			(_maxMemoryAllowed != 0 && workingMemSize > _maxMemoryAllowed))
 		{
-			ULOGGER_INFO("Removing old signatures because time limit is reached %f ms > %f ms...",
-				totalTime*1000, _maxTimeAllowed);
-		}
-		if(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed)
-		{
-			ULOGGER_INFO("Removing old signatures because memory limit is reached %d > %d...",
-				_memory->getWorkingMem().size(), _maxMemoryAllowed);
-		}
-		immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory
-		std::list<int> transferred = _memory->forget(immunizedLocations);
-		signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
-		if(!_someNodesHaveBeenTransferred && transferred.size())
-		{
-			_someNodesHaveBeenTransferred = true; // only used to hide a warning on close nodes immunization
+			if(_maxTimeAllowed!=0 && totalTime*1000 > _maxTimeAllowed)
+			{
+				ULOGGER_INFO("Removing old signatures because time limit is reached %f ms > %f ms...",
+					totalTime*1000, _maxTimeAllowed);
+			}
+			if(_maxMemoryAllowed != 0 && workingMemSize > _maxMemoryAllowed)
+			{
+				ULOGGER_INFO("Removing old signatures because memory limit is reached %d > %d...",
+					workingMemSize, _maxMemoryAllowed);
+			}
+			immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory
+			std::list<int> transferred = _memory->forget(immunizedLocations);
+			signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
+			if(!_someNodesHaveBeenTransferred && transferred.size())
+			{
+				_someNodesHaveBeenTransferred = true; // only used to hide a warning on close nodes immunization
+			}
 		}
 	}
 	_lastProcessTime = totalTime;
@@ -4709,8 +4728,10 @@ bool Rtabmap::process(
 		statistics_.addStatistic(Statistics::kMemoryImmunized_locally_max(), maxLocalLocationsImmunized);
 
 		// place after transfer because the memory/local graph may have changed
-		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
-		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
+		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMemSize(true));
+		statistics_.addStatistic(Statistics::kMemoryWorking_memory_inter_size(), _memory->getWorkingMemIntermediateNodesCount());
+		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size()-_memory->getStMemIntermediateNodesCount());
+		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_inter_size(), _memory->getStMemIntermediateNodesCount());
 		statistics_.addStatistic(Statistics::kMemoryDatabase_memory_used(), _memory->getDatabaseMemoryUsed());
 
 		// Set local graph
@@ -4859,7 +4880,7 @@ bool Rtabmap::process(
 		}
 
 		std::vector<int> ids;
-		ids.reserve(_memory->getWorkingMem().size() + _memory->getStMem().size());
+		ids.reserve(_memory->getWorkingMemSize(false) + _memory->getStMem().size());
 		for(std::set<int>::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter)
 		{
 			ids.push_back(*iter);
@@ -4921,7 +4942,7 @@ bool Rtabmap::process(
 									0,
 									refWordsCount,
 									dictionarySize,
-									int(_memory->getWorkingMem().size()),
+									int(_memory->getWorkingMemSize(false)),
 									rejectedLoopClosure?1:0,
 									0,
 									0,
@@ -5948,7 +5969,7 @@ void Rtabmap::getGraph(
 			}
 		}
 	}
-	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
+	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMemSize(!global) > 0))
 	{
 		UERROR("Last working signature is null!?");
 	}

tools/Reprocess/main.cpp

@@ -98,6 +98,7 @@ void showUsage()
 			"     -noimu      Don't republish IMU contained in input database.\n"
 			"     -pub_loops  Republish loop closures contained in input database.\n"
 			"     -pub_inter_as_normal Republish intermediate nodes as normal nodes.\n"
+			"     -abort_disconnected_sessions Return error if not all sessions are connected together.\n"
 			"     -loc_null   On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
 			"     -gt         When reprocessing a single database, load its original optimized graph, then \n"
 			"                 set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n"
@@ -231,6 +232,29 @@ void showLocalizationStats(const std::string & outputDatabasePath)
 	++sessionCount;
 }
 
+std::set<int> getMapIds(const std::set<int> & ids, const rtabmap::DBDriver & driver)
+{
+	std::set<int> mapIds;
+	for(auto id: ids)
+	{
+		Transform p;
+		int mapId;
+		int w;
+		std::string l;
+		double s;
+		Transform gt;
+		std::vector<float> v;
+		GPS g;
+		EnvSensors ss;
+
+		if(driver.getNodeInfo(id, p, mapId, w, l, s, gt, v, g, ss) && mapId>=0)
+		{
+			mapIds.insert(mapId);
+		}
+	}
+	return mapIds;
+}
+
 int main(int argc, char * argv[])
 {
 	signal(SIGABRT, &sighandler);
@@ -283,6 +307,7 @@ int main(int argc, char * argv[])
 	bool ignoreImu = false;
 	bool republishLoopClosures = false;
 	bool pubInterNodesAsNormalNodes = false;
+	bool abortDisconnectedSessions = false;
 	bool locNull = false;
 	bool originalGraphAsGT = false;
 	bool scanFromDepth = false;
@@ -523,6 +548,11 @@ int main(int argc, char * argv[])
 			pubInterNodesAsNormalNodes = true;
 			printf("Republish intermdiate nodes as normal nodes (-pub_inter_as_normal option).\n");
 		}
+		else if(strcmp(argv[i], "-abort_disconnected_sessions") == 0 || strcmp(argv[i], "--abort_disconnected_sessions") == 0)
+		{
+			abortDisconnectedSessions = true;
+			printf("Will abort if not all sessions are connected at the end (-abort_disconnected_sessions option).\n");
+		}
 		else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
 		{
 			locNull = true;
@@ -668,14 +698,14 @@ int main(int argc, char * argv[])
 	std::list<std::string> databases = uSplit(inputDatabasePath, ';');
 	if (databases.empty())
 	{
-		printf("No input database \"%s\" detected!\n", inputDatabasePath.c_str());
+		printf("[Error] No input database \"%s\" detected!\n", inputDatabasePath.c_str());
 		return 1;
 	}
 	for (std::list<std::string>::iterator iter = databases.begin(); iter != databases.end(); ++iter)
 	{
 		if (!UFile::exists(*iter))
 		{
-			printf("Input database \"%s\" doesn't exist!\n", iter->c_str());
+			printf("[Error] Input database \"%s\" doesn't exist!\n", iter->c_str());
 			if(uStrContains(inputDatabasePath,":"))
 			{
 				printf("Did you mean \"%s\"?\n", uReplaceChar(inputDatabasePath, ':', ";").c_str());
@@ -685,14 +715,14 @@ int main(int argc, char * argv[])
 
 		if (UFile::getExtension(*iter).compare("db") != 0)
 		{
-			printf("File \"%s\" is not a database format (*.db)!\n", iter->c_str());
+			printf("[Error] File \"%s\" is not a database format (*.db)! Aborting.\n", iter->c_str());
 			return 1;
 		}
 	}
 
 	if(UFile::getExtension(outputDatabasePath).compare("db") != 0)
 	{
-		printf("File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
+		printf("[Error] File \"%s\" is not a database format (*.db)! Aborting.\n", outputDatabasePath.c_str());
 		return 1;
 	}
 
@@ -705,7 +735,7 @@ int main(int argc, char * argv[])
 	std::shared_ptr<DBDriver> dbDriver(DBDriver::create());
 	if(!dbDriver->openConnection(databases.front(), false))
 	{
-		printf("Failed opening the input database!\n");
+		printf("[Error] Failed opening the input database!\n");
 		return 1;
 	}
 
@@ -719,7 +749,7 @@ int main(int argc, char * argv[])
 			std::shared_ptr<DBDriver> lastDbDriver(DBDriver::create());
 			if(!lastDbDriver->openConnection(databases.back(), true))
 			{
-				printf("Failed opening the last input database!\n");
+				printf("[Error] Failed opening the last input database!\n");
 				return 1;
 			}
 			parameters = lastDbDriver->getLastParameters();
@@ -732,7 +762,7 @@ int main(int argc, char * argv[])
 		}
 		if(parameters.empty())
 		{
-			printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", targetVersion.c_str());
+			printf("[Warning] Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", targetVersion.c_str());
 		}
 		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
 	}
@@ -787,7 +817,7 @@ int main(int argc, char * argv[])
 		if(databases.size() > 1)
 		{
 			printf("[Warning] \"pub_loops\" option cannot be used with multiple databases input. "
-					"Disabling \"pub_loops\" to avoid mismatched loop closue ids.\n");
+					"Disabling \"pub_loops\" to avoid mismatched loop closure ids.\n");
 			republishLoopClosures = false;
 		}
 		else
@@ -831,11 +861,30 @@ int main(int argc, char * argv[])
 	dbDriver->getAllNodeIds(ids, false, false, !pubInterNodesAsNormalNodes && !intermediateNodes);
 	if(ids.empty())
 	{
-		printf("Input database doesn't have any nodes saved in it.\n");
+		printf("[Error] Input database doesn't have any nodes saved in it.\n");
 		dbDriver->closeConnection(false);
 		return 1;
 	}
-	if(!((!incrementalMemory || appendMode) && databases.size() > 1))
+
+	int firstDbMapIds = 0;
+	int firstDbLastMapId = -1;
+	firstDbMapIds = getMapIds(ids, *dbDriver).size();
+	if(appendMode || !incrementalMemory)
+	{
+		if(databases.size() < 2)
+		{
+			if(appendMode) {
+				printf("[Error] -a (append mode) requires at least 2 input databases, provided \"%s\"! Aborting.\n", inputDatabasePath.c_str());
+			}
+			else if(!incrementalMemory) {
+				printf("[Error] Localization mode is enabled (%s=false), but it requires at least 2 input databases, provided \"%s\"! Aborting.\n",
+					Parameters::kMemIncrementalMemory().c_str(), inputDatabasePath.c_str());
+			}
+			return 1;
+		}
+		dbDriver->getLastMapId(firstDbLastMapId);
+	}
+	else
 	{
 		totalIds = ids.size();
 	}
@@ -849,16 +898,18 @@ int main(int argc, char * argv[])
 	dbDriver->closeConnection(false);
 
 	// Count remaining ids in the other databases
+	int newDbMapIds = 0;
 	for (std::list<std::string>::iterator iter = ++databases.begin(); iter != databases.end(); ++iter)
 	{
 		if (!dbDriver->openConnection(*iter, false))
 		{
-			printf("Failed opening input database!\n");
+			printf("[Error] Failed opening input database!\n");
 			return 1;
 		}
 		ids.clear();
 		dbDriver->getAllNodeIds(ids, false, false, !pubInterNodesAsNormalNodes && !intermediateNodes);
 		totalIds += ids.size();
+		newDbMapIds += getMapIds(ids, *dbDriver).size();
 		dbDriver->closeConnection(false);
 	}
 	dbDriver.reset();
@@ -895,11 +946,30 @@ int main(int argc, char * argv[])
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
 
+	if(rgbdEnabled && appendMode)
+	{
+		// Get the number of sessions linked in the current global graph
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> constraints;
+		rtabmap.getGraph(poses, constraints, /*optimized*/ false, /*global*/ true, 0, false, false, false, false, false, false);
+		std::set<int> mapIds;
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			int id;
+			if((id=rtabmap.getMemory()->getMapId(iter->first, true))>=0)
+			{
+				mapIds.insert(id);
+			}
+		}
+		printf("Sessions in global graph of the initial database: %ld/%d\n", mapIds.size(), firstDbMapIds);
+		firstDbMapIds = mapIds.size();
+	}
+
 	if(!cameraLocalTransformOffsetOverrides.empty())
 	{
 		if(!cameraLocalTransformOverrides.empty() && cameraLocalTransformOffsetOverrides.size() > 1 && cameraLocalTransformOffsetOverrides.size() != cameraLocalTransformOverrides.size())
 		{
-			printf("Error: -cam_tf_lens_offset size (%ld) is not equal to -cam_tf argument (%ld). "
+			printf("[Error] -cam_tf_lens_offset size (%ld) is not equal to -cam_tf argument (%ld). "
 				   "-cam_tf_lens_offset should be one to affect all cameras or same size than -cam_tf argument.\n",
 				   cameraLocalTransformOffsetOverrides.size(), cameraLocalTransformOverrides.size());
 			showUsage();
@@ -909,7 +979,7 @@ int main(int argc, char * argv[])
 		{
 			if(cameraLocalTransformOffsetOverrides.size() > 1)
 			{
-				printf("Error: -cam_tf_lens_offset size (%ld) should be one if -cam_tf is not set.\n",
+				printf("[Error] -cam_tf_lens_offset size (%ld) should be one if -cam_tf is not set.\n",
 				   cameraLocalTransformOffsetOverrides.size());
 				showUsage();
 				return 1;
@@ -1406,21 +1476,58 @@ int main(int argc, char * argv[])
 		printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d, Intra=%d, Inter=%d)\n",
 			loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion, loopIntra, loopInter);
 
-		if(databases.size()>1)
+		if(rgbdEnabled && incrementalMemory)
 		{
 			std::map<int, Transform> poses;
 			std::multimap<int, Link> constraints;
-			rtabmap.getGraph(poses, constraints, 0, 1, 0, false, false, false, false, false, false);
+			rtabmap.getGraph(poses, constraints, /*optimized*/ false, /*global*/ true, 0, false, false, false, false, false, false);
 			std::set<int> mapIds;
+			std::set<int> newMapIds;
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
 				int id;
 				if((id=rtabmap.getMemory()->getMapId(iter->first, true))>=0)
 				{
 					mapIds.insert(id);
+					if(id>firstDbLastMapId)
+					{
+						newMapIds.insert(id);
+					}
+				}
+			}
+			printf("Sessions linked to the last pose: %ld/%d\n", mapIds.size(), firstDbMapIds + newDbMapIds);
+			if(appendMode)
+			{
+				printf("New sessions connected to the global graph: %ld/%d\n", newMapIds.size(), newDbMapIds);
+				// Check if all new sessions were appended to the original graph.
+				if((int)newMapIds.size() != newDbMapIds)
+				{
+					if(abortDisconnectedSessions) {
+						printf("[Error] Not all new sessions are connected (option -abort_disconnected_sessions is set).\n");
+						return 1;
+					}
+					printf("[Warning] Not all new sessions are connected. Add option -abort_disconnected_sessions to report this as an error.\n");
+				}
+				if(mapIds.size() == newMapIds.size())
+				{
+					if(abortDisconnectedSessions) {
+						printf("[Error] The new sessions are not connected to original map (option -abort_disconnected_sessions is set).\n");
+						return 1;
+					}
+					printf("[Warning] The new sessions are not connected to original map. Add option -abort_disconnected_sessions to report this as an error.\n");
+				}
+			}
+			else // we reprocessed everything
+			{
+				if((int)mapIds.size() != firstDbMapIds + newDbMapIds)
+				{
+					if(abortDisconnectedSessions) {
+						printf("[Error] Not all sessions are connected (option -abort_disconnected_sessions is set).\n");
+						return 1;
+					}
+					printf("[Warning] Not all sessions are connected. Add option -abort_disconnected_sessions to report this as an error.\n");
 				}
 			}
-			printf("Sessions linked to last pose: %ld/%ld\n", mapIds.size(), databases.size());
 		}
 	}