From cd62e67011e09b55dd1b2437c6f0912c385acc8e Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Mon, 5 Dec 2016 15:16:04 -0500
Subject: [PATCH] Localization: Revert mapCorrection transform to one before
 being lost (when odom can be computed again)

---
 corelib/include/rtabmap/core/Rtabmap.h |  1 +
 corelib/src/Rtabmap.cpp                | 16 ++++++++++++++++
 guilib/src/PreferencesDialog.cpp       |  1 -
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h
index 1e08ef3f..1f404e52 100644
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -252,6 +252,7 @@ private:
 	std::map<int, Transform> _optimizedPoses;
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
+	Transform _mapCorrectionBackup; // used in localization mode when odom is lost
 	Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
 	int _lastLocalizationNodeId; // for localization mode
 
diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index 569a74b7..8d2cd461 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -330,6 +330,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_optimizedPoses.clear();
 	_constraints.clear();
 	_mapCorrection.setIdentity();
+	_mapCorrectionBackup.setNull();
 	_lastLocalizationPose.setNull();
 	_lastLocalizationNodeId = 0;
 	_distanceTravelled = 0.0f;
@@ -647,6 +648,8 @@ int Rtabmap::triggerNewMap()
 		_optimizedPoses.clear();
 		_constraints.clear();
 		_lastLocalizationNodeId = 0;
+		_mapCorrection.setIdentity();
+		_mapCorrectionBackup.setNull();
 
 		//Verify if there are nodes that were merged through graph reduction
 		if(reducedIds.size() && _path.size())
@@ -781,6 +784,7 @@ void Rtabmap::resetMemory()
 	_optimizedPoses.clear();
 	_constraints.clear();
 	_mapCorrection.setIdentity();
+	_mapCorrectionBackup.setNull();
 	_lastLocalizationPose.setNull();
 	_lastLocalizationNodeId = 0;
 	_distanceTravelled = 0.0f;
@@ -879,8 +883,15 @@ bool Rtabmap::process(
 	//============================================================
 	// If RGBD SLAM is enabled, a pose must be set.
 	//============================================================
+	bool fakeOdom = false;
 	if(_rgbdSlamMode)
 	{
+		if(!_memory->isIncremental() && !odomPose.isNull() && !_mapCorrectionBackup.isNull())
+		{
+			_mapCorrection = _mapCorrectionBackup;
+			_mapCorrectionBackup.setNull();
+		}
+
 		if(odomPose.isNull())
 		{
 			if(_memory->isIncremental())
@@ -895,6 +906,7 @@ bool Rtabmap::process(
 				{
 					_lastLocalizationPose = Transform::getIdentity();
 				}
+				fakeOdom = true;
 				odomPose = _mapCorrection.inverse() * _lastLocalizationPose;
 			}
 		}
@@ -2215,6 +2227,10 @@ bool Rtabmap::process(
 
 		// Update map correction, it should be identify when optimizing from the last node
 		UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
+		if(fakeOdom && _mapCorrectionBackup.isNull())
+		{
+			_mapCorrectionBackup = _mapCorrection;
+		}
 		_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
 		_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
 		if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index 3ba446e2..c5061789 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -49,7 +49,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ui_preferencesDialog.h"
 
 #include "rtabmap/core/Version.h"
-#include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryThread.h"