Fixed some issues related to intermediate nodes and retrieval/immunization (#1733)

* Allow ignoring intermediate nodes when calculating WM size * Fix the retrieval logic to avoid unnecessary code execution * Making getWorkingMemSize(true) complexity O(1) * Updated statistics to show STM size, WM size, immunized nodes without intermediate nodes count. Added two new statistics to track number of intermediate nodes in STM and WM. * Correct the condition for counting intermediate nodes * Restore the logic of retrieval 2/3 to avoid affecting updateAge --------- Co-authored-by: matlabbe <[email protected]>
2026-10-07 10:37:47 +08:00 · 2026-07-14 10:14:11 -07:00
parent d96c30710e
commit 0f56dd4b43
5 changed files with 161 additions and 62 deletions
@@ -131,6 +131,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_memoryChanged(false),
 	_linksChanged(false),
 	_signaturesAdded(0),
+	_workingMemIntermediateNodesCount(0),
+	_stMemIntermediateNodesCount(0),
 	_allNodesInWM(true),
 	_receivingOdometryFeatures(false),
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
@@ -267,6 +269,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()));
 				}
@@ -1079,7 +1085,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 "
@@ -1255,6 +1261,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()));
 		}
@@ -1276,6 +1286,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()));
@@ -1466,6 +1480,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()
@@ -1486,6 +1505,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
@@ -2022,6 +2054,7 @@ void Memory::clear()
 		ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
 	}
 	_stMem.clear();
+	_stMemIntermediateNodesCount = 0;

 	this->cleanUnusedWords();

@@ -2038,18 +2071,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)
@@ -2096,6 +2126,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());
@@ -2482,7 +2513,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>();
 		}
@@ -2590,18 +2621,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;
@@ -2618,7 +2652,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);
 			}
@@ -2727,6 +2761,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())
 		{
@@ -3054,6 +3102,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
@@ -5194,11 +5255,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