i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use lower_bound() before iterating multimap entries of a key (#1776)

Browse Source 
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) <[email protected]>
Co-authored-by: matlabbe <[email protected]>
This commit is contained in:
Nathan Totten
2026-09-27 18:37:17 -07:00
committed by GitHub
co-authored by Claude Opus 5.5 matlabbe
parent 8035be52ff
commit 5c9cfa98fe
6 changed files with 46 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Graph.cpp
+13 -13
View File
@@ -1073,7 +1073,7 @@ std::multimap<int, Link>::iterator findLink(
bool checkBothWays,
Link::Type type)
{
std::multimap<int, Link>::iterator iter = links.find(from);
std::multimap<int, Link>::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<int, Link>::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<int, std::pair<int, Link::Type> >::iterator findLink(
bool checkBothWays,
Link::Type type)
{
std::multimap<int, std::pair<int, Link::Type> >::iterator iter = links.find(from);
std::multimap<int, std::pair<int, Link::Type> >::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<int, std::pair<int, Link::Type> >::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<int, int>::iterator findLink(
int to,
bool checkBothWays)
{
std::multimap<int, int>::iterator iter = links.find(from);
std::multimap<int, int>::iterator iter = links.lower_bound(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second == to)
@@ -1151,7 +1151,7 @@ std::multimap<int, int>::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<int, Link>::const_iterator findLink(
bool checkBothWays,
Link::Type type)
{
std::multimap<int, Link>::const_iterator iter = links.find(from);
std::multimap<int, Link>::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<int, Link>::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<int, std::pair<int, Link::Type> >::const_iterator findLink(
bool checkBothWays,
Link::Type type)
{
std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter = links.find(from);
std::multimap<int, std::pair<int, Link::Type> >::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<int, std::pair<int, Link::Type> >::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<int, int>::const_iterator findLink(
int to,
bool checkBothWays)
{
std::multimap<int, int>::const_iterator iter = links.find(from);
std::multimap<int, int>::const_iterator iter = links.lower_bound(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second == to)
@@ -1248,7 +1248,7 @@ std::multimap<int, int>::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<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
for(std::multimap<int, Link>::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())