Optimizer: fixed some constraints between same nodes with different type ignored. GraphViewer: fixed display of links between same nodes with different type

corelib/include/rtabmap/core/Graph.h

@@ -136,6 +136,12 @@ std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
+std::multimap<int, std::pair<int, Link::Type> >::iterator RTABMAP_CORE_EXPORT findLink(
+		std::multimap<int, std::pair<int, Link::Type> > & links,
+		int from,
+		int to,
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
 std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, int> & links,
 		int from,
@@ -147,6 +153,12 @@ std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
+std::multimap<int, std::pair<int, Link::Type> >::const_iterator RTABMAP_CORE_EXPORT findLink(
+		const std::multimap<int, std::pair<int, Link::Type> > & links,
+		int from,
+		int to,
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
 std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, int> & links,
 		int from,

corelib/src/Graph.cpp

@@ -1054,6 +1054,39 @@ std::multimap<int, Link>::iterator findLink(
 	return links.end();
 }
 
+std::multimap<int, std::pair<int, Link::Type> >::iterator findLink(
+		std::multimap<int, std::pair<int, Link::Type> > & links,
+		int from,
+		int to,
+		bool checkBothWays,
+		Link::Type type)
+{
+	std::multimap<int, std::pair<int, Link::Type> >::iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	if(checkBothWays)
+	{
+		// let's try to -> from
+		iter = links.find(to);
+		while(iter != links.end() && iter->first == to)
+		{
+			if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
+			{
+				return iter;
+			}
+			++iter;
+		}
+	}
+	return links.end();
+}
+
 std::multimap<int, int>::iterator findLink(
 		std::multimap<int, int> & links,
 		int from,
@@ -1118,6 +1151,39 @@ std::multimap<int, Link>::const_iterator findLink(
 	return links.end();
 }
 
+std::multimap<int, std::pair<int, Link::Type> >::const_iterator findLink(
+		const std::multimap<int, std::pair<int, Link::Type> > & links,
+		int from,
+		int to,
+		bool checkBothWays,
+		Link::Type type)
+{
+	std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	if(checkBothWays)
+	{
+		// let's try to -> from
+		iter = links.find(to);
+		while(iter != links.end() && iter->first == to)
+		{
+			if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
+			{
+				return iter;
+			}
+			++iter;
+		}
+	}
+	return links.end();
+}
+
 std::multimap<int, int>::const_iterator findLink(
 		const std::multimap<int, int> & links,
 		int from,

corelib/src/Optimizer.cpp

@@ -202,15 +202,15 @@ void Optimizer::getConnectedGraph(
 
 	std::set<int> nextPoses;
 	nextPoses.insert(fromId);
-	std::multimap<int, int> biLinks;
+	std::multimap<int, std::pair<int, Link::Type> > biLinks;
 	for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
 	{
 		if(iter->second.from() != iter->second.to())
 		{
-			if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end())
+			if(graph::findLink(biLinks, iter->second.from(), iter->second.to(), true, iter->second.type()) == biLinks.end())
 			{
-				biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
+				biLinks.insert(std::make_pair(iter->second.from(), std::make_pair(iter->second.to(), iter->second.type())));
-				biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
+				biLinks.insert(std::make_pair(iter->second.to(), std::make_pair(iter->second.from(), iter->second.type())));
 			}
 		}
 	}
@@ -234,12 +234,13 @@ void Optimizer::getConnectedGraph(
 			}
 		}
 
-		for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
+		for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
-			int toId = iter->second;
+			int toId = iter->second.first;
+			Link::Type type = iter->second.second;
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
-				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
+				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
 				if(nextPoses.find(toId) == nextPoses.end())
 				{
 					if(!uContains(posesOut, toId))
@@ -282,7 +283,7 @@ void Optimizer::getConnectedGraph(
 					}
 
 					// only add unique links
-					if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
+					if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
 					{
 						if(kter->second.to() < 0)
 						{

guilib/src/GraphViewer.cpp

@@ -251,10 +251,10 @@ public:
 protected:
 	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
 	{
-		QString str = QString("%1->%2 (%3 m)").arg(_from).arg(_to).arg(_poseA.getDistance(_poseB));
+		QString str = QString("%1->%2 (type=%3 length=%4 m)").arg(_from).arg(_to).arg(_link.type()).arg(_poseA.getDistance(_poseB));
 		if(!_link.transform().isNull())
 		{
-			str.append(QString("\n%1\n%2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance()));
+			str.append(QString("\n%1\nvar= %2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance()));
 		}
 		this->setToolTip(str);
 		QPen pen = this->pen();
@@ -566,7 +566,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				itemIter = _linkItems.find(iter->first);
 				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
 				{
-					if(itemIter.value()->to() == idTo)
+					if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type())
 					{
 						itemIter.value()->setPoses(poseA, poseB, _viewPlane);
 						itemIter.value()->show();