From 5c9cfa98fec6968cab08f811865e2dc6b0f71c84 Mon Sep 17 00:00:00 2001 From: Nathan Totten Date: Sun, 27 Sep 2026 21:37:17 -0400 Subject: [PATCH] Use lower_bound() before iterating multimap entries of a key (#1776) Several places look up a multimap with find(key) and then iterate while iter->first == key, assuming find() returns the first element with that key. The standard does not guarantee this, and recent libc++ (Apple clang 21 / libc++ 2200) returns an arbitrary matching element. graph::findLink() then misses existing links and Optimizer::getConnectedGraph() aborts with "Condition (kter!=linksIn.end()) not met!" on graphs with loop closures or multiple sessions (rtabmap-export --opt 0, rtabmap-reprocess, etc.). Replace find() with lower_bound() at those sites and add a regression test. Co-authored-by: Claude Opus 5.5 (1M context) Co-authored-by: matlabbe --- corelib/src/Graph.cpp | 26 +++++++++++++------------- corelib/src/Optimizer.cpp | 4 ++-- corelib/src/Rtabmap.cpp | 2 +- corelib/src/Signature.cpp | 4 ++-- corelib/test/test_graph.cpp | 23 +++++++++++++++++++++++ guilib/src/DatabaseViewer.cpp | 10 +++++----- 6 files changed, 46 insertions(+), 23 deletions(-) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index b93c9798..52dda6d1 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -1073,7 +1073,7 @@ std::multimap::iterator findLink( bool checkBothWays, Link::Type type) { - std::multimap::iterator iter = links.find(from); + std::multimap::iterator iter = links.lower_bound(from); while(iter != links.end() && iter->first == from) { if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type())) @@ -1086,7 +1086,7 @@ std::multimap::iterator findLink( if(checkBothWays) { // let's try to -> from - iter = links.find(to); + iter = links.lower_bound(to); while(iter != links.end() && iter->first == to) { if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type())) @@ -1106,7 +1106,7 @@ std::multimap >::iterator findLink( bool checkBothWays, Link::Type type) { - std::multimap >::iterator iter = links.find(from); + std::multimap >::iterator iter = links.lower_bound(from); while(iter != links.end() && iter->first == from) { if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second)) @@ -1119,7 +1119,7 @@ std::multimap >::iterator findLink( if(checkBothWays) { // let's try to -> from - iter = links.find(to); + iter = links.lower_bound(to); while(iter != links.end() && iter->first == to) { if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second)) @@ -1138,7 +1138,7 @@ std::multimap::iterator findLink( int to, bool checkBothWays) { - std::multimap::iterator iter = links.find(from); + std::multimap::iterator iter = links.lower_bound(from); while(iter != links.end() && iter->first == from) { if(iter->second == to) @@ -1151,7 +1151,7 @@ std::multimap::iterator findLink( if(checkBothWays) { // let's try to -> from - iter = links.find(to); + iter = links.lower_bound(to); while(iter != links.end() && iter->first == to) { if(iter->second == from) @@ -1170,7 +1170,7 @@ std::multimap::const_iterator findLink( bool checkBothWays, Link::Type type) { - std::multimap::const_iterator iter = links.find(from); + std::multimap::const_iterator iter = links.lower_bound(from); while(iter != links.end() && iter->first == from) { if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type())) @@ -1183,7 +1183,7 @@ std::multimap::const_iterator findLink( if(checkBothWays) { // let's try to -> from - iter = links.find(to); + iter = links.lower_bound(to); while(iter != links.end() && iter->first == to) { if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type())) @@ -1203,7 +1203,7 @@ std::multimap >::const_iterator findLink( bool checkBothWays, Link::Type type) { - std::multimap >::const_iterator iter = links.find(from); + std::multimap >::const_iterator iter = links.lower_bound(from); while(iter != links.end() && iter->first == from) { if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second)) @@ -1216,7 +1216,7 @@ std::multimap >::const_iterator findLink( if(checkBothWays) { // let's try to -> from - iter = links.find(to); + iter = links.lower_bound(to); while(iter != links.end() && iter->first == to) { if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second)) @@ -1235,7 +1235,7 @@ std::multimap::const_iterator findLink( int to, bool checkBothWays) { - std::multimap::const_iterator iter = links.find(from); + std::multimap::const_iterator iter = links.lower_bound(from); while(iter != links.end() && iter->first == from) { if(iter->second == to) @@ -1248,7 +1248,7 @@ std::multimap::const_iterator findLink( if(checkBothWays) { // let's try to -> from - iter = links.find(to); + iter = links.lower_bound(to); while(iter != links.end() && iter->first == to) { if(iter->second == from) @@ -1611,7 +1611,7 @@ void reduceGraph( posesToHyperNodes.insert(std::make_pair(id, hyperNodeId)); hyperNodes.insert(std::make_pair(hyperNodeId, id)); - for(std::multimap::const_iterator jter=bidirectionalLoopClosureLinks.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter) + for(std::multimap::const_iterator jter=bidirectionalLoopClosureLinks.lower_bound(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter) { if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() && loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end()) diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index df27522f..4a8a8cd3 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -277,7 +277,7 @@ void Optimizer::getConnectedGraph( posesOut.insert(std::make_pair(currentId, currentPose)); // add prior links - for(std::multimap::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter) + for(std::multimap::const_iterator pter=linksIn.lower_bound(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter) { if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior)) { @@ -285,7 +285,7 @@ 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.lower_bound(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter) { int toId = iter->second.first; Link::Type type = iter->second.second; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 22f2e885..2c9fe90b 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -4664,7 +4664,7 @@ bool Rtabmap::process( int lastId = signaturesRemoved.front(); UDEBUG("Detected that only last signature has been removed (lastId=%d)", lastId); _optimizedPoses.erase(lastId); - for(std::multimap::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter) + for(std::multimap::iterator iter=_constraints.lower_bound(lastId); iter!=_constraints.end() && iter->first==lastId;++iter) { if(iter->second.to() != iter->second.from()) { diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index 1c1ea27a..7aaedb20 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -144,7 +144,7 @@ bool Signature::hasLink(int idTo, Link::Type type) const } else { - for(std::multimap::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter) + for(std::multimap::const_iterator iter=_links.lower_bound(idTo); iter!=_links.end() && iter->first == idTo; ++iter) { if(type == iter->second.type()) { @@ -157,7 +157,7 @@ bool Signature::hasLink(int idTo, Link::Type type) const void Signature::changeLinkIds(int idFrom, int idTo) { - std::multimap::iterator iter = _links.find(idFrom); + std::multimap::iterator iter = _links.lower_bound(idFrom); while(iter != _links.end() && iter->first == idFrom) { Link link = iter->second; diff --git a/corelib/test/test_graph.cpp b/corelib/test/test_graph.cpp index bd1fcc9a..f3c7c9b6 100644 --- a/corelib/test/test_graph.cpp +++ b/corelib/test/test_graph.cpp @@ -124,6 +124,29 @@ TEST(GraphTest, FindLinkForwardAndReverse) EXPECT_NE(graph::findLink(links, 1, 2, true, Link::kNeighbor), links.end()); } +TEST(GraphTest, FindLinkWithManyLinksPerNode) +{ + // multimap::find() may return any element with the key (recent libc++ does), + // so lookups must start from lower_bound() to see every link of a node. + std::multimap links; + std::multimap > biLinks; + std::multimap intLinks; + for(int to=2; to<=40; ++to) + { + insertLink(links, Link(1, to, to%2?Link::kGlobalClosure:Link::kNeighbor, Transform::getIdentity())); + biLinks.insert(std::make_pair(1, std::make_pair(to, to%2?Link::kGlobalClosure:Link::kNeighbor))); + intLinks.insert(std::make_pair(1, to)); + } + for(int to=2; to<=40; ++to) + { + Link::Type type = to%2?Link::kGlobalClosure:Link::kNeighbor; + EXPECT_NE(graph::findLink(links, 1, to, false, type), links.end()) << "to=" << to; + EXPECT_NE(graph::findLink(links, to, 1, true, type), links.end()) << "to=" << to; + EXPECT_NE(graph::findLink(biLinks, 1, to, false, type), biLinks.end()) << "to=" << to; + EXPECT_NE(graph::findLink(intLinks, 1, to), intLinks.end()) << "to=" << to; + } +} + TEST(GraphTest, FindLinkIntMultimap) { std::multimap links; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 6f8585cd..794a0425 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2029,7 +2029,7 @@ void DatabaseViewer::updateIds() envSensors_.insert(std::make_pair(ids_[i], sensors)); if(w>=0) { - for(std::multimap::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter) + for(std::multimap::iterator iter=links.lower_bound(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter) { // Make compatible with old databases, when "weight=-1" was not yet introduced to identify ignored nodes if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged) @@ -2070,7 +2070,7 @@ void DatabaseViewer::updateIds() previousPose=p; //links - for(std::multimap::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter) + for(std::multimap::iterator jter=links.lower_bound(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter) { if(jter->second.type() == Link::kNeighborMerged) { @@ -4852,7 +4852,7 @@ void DatabaseViewer::updateCovariances(const QList & links) infMatrix.clone(), currentLink.userDataCompressed()); bool updated = false; - std::multimap::iterator iter = linksRefined_.find(currentLink.from()); + std::multimap::iterator iter = linksRefined_.lower_bound(currentLink.from()); while(iter != linksRefined_.end() && iter->first == currentLink.from()) { if(iter->second.to() == currentLink.to() && @@ -6681,7 +6681,7 @@ void DatabaseViewer::editConstraint() { cv::Mat covariance = dialog.getCovariance(); Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv()); - std::multimap::iterator iter = linksRefined_.find(link.from()); + std::multimap::iterator iter = linksRefined_.lower_bound(link.from()); while(iter != linksRefined_.end() && iter->first == link.from()) { if(iter->second.to() == link.to() && @@ -9462,7 +9462,7 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed()); bool updated = false; - std::multimap::iterator iter = linksRefined_.find(currentLink.from()); + std::multimap::iterator iter = linksRefined_.lower_bound(currentLink.from()); while(iter != linksRefined_.end() && iter->first == currentLink.from()) { if(iter->second.to() == currentLink.to() &&