fixed some edge cases

Author: matlabbe

corelib/include/rtabmap/core/Memory.h

@@ -144,7 +144,7 @@ class RTABMAP_CORE_EXPORT Memory
 	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
-	int reduceNode(int id, float maxProximityDistance = 0.0f, bool keepLinkedInDb = false);
+	int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false);
 
 	//getters
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
@@ -278,6 +278,7 @@ class RTABMAP_CORE_EXPORT Memory
 	void initCountId();
 	void rehearsal(Signature * signature, Statistics * stats = 0);
 	bool rehearsalMerge(int oldId, int newId);
+	std::set<int> reduceNode(int id, float maxDistance, bool keepLinkedInDb, bool propagateNeighborMergedLinks);
 
 	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
 

corelib/src/Memory.cpp

@@ -1185,47 +1185,57 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
 	}
 }
 
-int Memory::reduceNode(int id, float maxProximityDistance, bool keepLinkedInDb)
+int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb)
 {
+	std::set<int> reducedTo = reduceNode(id, maxDistance, keepLinkedInDb, false);
+	return reducedTo.empty()?0:*reducedTo.rbegin();
+}
+
+bool canBeReduced(const Link & link, float maxDistance)
+{
+	return  link.to() != link.from() &&
+			link.type() != Link::kNeighbor &&
+			link.type() != Link::kNeighborMerged &&
+			link.userDataCompressed().empty() &&
+			link.type() != Link::kUndef &&
+			link.type() != Link::kVirtualClosure &&
+			(maxDistance==0 || link.transform().getNormSquared() < maxDistance*maxDistance);
+}
+
+std::set<int> Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, bool propagateNeighborMergedLinks)
+{
+	std::set<int> reducedTo;
 	Signature * s = this->_getSignature(id);
-	UASSERT(s!=0);
+	if(s==0)
+	{
+		UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
+		return reducedTo;
+	}
 
 	if(!s->getLabel().empty())
 	{
 		// We currently not remove nodes with labels
-		return 0;
+		return reducedTo;
 	}
 
-	int reducedTo = 0;
-	bool merge = false;
 	const std::multimap<int, Link> & links = s->getLinks();
 	std::map<int, Link> neighbors;
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
-		if(!merge || iter->second.type() == Link::kGlobalClosure) // prioritize reducing to global loop closures
-		{
-			merge = iter->second.to() != iter->second.from() &&
-					iter->second.type() != Link::kNeighbor &&
-					iter->second.type() != Link::kNeighborMerged &&
-					iter->second.userDataCompressed().empty() &&
-					iter->second.type() != Link::kUndef &&
-					iter->second.type() != Link::kVirtualClosure &&
-					(maxProximityDistance==0 || 
-					 (iter->second.type() == Link::kLocalSpaceClosure && 
-					  iter->second.transform().getNormSquared() < maxProximityDistance*maxProximityDistance)); // in case of far lidar proximity links
-			if(merge)
+			if(canBeReduced(iter->second, maxDistance))
 			{
-				reducedTo = iter->second.to();
-				UDEBUG("Reduce %d to %d", s->id(), reducedTo);
+				reducedTo.insert(iter->second.to());
+				UDEBUG("Reduce %d to %d (distance=%f)", s->id(), iter->second.to(), iter->second.transform().getNorm());
 			}
-		}
+		
 		if(iter->second.type() == Link::kNeighbor)
 		{
 			neighbors.insert(*iter);
 		}
 	}
-	if(merge)
+	if(!reducedTo.empty())
 	{
+		std::vector<int> propagateIds;
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
 			Signature * sTo = this->_getSignature(iter->first);
@@ -1234,20 +1244,21 @@ int Memory::reduceNode(int id, float maxProximityDistance, bool keepLinkedInDb)
 				UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
 				sTo->removeLink(s->id());
 				if(iter->second.type() != Link::kNeighbor &&
-					iter->second.type() != Link::kNeighborMerged &&
+					(iter->second.type() != Link::kNeighborMerged || propagateNeighborMergedLinks) &&
 					iter->second.type() != Link::kUndef)
 				{
 					// link to all neighbors
 					for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
 					{
 						if(!sTo->hasLink(jter->second.to()))
 						{
-							UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)",
-									iter->second.from(), iter->second.to(), iter->second.type(),
-									jter->second.from(), jter->second.to(), jter->second.type());
 							Link l = iter->second.inverse().merge(
 									jter->second,
 									iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
+							UDEBUG("Merging link %d->%d (type=%d) to with %d->%d (type %d). Adding %d->%d (type %d) to %d and %d",
+								iter->second.to(), iter->second.from(), iter->second.type(),
+								jter->second.from(), jter->second.to(), jter->second.type(),
+								l.from(), l.to(), l.type(), sTo->id(), l.to());
 							sTo->addLink(l);
 							Signature * sB = this->_getSignature(l.to());
 							UASSERT(sB!=0);
@@ -1281,6 +1292,19 @@ int Memory::reduceNode(int id, float maxProximityDistance, bool keepLinkedInDb)
 						}
 					}
 				}
+				
+				// Check if node merged to can also be reduced, if so, we have to propagate the neighbor merged links to the next node
+				if(reducedTo.find(sTo->id()) != reducedTo.end())
+				{
+					for(std::multimap<int, Link>::const_iterator lter=sTo->getLinks().begin(); lter!=sTo->getLinks().end(); ++lter)
+					{
+						if(canBeReduced(lter->second, maxDistance))
+						{
+							UDEBUG("Will progagate %d to %d (distance=%f)", sTo->id(), lter->second.to(), lter->second.transform().getNorm());
+							propagateIds.push_back(lter->second.to());
+						}
+					}
+				}
 			}
 		}
 
@@ -1300,6 +1324,15 @@ int Memory::reduceNode(int id, float maxProximityDistance, bool keepLinkedInDb)
 		s = 0;
 		_linksChanged = true;
 		_memoryChanged = true;
+
+		for(auto id: propagateIds)
+		{
+			//Nodes can be already reduced by other nodes, check if they are still there
+			if(getSignature(id) != 0)
+			{
+				reducedTo = reduceNode(id, maxDistance, keepLinkedInDb, true);
+			}
+		}
 	}
 	return reducedTo;
 }

tools/ReduceGraph/main.cpp

@@ -54,7 +54,8 @@ void showUsage(const char * exec)
 			"Options:\n"
 			"    --keep_latest  Merge old nodes to newer nodes, thus keeping only latest nodes.\n"
 			"    --keep_linked  Keep reduced nodes linked to graph.\n"
-			"\n%s", exec, Parameters::showUsage());
+			"    --radius #.#   Maximum loop closure distance that can be merged.\n"
+			"\n", exec);
 	exit(1);
 }
 
@@ -70,6 +71,7 @@ int main(int argc, char * argv[])
 
 	bool keepLatest = false;
 	bool keepLinked = false;
+	float radius = 0.0f;
 	for(int i=1; i<argc-1; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -84,10 +86,24 @@ int main(int argc, char * argv[])
 		{
 			keepLinked = uStr2Bool(argv[i]);
 		}
+		else if(std::strcmp(argv[i], "--radius") == 0)
+		{
+			++i;
+			if(i < argc-1)
+			{
+				radius = uStr2Float(argv[i]);
+			}
+			else {
+				showUsage(argv[0]);
+			}
+		}
 	}
-	ParametersMap inputParams = Parameters::parseArguments(argc,  argv);
+	
+	// Just parse logging options
+	Parameters::parseArguments(argc, argv);
 
-	// Add some optimizations (soft set, can be overriden by arguments)
+	// Add some optimizations
+	ParametersMap inputParams;
 	inputParams.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true")); // load the whole map in RAM
 	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
 	inputParams.insert(ParametersPair(Parameters::kKpNNStrategy(), "3")); // don't need flann index
@@ -96,6 +112,7 @@ int main(int argc, char * argv[])
 	if(!UFile::exists(dbPath))
 	{
 		printf("Database %s doesn't exist!\n", dbPath.c_str());
+		return 1;
 	}
 
 	printf("\nDatabase: %s\n", dbPath.c_str());
@@ -114,11 +131,6 @@ int main(int argc, char * argv[])
 	}
 	delete driver;
 
-	for(ParametersMap::iterator iter=inputParams.begin(); iter!=inputParams.end(); ++iter)
-	{
-		printf(" Using parameter \"%s=%s\" from arguments\n", iter->first.c_str(), iter->second.c_str());
-	}
-
 	Memory memory;
 	printf("Initialization...\n");
 	UTimer timer;
@@ -140,30 +152,25 @@ int main(int argc, char * argv[])
 	Transform lastLocalizationPose;
 	bool hasOptimizedPoses = !memory.loadOptimizedPoses(&lastLocalizationPose).empty();
 
-	float proximityMaxRadius = Parameters::defaultRGBDProximityPathFilteringRadius();
-	Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityMaxRadius);
 	int totalNodesReduced = 0;
+	std::vector<int> vids;
+	vids.reserve(ids.size());
 	if(keepLatest)
 	{
 		// we process older to newer nodes, merging to new nodes
-		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
-		{
-			int id = *iter;
-			int reducedId = memory.reduceNode(id, proximityMaxRadius, keepLinked);
-			if(reducedId > 0)
-			{
-				printf("Reduced node %d to node %d!\n", id, reducedId);
-				++totalNodesReduced;
-			}
-		}
+		vids.insert(vids.end(), ids.begin(), ids.end());
 	}
 	else
 	{
 		// we process newer to older nodes, merging to old nodes
-		for(std::set<int>::reverse_iterator iter = ids.rbegin(); iter!=ids.rend(); ++iter)
+		vids.insert(vids.end(), ids.rbegin(), ids.rend());
+	}
+	for(auto id: vids)
+	{
+		// Nodes can be already reduced by other nodes, check if they are still there
+		if(memory.getSignature(id) != 0)
 		{
-			int id = *iter;
-			int reducedId = memory.reduceNode(id, proximityMaxRadius, keepLinked);
+			int reducedId = memory.reduceNode(id, radius, keepLinked);
 			if(reducedId > 0)
 			{
 				printf("Reduced node %d to node %d!\n", id, reducedId);