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) <[email protected]>
Co-authored-by: matlabbe <[email protected]>

corelib/src/Graph.cpp

@@ -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())

corelib/src/Optimizer.cpp

@@ -277,7 +277,7 @@ void Optimizer::getConnectedGraph(
 		posesOut.insert(std::make_pair(currentId, currentPose));
 
 		// add prior links
-		for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
+		for(std::multimap<int, Link>::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<int, std::pair<int, Link::Type> >::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.lower_bound(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
 			int toId = iter->second.first;
 			Link::Type type = iter->second.second;

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<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
+				for(std::multimap<int, Link>::iterator iter=_constraints.lower_bound(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
 				{
 					if(iter->second.to() != iter->second.from())
 					{

corelib/src/Signature.cpp

@@ -144,7 +144,7 @@ bool Signature::hasLink(int idTo, Link::Type type) const
 	}
 	else
 	{
-		for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
+		for(std::multimap<int, Link>::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<int, Link>::iterator iter = _links.find(idFrom);
+	std::multimap<int, Link>::iterator iter = _links.lower_bound(idFrom);
 	while(iter != _links.end() && iter->first == idFrom)
 	{
 		Link link = iter->second;

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<int, Link> links;
+	std::multimap<int, std::pair<int, Link::Type> > biLinks;
+	std::multimap<int, int> 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<int, int> links;

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<int, Link>::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
+			for(std::multimap<int, Link>::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<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
+		for(std::multimap<int, Link>::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<Link> & links)
 					infMatrix.clone(),
 					currentLink.userDataCompressed());
 			bool updated = false;
-			std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
+			std::multimap<int, Link>::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<int, Link>::iterator iter = linksRefined_.find(link.from());
+				std::multimap<int, Link>::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<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
+		std::multimap<int, Link>::iterator iter = linksRefined_.lower_bound(currentLink.from());
 		while(iter != linksRefined_.end() && iter->first == currentLink.from())
 		{
 			if(iter->second.to() == currentLink.to() &&