Updated default RGBD/OptimizeMaxError to 0.05 m for Tango app

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="7"
+        android:versionCode="8"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />

app/android/jni/RTABMapApp.cpp

@@ -77,7 +77,8 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // PnP
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.05")));
 
 	return parameters;
 }
@@ -1084,8 +1085,10 @@ void RTABMapApp::handleEvent(UEvent * event)
 		int words = (int)uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
 		float updateTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
 		int loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
+		int highestHypId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f);
 		int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
 		int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
+		int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
 		int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
 		float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
 
@@ -1101,7 +1104,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIFI)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFII)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -1111,11 +1114,13 @@ void RTABMapApp::handleEvent(UEvent * event)
 								totalPolygons_,
 								updateTime,
 								loopClosureId,
+								highestHypId,
 								databaseMemoryUsed,
 								inliers,
 								featuresExtracted,
 								hypothesis,
-								lastDrawnCloudsCount_);
+								lastDrawnCloudsCount_,
+								rejected);
 						success = true;
 					}
 				}

app/android/res/menu/optionmenu.xml

@@ -49,11 +49,16 @@
 	           <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
 	           <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
 	           <item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
-	           <item android:id="@+id/update_rate" android:checkable="false" android:title="Map Update Rate..." />
-	           <item android:id="@+id/time_threshold" android:checkable="false" android:title="Time Threshold..." />
-	           <item android:id="@+id/loop_threshold" android:checkable="false" android:title="Loop Closure Threshold..." />
-	           <item android:id="@+id/features" android:checkable="false" android:title="Max Features Extracted..." />
 	           <item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
+	           <item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters...">
+		           <menu >
+			           <item android:id="@+id/update_rate" android:title="Map Update Rate..." />
+			           <item android:id="@+id/time_threshold" android:title="Time Threshold..." />
+			           <item android:id="@+id/loop_threshold" android:title="Loop Closure Threshold..." />
+			           <item android:id="@+id/optimize_error" android:title="Max Optimization Error..." />
+			           <item android:id="@+id/features" android:title="Max Features Extracted..." />
+			       </menu>
+			    </item>
 	       </group>   
 	       </menu>
 	   </item>

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -85,11 +85,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private int mParamTimeThrMsIndex = 4;
   private int mParamMaxFeaturesIndex = 4;
   private int mParamLoopThrMsIndex = 1;
+  private int mParamOptimizeErrorIndex = 3;
   
   final String[] mUpdateRateValues = {"0.5", "1", "2", "Max"};
   final String[] mTimeThrValues = {"400", "500", "600", "700", "800", "900", "1000", "1100", "1200", "1300", "1400", "1500", "No Limit"};
   final String[] mMaxFeaturesValues = {"Disabled", "100", "200", "300", "400", "500", "600", "700", "800", "900", "1000", "No Limit"};
   final String[] mLoopThrValues = {"Disabled", "0.11", "0.20", "0.30", "0.40", "0.50", "0.60", "0.70", "0.80", "0.90"};
+  final String[] mOptimizeErrorValues = {"Disabled", "0.01", "0.025", "0.05", "0.1", "0.2", "0.35", "0.5", "1"};
   
   private LinearLayout mLayoutDebug;
   
@@ -310,11 +312,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  int polygons,
 		  float updateTime, 
 		  int loopClosureId,
+		  int highestHypId,
 		  int databaseMemoryUsed,
 		  int inliers,
 		  int featuresExtracted,
 		  float hypothesis,
-		  int nodesDrawn)
+		  int nodesDrawn,
+		  int rejected)
   {
 	  if(mItemPause!=null)
 	  {
@@ -330,13 +334,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  ((TextView)findViewById(R.id.inliers)).setText(String.valueOf(inliers));
 	  ((TextView)findViewById(R.id.features)).setText(String.format("%d / %s", featuresExtracted, mMaxFeaturesValues[mParamMaxFeaturesIndex]));
 	  ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f / %s", updateTime, mTimeThrValues[mParamTimeThrMsIndex]));
-	  ((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f / %s (%d)", hypothesis, mLoopThrValues[mParamLoopThrMsIndex], loopClosureId));
+	  ((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f / %s (%d)", hypothesis, mLoopThrValues[mParamLoopThrMsIndex], loopClosureId>0?loopClosureId:highestHypId));
-	  if(loopClosureId > 0)
+	  if(mItemPause!=null && !mItemPause.isChecked())
 	  {
-		  ++mTotalLoopClosures;
+		  if(loopClosureId > 0)
+		  {
+			  ++mTotalLoopClosures;
 	
-		  mToast.setText("Loop closure detected!");
+			  mToast.setText("Loop closure detected!");
-		  mToast.show();
+			  mToast.show();
+		  }
+		  else if(rejected > 0 && inliers >= 15)
+		  {
+			  mToast.setText("Loop closure rejected after graph optimization.");
+			  mToast.show();
+		  }
 	  }
 	  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
   }
@@ -349,17 +361,19 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final int polygons,
 		  final float updateTime, 
 		  final int loopClosureId,
+		  final int highestHypId,
 		  final int databaseMemoryUsed,
 		  final int inliers,
 		  final int features,
 		  final float hypothesis,
-		  final int nodesDrawn)
+		  final int nodesDrawn,
+		  final int rejected)
   {
 	  Log.i(TAG, String.format("updateStatsCallback()"));
 
 	  runOnUiThread(new Runnable() {
 		    public void run() {
-		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn);
+		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, rejected);
 		    } 
 		});
   }
@@ -629,6 +643,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 								mProgressDialog.dismiss();
 								if(loopDetected >= 0)
 								{
+									mTotalLoopClosures+=loopDetected;
 									mToast.makeText(getActivity(), String.format("Detection done! %d new loop closure(s) added.", loopDetected), mToast.LENGTH_SHORT).show();
 								}
 								else if(loopDetected < 0)
@@ -896,6 +911,27 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  });
 		  builder.show();
       }
+      else if(itemId == R.id.optimize_error)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Max Optimization Error (m)");
+		  builder.setSingleChoiceItems(mOptimizeErrorValues, mParamOptimizeErrorIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < mOptimizeErrorValues.length)
+		        {
+		        	mParamOptimizeErrorIndex = which;
+			        if(RTABMapLib.setMappingParameter("RGBD/OptimizeMaxError", which==0?"0":mOptimizeErrorValues[which]) != 0)
+			        {
+			        	mToast.makeText(getActivity(), "Failed to set parameter \"RGBD/OptimizeMaxError\"!", mToast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
       else if (itemId == R.id.save)
       {
     	  if(mOpenedDatabasePath.isEmpty())