Reduce graph: remove orphan nodes from WM after reduction

corelib/include/rtabmap/core/Memory.h

@@ -141,7 +141,7 @@ public:
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
 	void convertToIntermediate(int locationId);
-	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
+	void deleteLocation(int locationId, std::list<int> * deletedWords = 0, bool keepLinked = false);
 	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true, bool occupancyGrid = true);

corelib/src/Memory.cpp

@@ -3071,13 +3071,15 @@ void Memory::convertToIntermediate(int locationId)
 	}
 }
 
-void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
+void Memory::deleteLocation(int locationId, std::list<int> * deletedWords, bool keepLinked = false)
 {
 	UDEBUG("Deleting location %d", locationId);
 	Signature * location = _getSignature(locationId);
 	if(location)
 	{
-		this->moveToTrash(location, false, deletedWords);
+		this->moveToTrash(location, keepLinked, deletedWords);
+		_linksChanged = true;
+		_memoryChanged = true;
 	}
 }
 

tools/ReduceGraph/main.cpp

@@ -235,6 +235,19 @@ int main(int argc, char * argv[])
 	}
 	printf("Reduced a total of %d nodes out of %ld nodes\n", totalNodesReduced, ids.size());
 
+	// Remove orphan nodes
+	std::map<int, double> wm = memory.getWorkingMem();
+	for(auto iter: wm)
+	{
+		std::multimap<int, rtabmap::Link> links = memory.getLinks(iter.first);
+		if(graph::filterLinks(links, Link::kSelfRefLink).empty())
+		{
+			// orphan
+			memory.deleteLocation(iter.first, 0, keepLinked);
+			printf("Removed orphan node %d\n", iter.first);
+		}
+	}
+
 	if(!optimizedPoses.empty())
 	{
 		size_t removed = 0;