From 1c555f321b2b952da8cf235c0281d94adea32c12 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 7 Feb 2017 15:57:52 -0500
Subject: [PATCH] Fixed Tango odometry variance

---
 app/android/AndroidManifest.xml.in |  2 +-
 app/android/jni/CameraTango.cpp    |  4 ++--
 app/android/jni/RTABMapApp.cpp     | 12 ++++++++----
 app/android/res/values/strings.xml |  2 +-
 4 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in
index 9623da72..079322b5 100644
--- a/app/android/AndroidManifest.xml.in
+++ b/app/android/AndroidManifest.xml.in
@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="35"
+        android:versionCode="36"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp
index 94de5816..6c97ee19 100644
--- a/app/android/jni/CameraTango.cpp
+++ b/app/android/jni/CameraTango.cpp
@@ -685,8 +685,8 @@ void CameraTango::mainLoop()
 				stampEpochOffset_ = UTimer::now()-data.stamp();
 			}
 			data.setStamp(stampEpochOffset_ + data.stamp());
-			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
-			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
+			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.000001);
+			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.000001, firstFrame_?9999:0.000001));
 			firstFrame_ = false;
 		}
 		else if(!this->isKilled())
diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp
index 5070c7fd..61b2f790 100644
--- a/app/android/jni/RTABMapApp.cpp
+++ b/app/android/jni/RTABMapApp.cpp
@@ -101,13 +101,10 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 		int featuresLoop = uStr2Int(parameters.at(rtabmap::Parameters::kVisMaxFeatures()));
 		if(featuresVoc==0 || featuresLoop < featuresVoc)
 		{
-			// re-use already extracted descriptors
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemRawDescriptorsKept(), std::string("true")));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
 		}
 		else
 		{
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemRawDescriptorsKept(), std::string("false")));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("true")));
 		}
 	}
@@ -2226,8 +2223,15 @@ int RTABMapApp::postProcessing(int approach)
 		// detect more loop closures
 		if(approach == -1 || approach == 2)
 		{
-			// detect more loop closures
+			// detect more loop closures, don't re-extract features for this
+			rtabmap::ParametersMap parameters;
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
+			rtabmap_->parseParameters(parameters);
+
 			returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1);
+
+			// put back re-extraction if it was set
+			rtabmap_->parseParameters(this->getRtabmapParameters());
 		}
 
 		// graph optimization
diff --git a/app/android/res/values/strings.xml b/app/android/res/values/strings.xml
index b4167418..275deeaa 100644
--- a/app/android/res/values/strings.xml
+++ b/app/android/res/values/strings.xml
@@ -64,7 +64,7 @@
     <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>	<string name="pref_default_max_texture_distance">0</string>
     <string name="pref_key_block_render">pref_key_block_render</string>					<string name="pref_default_block_render">false</string>
     <string name="pref_key_opt_depth">pref_key_opt_depth</string>						<string name="pref_default_opt_depth">8</string>
-    <string name="pref_key_opt_decimation_factor">pref_key_opt_decimation_factor</string><string name="pref_default_opt_decimation_factor">80</string>
+    <string name="pref_key_opt_decimation_factor">pref_key_opt_decimation_factor</string><string name="pref_default_opt_decimation_factor">60</string>
     <string name="pref_key_opt_color_radius">pref_key_opt_color_radius</string>			<string name="pref_default_opt_color_radius">0</string>
     <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>			<string name="pref_default_opt_clean_white">true</string>