Updated how local scan matching is done (doing it on segmented local paths)

corelib/src/Graph.cpp

@@ -907,6 +907,34 @@ std::multimap<int, Link>::iterator findLink(
 	return links.end();
 }
 
+std::multimap<int, int>::iterator findLink(
+		std::multimap<int, int> & links,
+		int from,
+		int to)
+{
+	std::multimap<int, int>::iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second == to)
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	// let's try to -> from
+	iter = links.find(to);
+	while(iter != links.end() && iter->first == to)
+	{
+		if(iter->second == from)
+		{
+			return iter;
+		}
+		++iter;
+	}
+	return links.end();
+}
+
 std::map<int, Transform> radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
 		float radius,