Merge branch 'jade-devel' of https://github.com/introlab/rtabmap into jade-devel

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="8"
+        android:versionCode="9"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -11,7 +11,7 @@
     <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
     <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
     <uses-feature android:glEsVersion="0x00020000" />
-    <uses-library android:name="com.projecttango.libtango_device" android:required="true" />
+    <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
 
     <!-- This is the platform API where NativeActivity was introduced. -->
     <uses-sdk android:minSdkVersion="17" />

app/android/jni/RTABMapApp.cpp

@@ -56,6 +56,10 @@ const int kVersionStringLength = 128;
 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
 
+namespace {
+constexpr int kTangoCoreMinimumVersion = 9377;
+}  // anonymous namespace.
+
 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 {
 	rtabmap::ParametersMap parameters;
@@ -101,7 +105,9 @@ RTABMapApp::RTABMapApp() :
 		clearSceneOnNextRender_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
-		lastDrawnCloudsCount_(0)
+		lastDrawnCloudsCount_(0),
+		renderingFPS_(0.0f)
+
 {
 
 }
@@ -122,19 +128,19 @@ RTABMapApp::~RTABMapApp() {
   }
 }
 
-int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
+void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 {
-
 	env->GetJavaVM(&jvm);
 	RTABMapActivity = env->NewGlobalRef(caller_activity);
 
-	LOGI("RTABMapApp::TangoInitialize()");
+	LOGI("RTABMapApp::onCreate()");
 	createdMeshes_.clear();
 	rawPoses_.clear();
 	clearSceneOnNextRender_ = true;
 	totalPoints_ = 0;
 	totalPolygons_ = 0;
 	lastDrawnCloudsCount_ = 0;
+	renderingFPS_ = 0.0f;
 
 	if(camera_)
 	{
@@ -158,12 +164,6 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
 	this->registerToEventsManager();
 
 	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_);
-
-
-  // The first thing we need to do for any Tango enabled application is to
-  // initialize the service. We'll do that here, passing on the JNI environment
-  // and jobject corresponding to the Android activity that is calling us.
-  return TangoService_initialize(env, caller_activity);
 }
 
 void RTABMapApp::openDatabase(const std::string & databasePath)
@@ -218,22 +218,27 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 	rtabmapMutex_.unlock();
 }
 
-int RTABMapApp::onResume()
+bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
 {
-	LOGW("onResume()");
+	LOGW("onTangoServiceConnected()");
 	if(camera_)
 	{
 		camera_->join(true);
 
+		if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
+		    LOGE("TangoHandler::ConnectTango, TangoService_setBinder error");
+		    return false;
+		}
+
 		if(camera_->init())
 		{
 			LOGI("Start camera thread");
 			camera_->start();
-			return TANGO_SUCCESS;
+			return true;
 		}
 		LOGE("Failed camera initialization!");
 	}
-	return TANGO_ERROR;
+	return false;
 }
 
 void RTABMapApp::onPause()
@@ -307,6 +312,7 @@ int RTABMapApp::Render()
 		totalPoints_ = 0;
 		totalPolygons_ = 0;
 		lastDrawnCloudsCount_ = 0;
+		renderingFPS_ = 0.0f;
 	}
 
 	// Process events
@@ -543,7 +549,9 @@ int RTABMapApp::Render()
 		}
 	}
 
+	UTimer fpsTime;
     lastDrawnCloudsCount_ = main_scene_.Render();
+    renderingFPS_ = 1.0/fpsTime.elapsed();
 
     if(rtabmapEvents.size())
     {
@@ -647,10 +655,7 @@ void RTABMapApp::setAutoExposure(bool enabled)
 		autoExposure_ = enabled;
 		if(camera_)
 		{
-			camera_->join(true);
-			camera_->close();
 			camera_->setAutoExposure(autoExposure_);
-			onResume();
 		}
 	}
 }
@@ -1104,7 +1109,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFII)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFI)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -1120,6 +1125,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 								featuresExtracted,
 								hypothesis,
 								lastDrawnCloudsCount_,
+								renderingFPS_,
 								rejected);
 						success = true;
 					}

app/android/jni/RTABMapApp.h

@@ -50,14 +50,11 @@ class RTABMapApp : public UEventsHandler {
   RTABMapApp();
   ~RTABMapApp();
 
-  // Initialize the Tango Service, this function starts the communication
+  void onCreate(JNIEnv* env, jobject caller_activity);
-  // between the application and the Tango Service.
-  // The activity object is used for checking if the API version is outdated.
-  int TangoInitialize(JNIEnv* env, jobject caller_activity);
 
   void openDatabase(const std::string & databasePath);
 
-  int onResume();
+  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
   // Explicitly reset motion tracking and restart the pipeline.
   // Note that this will cause motion tracking to re-initialize.
@@ -163,6 +160,7 @@ class RTABMapApp : public UEventsHandler {
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
+  float renderingFPS_;
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.

app/android/jni/jni_interface.cpp

@@ -48,11 +48,11 @@ void GetJStringContent(JNIEnv *AEnv, jstring AStr, std::string &ARes) {
   AEnv->ReleaseStringUTFChars(AStr,s);
 }
 
-JNIEXPORT jint JNICALL
+JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_initialize(
+Java_com_introlab_rtabmap_RTABMapLib_onCreate(
     JNIEnv* env, jobject, jobject activity)
 {
-	return app.TangoInitialize(env, activity);
+	return app.onCreate(env, activity);
 }
 
 JNIEXPORT void JNICALL
@@ -64,10 +64,10 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	return app.openDatabase(databasePathC);
 }
 
-JNIEXPORT jint JNICALL
+JNIEXPORT bool JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onResume(
+Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
-    JNIEnv*, jobject) {
+		JNIEnv* env, jobject, jobject iBinder) {
-  return app.onResume();
+  return app.onTangoServiceConnected(env, iBinder);
 }
 
 JNIEXPORT void JNICALL

app/android/res/layout/activity_rtabmap.xml

@@ -219,6 +219,21 @@
                 android:layout_width="wrap_content"
                 android:layout_height="wrap_content" />
         </LinearLayout>
+        <LinearLayout
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:orientation="horizontal" >
+
+            <TextView
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content"
+                android:text="@string/fps" />
+
+            <TextView
+                android:id="@+id/fps"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content" />
+        </LinearLayout>
            
     </LinearLayout>
 

app/android/res/values/strings.xml

@@ -20,5 +20,6 @@
     <string name="polygons">"Polygons: "</string>
     <string name="memory">"Memory (MB): "</string>
     <string name="hypothesis">"Hypothesis: "</string>
+    <string name="fps">"FPS (rendering): "</string>
     
 </resources>

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

@@ -9,8 +9,10 @@ import android.app.Notification;
 import android.app.NotificationManager;
 import android.app.PendingIntent;
 import android.app.ProgressDialog;
+import android.content.ComponentName;
 import android.content.DialogInterface;
 import android.content.Intent;
+import android.content.ServiceConnection;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
@@ -20,6 +22,7 @@ import android.os.Bundle;
 import android.os.Environment;
 import android.os.Handler;
 import android.os.Debug;
+import android.os.IBinder;
 import android.text.Editable;
 import android.text.InputType;
 import android.util.Log;
@@ -99,6 +102,24 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   
   private Toast mToast = null;
   
+  //Tango Service connection.
+  ServiceConnection mTangoServiceConnection = new ServiceConnection() {
+     public void onServiceConnected(ComponentName name, IBinder service) {
+       if(!RTABMapLib.onTangoServiceConnected(service))
+       {
+    	   mToast.makeText(getApplicationContext(), 
+    				String.format("Failed to intialize Tango!"), mToast.LENGTH_SHORT).show();
+       }
+     }
+
+     public void onServiceDisconnected(ComponentName name) {
+       // Handle this if you need to gracefully shutdown/retry
+       // in the event that Tango itself crashes/gets upgraded while running.
+       mToast.makeText(getApplicationContext(), 
+ 				String.format("Tango disconnected!"), mToast.LENGTH_SHORT).show();
+     }
+   };
+  
   @Override
   protected void onCreate(Bundle savedInstanceState) {
     super.onCreate(savedInstanceState);
@@ -172,7 +193,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						Environment.getExternalStorageState()), mToast.LENGTH_LONG).show();
     }
     
-    RTABMapLib.initialize(this);
+    RTABMapLib.onCreate(this);
     RTABMapLib.openDatabase(mTempDatabasePath);
   }
   
@@ -193,6 +214,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   protected void onResume() {
     super.onResume();
     
+    TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection);
+    
     Log.i(TAG, String.format("onResume()"));
     
     if (Tango.hasPermission(this, Tango.PERMISSIONTYPE_MOTION_TRACKING)) {
@@ -209,12 +232,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     		mItemPostProcessing.setEnabled(false);
         }
 
-        if(RTABMapLib.onResume()!=0)
-        {
-        	mToast.makeText(getApplicationContext(), 
-    				String.format("Failed to connect with Tango!"), mToast.LENGTH_SHORT).show();
-        }
-
     } else {
     	Log.i(TAG, String.format("Asking for motion tracking permission"));
         startActivityForResult(
@@ -232,6 +249,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     RTABMapLib.onPause();
     mOpenedDatabasePath = "";
     RTABMapLib.openDatabase(mTempDatabasePath);
+    
+    unbindService(mTangoServiceConnection);
   }
 
   @Override
@@ -318,6 +337,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  int featuresExtracted,
 		  float hypothesis,
 		  int nodesDrawn,
+		  float fps,
 		  int rejected)
   {
 	  if(mItemPause!=null)
@@ -335,6 +355,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  ((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>0?loopClosureId:highestHypId));
+	  ((TextView)findViewById(R.id.fps)).setText(String.format("%.3f Hz", fps));
 	  if(mItemPause!=null && !mItemPause.isChecked())
 	  {
 		  if(loopClosureId > 0)
@@ -367,13 +388,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final int features,
 		  final float hypothesis,
 		  final int nodesDrawn,
+		  final float fps,
 		  final int rejected)
   {
 	  Log.i(TAG, String.format("updateStatsCallback()"));
 
 	  runOnUiThread(new Runnable() {
 		    public void run() {
-		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, rejected);
+		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, fps, rejected);
 		    } 
 		});
   }
@@ -469,6 +491,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
 			((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
 			((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
+			((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 			mTotalLoopClosures = 0;
 			((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
 			
@@ -764,6 +787,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setAutoExposure(item.isChecked());
+    	  
+    	  // restart Tango service
+    	  onPause();
+    	  onResume();    	  
       }
       else if(itemId == R.id.resolution)
       {
@@ -1024,6 +1051,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  ((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
     	  ((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
     	  ((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
+    	  ((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 		  mTotalLoopClosures = 0;
 		  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
     	  

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

@@ -1,6 +1,8 @@
 
 package com.introlab.rtabmap;
+import android.os.IBinder;
 import android.view.KeyEvent;
+import android.util.Log;
 
 
 // Wrapper for native library
@@ -8,19 +10,29 @@ import android.view.KeyEvent;
 public class RTABMapLib
 {
 
-    static
+	static {
-    {
+        // This project depends on tango_client_api, so we need to make sure we load
-      System.loadLibrary("NativeRTABMap");
+        // the correct library first.
+        if (TangoInitializationHelper.loadTangoSharedLibrary() ==
+                TangoInitializationHelper.ARCH_ERROR) {
+            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+        }
+        System.loadLibrary("NativeRTABMap");
     }
 
  // Initialize the Tango Service, this function starts the communication
     // between the application and Tango Service.
     // The activity object is used for checking if the API version is outdated.
-    public static native int initialize(RTABMapActivity activity);
+    public static native void onCreate(RTABMapActivity activity);
     
     public static native void openDatabase(String databasePath);
     
-    public static native int onResume();
+    /*
+     * Called when the Tango service is connected.
+     *
+     * @param binder The native binder object.
+     */
+    public static native boolean onTangoServiceConnected(IBinder binder);
 
     // Release all non OpenGl resources that are allocated from the program.
     public static native void onPause();

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

@@ -0,0 +1,136 @@
+/*
+ * Copyright 2016 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ * 
+ * Copied for convenience from https://github.com/googlesamples/tango-examples-c/blob/master/cpp_example_util/app/src/main/java/com/projecttango/examples/cpp/util/TangoInitializationHelper.java
+ */
+
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.content.Intent;
+import android.content.ServiceConnection;
+import android.os.Build;
+import android.os.IBinder;
+import android.util.Log;
+
+import java.io.File;
+
+/**
+ * Functions for simplifying the process of initializing TangoService, and function
+ * handles loading correct libtango_client_api.so.
+ */
+public class TangoInitializationHelper {
+  public static final int ARCH_ERROR = -2;
+  public static final int ARCH_FALLBACK = -1;
+  public static final int ARCH_DEFAULT = 0;
+  public static final int ARCH_ARM64 = 1;
+  public static final int ARCH_ARM32 = 2;
+  public static final int ARCH_X86_64 = 3;
+  public static final int ARCH_X86 = 4;
+
+  /**
+   * Only for apps using the C API:
+   * Initializes the underlying TangoService for native apps.
+   *
+   * @return returns false if the device doesn't have the Tango running as Android Service.
+   *     Otherwise ture.
+   */
+  public static final boolean bindTangoService(final Context context,
+                                               ServiceConnection connection) {
+    Intent intent = new Intent();
+    intent.setClassName("com.google.tango", "com.google.atap.tango.TangoService");
+
+    boolean hasJavaService = (context.getPackageManager().resolveService(intent, 0) != null);
+
+    // User doesn't have the latest packagename for TangoCore, fallback to the previous name.
+    if (!hasJavaService) {
+      intent = new Intent();
+      intent.setClassName("com.projecttango.tango", "com.google.atap.tango.TangoService");
+      hasJavaService = (context.getPackageManager().resolveService(intent, 0) != null);
+    }
+
+    // User doesn't have a Java-fied TangoCore at all; fallback to the deprecated approach
+    // of doing nothing and letting the native side auto-init to the system-service version
+    // of Tango.
+    if (!hasJavaService) {
+      return false;
+    }
+
+    return context.bindService(intent, connection, Context.BIND_AUTO_CREATE);
+  }
+
+    
+  /**
+   * Load the libtango_client_api.so library based on different Tango device setup.
+   *
+   * @return returns the loaded architecture id.
+   */
+  public static final int loadTangoSharedLibrary() {
+    int loadedSoId = ARCH_ERROR;
+    String basePath = "/data/data/com.google.tango/libfiles/";
+    if (!(new File(basePath).exists())) {
+      basePath = "/data/data/com.projecttango.tango/libfiles/";
+    }
+    Log.i("TangoInitializationHelper", "basePath: " + basePath);
+
+    try {
+      System.load(basePath + "arm64-v8a/libtango_client_api.so");
+      loadedSoId = ARCH_ARM64;
+      Log.i("TangoInitializationHelper", "Success! Using arm64-v8a/libtango_client_api.");
+    } catch (UnsatisfiedLinkError e) {
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "armeabi-v7a/libtango_client_api.so");
+        loadedSoId = ARCH_ARM32;
+        Log.i("TangoInitializationHelper", "Success! Using armeabi-v7a/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "x86_64/libtango_client_api.so");
+        loadedSoId = ARCH_X86_64;
+        Log.i("TangoInitializationHelper", "Success! Using x86_64/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "x86/libtango_client_api.so");
+        loadedSoId = ARCH_X86;
+        Log.i("TangoInitializationHelper", "Success! Using x86/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "default/libtango_client_api.so");
+        loadedSoId = ARCH_DEFAULT;
+        Log.i("TangoInitializationHelper", "Success! Using default/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.loadLibrary("tango_client_api");
+        loadedSoId = ARCH_FALLBACK;
+        Log.i("TangoInitializationHelper", "Falling back to libtango_client_api.so symlink.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    return loadedSoId;
+  }
+}