From 4812ce4eafe8382aa42071fde9ced4d6aca0435a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 12 Nov 2023 17:14:17 -0800 Subject: [PATCH] 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 | 12 +++++ corelib/src/Graph.cpp | 66 ++++++++++++++++++++++++++++ corelib/src/Optimizer.cpp | 17 +++---- guilib/src/GraphViewer.cpp | 6 +-- 4 files changed, 90 insertions(+), 11 deletions(-) diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 1d53fd0d44..324323d252 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -136,6 +136,12 @@ std::multimap::iterator RTABMAP_CORE_EXPORT findLink( int to, bool checkBothWays = true, Link::Type type = Link::kUndef); +std::multimap >::iterator RTABMAP_CORE_EXPORT findLink( + std::multimap > & links, + int from, + int to, + bool checkBothWays = true, + Link::Type type = Link::kUndef); std::multimap::iterator RTABMAP_CORE_EXPORT findLink( std::multimap & links, int from, @@ -147,6 +153,12 @@ std::multimap::const_iterator RTABMAP_CORE_EXPORT findLink( int to, bool checkBothWays = true, Link::Type type = Link::kUndef); +std::multimap >::const_iterator RTABMAP_CORE_EXPORT findLink( + const std::multimap > & links, + int from, + int to, + bool checkBothWays = true, + Link::Type type = Link::kUndef); std::multimap::const_iterator RTABMAP_CORE_EXPORT findLink( const std::multimap & links, int from, diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 7932ec1443..da85efcc0c 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -1054,6 +1054,39 @@ std::multimap::iterator findLink( return links.end(); } +std::multimap >::iterator findLink( + std::multimap > & links, + int from, + int to, + bool checkBothWays, + Link::Type type) +{ + std::multimap >::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::iterator findLink( std::multimap & links, int from, @@ -1118,6 +1151,39 @@ std::multimap::const_iterator findLink( return links.end(); } +std::multimap >::const_iterator findLink( + const std::multimap > & links, + int from, + int to, + bool checkBothWays, + Link::Type type) +{ + std::multimap >::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::const_iterator findLink( const std::multimap & links, int from, diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index cf1d4e10e3..4898a5a722 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -202,15 +202,15 @@ void Optimizer::getConnectedGraph( std::set nextPoses; nextPoses.insert(fromId); - std::multimap biLinks; + std::multimap > biLinks; for(std::multimap::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.to(), iter->second.from())); + 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(), std::make_pair(iter->second.from(), iter->second.type()))); } } } @@ -234,12 +234,13 @@ void Optimizer::getConnectedGraph( } } - for(std::multimap::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter) + for(std::multimap >::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::const_iterator kter = graph::findLink(linksIn, currentId, toId); + std::multimap::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) { diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 75a13e7603..c9b1110f16 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -251,10 +251,10 @@ class LinkItem: public QGraphicsLineItem 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 & 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();