From a0a566140c181e3a4c8f2495106a26c394f3fc9e Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 14 Mar 2019 17:16:51 -0400
Subject: [PATCH] rtabmap::computePath(pose) fixed fatal error when landmarks
 are in the graph

---
 corelib/src/Rtabmap.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index 81dd17d4..59e8196d 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -4655,7 +4655,7 @@ bool Rtabmap::computePath(const Transform & targetPose, float tolerance)
 	UTimer timer;
 	std::map<int, Transform> nodes = _optimizedPoses;
 	std::multimap<int, int> links;
-	for(std::map<int, Transform>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+	for(std::map<int, Transform>::iterator iter=nodes.upper_bound(0); iter!=nodes.end(); ++iter)
 	{
 		const Signature * s = _memory->getSignature(iter->first);
 		UASSERT(s);