exposing neighbor merged ratio parameter to the tool

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 maxDistance = 0.0f, bool keepLinkedInDb = false);
+	int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, float neighborMergedRatio = 0.2);
 
 	//getters
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
@@ -278,7 +278,7 @@ class RTABMAP_CORE_EXPORT Memory
 	void initCountId();
 	void rehearsal(Signature * signature, Statistics * stats = 0);
 	bool rehearsalMerge(int oldId, int newId);
-	std::map<int, float> reduceNodeImpl(int id, float maxDistance, bool keepLinkedInDb);
+	std::map<int, float> reduceNodeImpl(int id, float maxDistance, bool keepLinkedInDb, float neighborMergedRatio);
 
 	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
 

corelib/src/Memory.cpp

@@ -1288,9 +1288,9 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
 	}
 }
 
-int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb)
+int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, float neighborMergedRatio)
 {
-	std::map<int, float> reducedTo = reduceNodeImpl(id, maxDistance, keepLinkedInDb);
+	std::map<int, float> reducedTo = reduceNodeImpl(id, maxDistance, keepLinkedInDb, neighborMergedRatio);
 	return reducedTo.empty()?0:reducedTo.rbegin()->first;
 }
 
@@ -1305,7 +1305,7 @@ bool canBeReduced(const Link & link, float maxDistance)
 			(maxDistance==0 || link.transform().getNorm() < maxDistance);
 }
 
-std::map<int, float> Memory::reduceNodeImpl(int id, float maxDistance, bool keepLinkedInDb)
+std::map<int, float> Memory::reduceNodeImpl(int id, float maxDistance, bool keepLinkedInDb, float neighborMergedRatio)
 {
 	UDEBUG("Reducing %d (max distance=%f, keep linked in db=%s)", id, maxDistance, keepLinkedInDb?"true":"false");
 	std::map<int, float> reducedTo;
@@ -1353,15 +1353,15 @@ std::map<int, float> Memory::reduceNodeImpl(int id, float maxDistance, bool keep
 				{
 					if(iter->second.type() == Link::kNeighborMerged)
 					{
+						s->removeLink(sTo->id());
 						if(maxDistance == 0.0f)
 						{
 							// online graph reduction, always skip these links
 							continue;
 						}
-						s->removeLink(sTo->id());
 						std::list<std::pair<int, Transform> > path = graph::computePath(s->id(), sTo->id(), this, false);
 						float pathLength = graph::computePathLength(uListToVector(path));
-						if(!path.empty() && iter->second.transform().getNorm() / pathLength > 0.2)
+						if(!path.empty() && pathLength>0.0f && iter->second.transform().getNorm() / pathLength > neighborMergedRatio)
 						{
 							// skip, reachable by another path of similar size
 							continue;
@@ -1429,18 +1429,6 @@ std::map<int, float> Memory::reduceNodeImpl(int id, float maxDistance, bool keep
 			}
 		}
 
-		//remove neighbor links
-		/*std::multimap<int, Link> linksCopy = links;
-		for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
-		{
-			if(iter->second.type() == Link::kNeighborMerged)
-			{
-				// Removing only merged neighbor links, we keep original neighbor 
-				// links to be able to reprocess databases with correct odometry covariance.
-				s->removeLink(iter->first);
-			}
-		}*/
-
 		this->moveToTrash(s, keepLinkedInDb);
 		s = 0;
 		_linksChanged = true;
@@ -1451,7 +1439,7 @@ std::map<int, float> Memory::reduceNodeImpl(int id, float maxDistance, bool keep
 			//Nodes can be already reduced by other nodes, check if they are still there
 			if(getSignature(pair.first) != 0)
 			{
-				reducedTo = reduceNodeImpl(pair.first, pair.second, keepLinkedInDb);
+				reducedTo = reduceNodeImpl(pair.first, pair.second, keepLinkedInDb, neighborMergedRatio);
 			}
 		}
 	}

tools/ReduceGraph/main.cpp

@@ -55,6 +55,7 @@ void showUsage(const char * exec)
 			"    --keep_latest  Merge old nodes to newer nodes, thus keeping only latest nodes.\n"
 			"    --keep_linked  Keep reduced nodes linked to graph.\n"
 			"    --radius #.#   Maximum loop closure distance that can be merged. Default is 1 m. Should be > 0.\n"
+			"    --ratio #.#    Neighbor links with length over this ratio relative to graph distance to same node are not propagated. Default is 0.2.\n"
 			"    --udebug/--uinfo/--warn can also be used to change verbosity.\n"
 			"\n", exec);
 	exit(1);
@@ -73,6 +74,7 @@ int main(int argc, char * argv[])
 	bool keepLatest = false;
 	bool keepLinked = false;
 	float radius = 1.0f;
+	float ratio = 0.2f;
 	for(int i=1; i<argc; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -103,6 +105,17 @@ int main(int argc, char * argv[])
 				showUsage(argv[0]);
 			}
 		}
+		else if(std::strcmp(argv[i], "--ratio") == 0)
+		{
+			++i;
+			if(i < argc-1)
+			{
+				ratio = uStr2Float(argv[i]);
+			}
+			else {
+				showUsage(argv[0]);
+			}
+		}
 	}
 	printf("Parameters:\n");
 	printf("  radius = %f m\n", radius);
@@ -183,7 +196,7 @@ int main(int argc, char * argv[])
 		// Nodes can be already reduced by other nodes, check if they are still there
 		if(memory.getSignature(id) != 0)
 		{
-			int reducedId = memory.reduceNode(id, radius, keepLinked);
+			int reducedId = memory.reduceNode(id, radius, keepLinked, ratio);
 			if(reducedId > 0)
 			{
 				printf("Reduced node %d to node %d!\n", id, reducedId);