From 61f6e5ff79d1b151669a6735b8a239c89e6a888a Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 8 Jul 2016 19:22:48 -0400
Subject: [PATCH] Using fly distance instead of path distance to update the
 farthest goals

---
 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 e8cb2ff0..d46ded00 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -3787,7 +3787,7 @@ void Rtabmap::updateGoalIndex()
 				{
 					if(_localRadius > 0.0f)
 					{
-						distanceFromCurrentNode += _path[i-1].second.getDistance(_path[i].second);
+						distanceFromCurrentNode = _path[_pathCurrentIndex].second.getDistance(_path[i].second);
 					}
 
 					if((goalIndex == _pathCurrentIndex && i == _path.size()-1) ||