Tango refactoring (#534)

* Created general CameraMobile interface. Tango is now optional. Camera is disabled on visualizaion (battery saving). * Working android app on non-tango android phones (tested on x86_64 android emulator). * fixed typo * android: updated tango not available msg * android: fixed build with latest android sdk/ndk * Added ARCore limited support for pose and rgb streams. * Added AREngine support. Don't assert if depth size is not a modulo of rgb size. * Added ARCore shared camera support * android: fixed read/write runtime permissions for >=api23. arcore ndk: added feature point cloud. Fixed file sharing persmissions (>=api24 issue) * ARCore NDK: mapping with feature point cloud * android: Added post build strip command to reduce native library size * android: fixed some compilation issues * android: put back gtsam as default optimizer, manifest min api is dynamic based on cmake parameters * AREngine: min api 24 * android: Fixed build without AREngine * android: fixed not available libraries for API<24 * android: fixed build with old cmake versions * android: set arcore min api to 23 * android: fixed arengine error on start when not built with native arengine support * android: fixed tango camera permission for api>=23 * Added bionic android docker files * Fixed wrong 3D words projection when depth size is not an exact multiple of rgb size. DbViewer: fixed images size not correctly shown in label of the calibration. ImageView: fixed depth scale when depth size is not a multiple of rgb size * CameraMobile: added exact display rotation for local transform * DbViewer: fixed gravity link shown in constraint view, show full local transform matrix in camera calibration label * Android: fixed localization mode in visualization, fixed some tansitions between some UI states * Android: Hide stop button when HUD is hidden. Updated About years.
2026-09-02 09:30:25 +08:00 · 2020-04-22 15:05:07 -04:00
parent 931d525874
commit 012439aa0b
62 changed files with 6260 additions and 2194 deletions
--- a/app/android/jni/.gitignore
+++ b/app/android/jni/.gitignore
@@ -0,0 +1 @@
+CameraAvailability.h
--- a/app/android/jni/CMakeLists.txt
+++ b/app/android/jni/CMakeLists.txt
@@ -1,6 +1,4 @@

-find_package(Tango REQUIRED)
-
 SET(INCLUDE_DIRS
    ${CMAKE_CURRENT_SOURCE_DIR}
    ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
@@ -9,23 +7,17 @@ SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/utilite/include
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
-    ${Tango_INCLUDE_DIRS}
    "${ANDROID_NDK}/platforms/android-${ANDROID_NATIVE_API_LEVEL}/arch-${ANDROID_ARCH_NAME}/usr/include"
 )

 SET(LIBRARIES
 	${OpenCV_LIBRARIES} 
 	${PCL_LIBRARIES}
-	${Tango_LIBRARIES}
 )

-add_definitions(${PCL_DEFINITIONS})
-
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
 set(sources
  jni_interface.cpp
-  CameraTango.cpp
+  CameraMobile.cpp
  RTABMapApp.cpp
  scene.cpp
  point_cloud_drawable.cpp
@@ -44,6 +36,82 @@ set(sources
  tango-gl/util.cpp
 )

+IF(OPENMP_FOUND)
+file(COPY ${OpenMP_CXX_LIBRARIES}
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+ENDIF(OPENMP_FOUND)
+
+IF(Tango_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraTango.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${Tango_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${Tango_LIBRARIES}
+  )
+  
+  file(COPY ${Tango_support_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}) 
+ENDIF(Tango_FOUND)
+
+IF(ARCore_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraARCore.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${ARCore_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${ARCore_LIBRARIES}
+  )
+ 
+  file(COPY ${ARCore_c_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${ARCore_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(ARCore_FOUND)
+
+IF(AREngine_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraAREngine.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${AREngine_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${AREngine_LIBRARIES}
+    camera2ndk
+    mediandk
+  )
+ 
+  file(COPY ${AREngine_impl_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_ndk_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(AREngine_FOUND)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
 add_library(NativeRTABMap SHARED ${sources})        
 target_link_libraries(NativeRTABMap ${LIBRARIES}
  android
@@ -58,3 +126,10 @@ set_target_properties(NativeRTABMap PROPERTIES
         LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
         LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
         LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}")
+         
+IF(ANDROID_NATIVE_API_LEVEL GREATER 22)
+add_custom_command(TARGET NativeRTABMap POST_BUILD
+    COMMAND "${ANDROID_TOOLCHAIN_PREFIX}strip" -g -S -d --strip-debug --verbose
+    "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}/libNativeRTABMap.so"
+    COMMENT "Strip debug symbols done on final binary.")
+ENDIF(ANDROID_NATIVE_API_LEVEL GREATER 22)
--- a/app/android/jni/CameraARCore.cpp
+++ b/app/android/jni/CameraARCore.cpp
@@ -0,0 +1,655 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CameraARCore.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+namespace rtabmap {
+
+#ifdef DEPTH_TEST
+// Camera Callbacks
+static void CameraDeviceOnDisconnected(void* context, ACameraDevice* device) {
+  LOGE("Camera(id: %s) is disconnected.\n", ACameraDevice_getId(device));
+}
+static void CameraDeviceOnError(void* context, ACameraDevice* device,
+                                int error) {
+  LOGE("Error(code: %d) on Camera(id: %s).\n", error,
+       ACameraDevice_getId(device));
+}
+// Capture Callbacks
+bool g_captureSessionReady = false;
+static void CaptureSessionOnReady(void* context,
+                                  ACameraCaptureSession* session) {
+  LOGI("Session is ready.\n");
+  g_captureSessionReady = true;
+}
+static void CaptureSessionOnActive(void* context,
+                                   ACameraCaptureSession* session) {
+  LOGI("Session is activated.\n");
+}
+#endif // DEPTH_TEST
+
+//////////////////////////////
+// CameraARCore
+//////////////////////////////
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smoothing):
+	CameraMobile(smoothing),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false)
+{
+	glGenTextures(1, &textureId_);
+}
+
+CameraARCore::~CameraARCore() {
+	// Disconnect ARCore service
+	close();
+
+	glDeleteTextures(1, &textureId_);
+}
+
+
+struct CameraConfig {
+  int32_t width = 0;
+  int32_t height = 0;
+  std::string config_label;
+  ArCameraConfig* config = nullptr;
+};
+
+void getCameraConfigLowestAndHighestResolutions(
+		std::vector<CameraConfig> & camera_configs,
+    CameraConfig** lowest_resolution_config,
+    CameraConfig** highest_resolution_config) {
+  if (camera_configs.empty()) {
+    return;
+  }
+
+  int low_resolution_config_idx = 0;
+  int high_resolution_config_idx = 0;
+  int32_t smallest_height = camera_configs[0].height;
+  int32_t largest_height = camera_configs[0].height;
+
+  for (int i = 1; i < camera_configs.size(); ++i) {
+    int32_t image_height = camera_configs[i].height;
+    if (image_height < smallest_height) {
+      smallest_height = image_height;
+      low_resolution_config_idx = i;
+    } else if (image_height > largest_height) {
+      largest_height = image_height;
+      high_resolution_config_idx = i;
+    }
+  }
+
+  if (low_resolution_config_idx == high_resolution_config_idx) {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+  } else {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+    *highest_resolution_config = &camera_configs[high_resolution_config_idx];
+  }
+}
+
+void copyCameraConfig(
+    const ArSession* ar_session, const ArCameraConfigList* all_configs,
+    int index, int num_configs, CameraConfig* camera_config) {
+  if (camera_config != nullptr && index >= 0 && index < num_configs) {
+    ArCameraConfig_create(ar_session, &camera_config->config);
+    ArCameraConfigList_getItem(ar_session, all_configs, index,
+                               camera_config->config);
+    ArCameraConfig_getImageDimensions(ar_session, camera_config->config,
+                                      &camera_config->width,
+                                      &camera_config->height);
+    camera_config->config_label = "(" + std::to_string(camera_config->width) +
+                                  "x" + std::to_string(camera_config->height) +
+                                  ")";
+  }
+}
+
+void destroyCameraConfigs(std::vector<CameraConfig> & camera_configs) {
+  for (int i = 0; i < camera_configs.size(); ++i) {
+    if (camera_configs[i].config != nullptr) {
+      ArCameraConfig_destroy(camera_configs[i].config);
+    }
+  }
+}
+
+std::string CameraARCore::getSerial() const
+{
+	return "ARCore";
+}
+
+#ifdef DEPTH_TEST
+void OnImageCallback(void *ctx, AImageReader *reader) {
+  reinterpret_cast<CameraARCore *>(ctx)->imageCallback(reader);
+}
+void CameraARCore::imageCallback(AImageReader *reader) {
+	int32_t format;
+	media_status_t status = AImageReader_getFormat(reader, &format);
+	UWARN("format=%d", format);
+	UASSERT_MSG(status == AMEDIA_OK, "Failed to get the media format");
+
+	if (format == AIMAGE_FORMAT_DEPTH16) {
+		// Create a thread and write out the jpeg files
+		AImage *image = nullptr;
+		media_status_t status = AImageReader_acquireNextImage(reader, &image);
+		UASSERT_MSG(status == AMEDIA_OK && image, "Image is not available");
+
+		int planeCount;
+		status = AImage_getNumberOfPlanes(image, &planeCount);
+		UASSERT_MSG(status == AMEDIA_OK && planeCount == 1,
+				uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+		uint8_t *data = nullptr;
+		int len = 0;
+		int stride;
+		int width;
+		int height;
+		AImage_getWidth(image, &width);
+		AImage_getHeight(image, &height);
+		AImage_getPlaneRowStride(image, 0, &stride);
+		AImage_getPlaneData(image, 0, &data, &len);
+
+		cv::Mat output(height, width, CV_16UC1);
+		uint16_t *dataShort = (uint16_t *)data;
+		uint16_t max=0x0;
+		for (int y = 0; y < output.rows; ++y)
+		{
+			for (int x = 0; x < output.cols; ++x)
+			{
+				uint16_t depthSample = dataShort[y*output.cols + x];
+				uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
+				output.at<uint16_t>(y,x) = depthRange;
+				if(depthRange > max)
+				{
+					max = depthRange;
+				}
+			}
+		}
+		UWARN("width=%d, height=%d, bytes=%d stride=%d max=%dmm",
+				width, height, len, stride, (int)max);
+
+		std::string path = "/storage/emulated/0/RTAB-Map/depth.png";
+		cv::imwrite(path, output);
+		UWARN("depth image saved to %s", path.c_str());
+
+		AImage_delete(image);
+	}
+}
+#endif // DEPTH_TEST
+
+bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+#ifdef DEPTH_TEST
+	///////////////////////////
+	// Depth image using camera2 API
+	/////////////////////////////
+	camera_status_t cameraStatus = ACAMERA_OK;
+
+	cameraManager_ = ACameraManager_create();
+
+	deviceStateCallbacks_.onDisconnected = CameraDeviceOnDisconnected;
+	deviceStateCallbacks_.onError = CameraDeviceOnError;
+
+	const char * cameraId = "0";
+	cameraStatus = ACameraManager_openCamera(cameraManager_, cameraId, &deviceStateCallbacks_, &cameraDevice_);
+	UASSERT_MSG(cameraStatus == ACAMERA_OK, uFormat("Failed to open camera device (id: %s)",
+			cameraId).c_str());
+
+	// Currently only working resolution on Huawei P30 Pro
+	cv::Size size(240, 180);
+	int format = AIMAGE_FORMAT_DEPTH16;
+
+	media_status_t mediaStatus = AImageReader_new(size.width, size.height, format, 2, &imageReader_);
+	UASSERT_MSG(imageReader_ && mediaStatus == AMEDIA_OK, uFormat("Failed to create AImageReader %dx%d format=%d",
+			size.width, size.height, format).c_str());
+
+	AImageReader_ImageListener listener{
+	  .context = this,
+	  .onImageAvailable = OnImageCallback,
+	};
+	AImageReader_setImageListener(imageReader_, &listener);
+
+	//
+	ANativeWindow *nativeWindow;
+	mediaStatus = AImageReader_getWindow(imageReader_, &nativeWindow);
+	UASSERT_MSG(mediaStatus == AMEDIA_OK, "Could not get ANativeWindow");
+
+	outputNativeWindow_ = nativeWindow;
+	ACaptureSessionOutputContainer_create(&captureSessionOutputContainer_);
+	ANativeWindow_acquire(outputNativeWindow_);
+	ACaptureSessionOutput_create(outputNativeWindow_, &sessionOutput_);
+	ACaptureSessionOutputContainer_add(captureSessionOutputContainer_, sessionOutput_);
+	ACameraOutputTarget_create(outputNativeWindow_, &cameraOutputTarget_);
+
+	cameraStatus = ACameraDevice_createCaptureRequest(cameraDevice_, TEMPLATE_RECORD, &captureRequest_);
+	UASSERT_MSG(cameraStatus == ACAMERA_OK,
+			uFormat("Failed to create preview capture request (id: %s, status=%d)",
+					cameraId, cameraStatus).c_str());
+
+	ACaptureRequest_addTarget(captureRequest_, cameraOutputTarget_);
+
+	captureSessionStateCallbacks_.onReady = CaptureSessionOnReady;
+	captureSessionStateCallbacks_.onActive = CaptureSessionOnActive;
+	ACameraDevice_createCaptureSession(
+			cameraDevice_,
+			captureSessionOutputContainer_, // outputs
+			&captureSessionStateCallbacks_, // callbacks
+			&captureSession_);
+
+	ACameraCaptureSession_setRepeatingRequest(captureSession_, nullptr, 1,
+			&captureRequest_, nullptr);
+
+	// Don't start ARCore as we cannot use both at the same time
+	return true;
+#endif // DEPTH_TEST
+
+	UScopeMutex lock(arSessionMutex_);
+
+	ArInstallStatus install_status;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	ArCoreApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case AR_INSTALL_STATUS_INSTALLED:
+			break;
+		case AR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
+	UASSERT(arSession_);
+
+	ArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
+	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
+
+	ArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	ArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_);
+	UASSERT(arCameraIntrinsics_);
+
+	ArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	ArCameraConfigList* all_camera_configs = nullptr;
+	int32_t num_configs = 0;
+	ArCameraConfigList_create(arSession_, &all_camera_configs);
+	// Create filter first to get both 30 and 60 fps.
+	ArCameraConfigFilter* camera_config_filter = nullptr;
+	ArCameraConfigFilter_create(arSession_, &camera_config_filter);
+	ArCameraConfigFilter_setTargetFps(arSession_, camera_config_filter, AR_CAMERA_CONFIG_TARGET_FPS_30 | AR_CAMERA_CONFIG_TARGET_FPS_60);
+	ArSession_getSupportedCameraConfigsWithFilter(arSession_, camera_config_filter, all_camera_configs);
+	ArCameraConfigList_getSize(arSession_, all_camera_configs, &num_configs);
+
+	if (num_configs < 1) {
+		UERROR("No camera config found");
+		close();
+		return false;
+	}
+
+	std::vector<CameraConfig> camera_configs;
+	CameraConfig* cpu_low_resolution_camera_config_ptr = nullptr;
+	CameraConfig* cpu_high_resolution_camera_config_ptr = nullptr;
+	camera_configs.resize(num_configs);
+	for (int i = 0; i < num_configs; ++i) {
+		copyCameraConfig(arSession_, all_camera_configs, i, num_configs,
+						 &camera_configs[i]);
+	}
+	// Determine the highest and lowest CPU resolutions.
+	cpu_low_resolution_camera_config_ptr = nullptr;
+	cpu_high_resolution_camera_config_ptr = nullptr;
+	getCameraConfigLowestAndHighestResolutions(
+			camera_configs,
+			&cpu_low_resolution_camera_config_ptr,
+			&cpu_high_resolution_camera_config_ptr);
+
+	// Cleanup the list obtained as it is safe to destroy the list as camera
+	// config instances were explicitly created and copied. Refer to the
+	// previous comment.
+	ArCameraConfigList_destroy(all_camera_configs);
+	ArSession_setCameraConfig(arSession_, cpu_low_resolution_camera_config_ptr->config);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	ArConfig_setUpdateMode(arSession_, arConfig_, AR_UPDATE_MODE_BLOCKING);
+
+	deviceTColorCamera_ = opticalRotation;
+
+	// Required as ArSession_update does some off-screen OpenGL stuff...
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
+	if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
+	{
+		UERROR("Cannot resume camera!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraARCore::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if(arSession_!= nullptr)
+	{
+		ArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		ArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		ArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		ArCameraIntrinsics_destroy(arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		ArPose_destroy(arPose_);
+	}
+	arPose_ = nullptr;
+
+#ifdef DEPTH_TEST
+
+	if(captureSession_!=nullptr)
+	{
+		g_captureSessionReady = false;
+		ACameraCaptureSession_stopRepeating(captureSession_);
+		double start = UTimer::now();
+		while(g_captureSessionReady != true && UTimer::now()-start < 2.0){
+			uSleep(100);
+			UWARN("Waiting session to close.... max 2 seconds");
+		}
+		//ACameraCaptureSession_close(captureSession_); // FIXME: this crashes?!
+		captureSession_ = nullptr;
+
+		ACaptureRequest_removeTarget(captureRequest_, cameraOutputTarget_);
+		ACaptureRequest_free(captureRequest_);
+		ACameraOutputTarget_free(cameraOutputTarget_);
+		captureRequest_ = nullptr;
+		cameraOutputTarget_ = nullptr;
+
+		ACaptureSessionOutputContainer_remove(captureSessionOutputContainer_, sessionOutput_);
+		ANativeWindow_release(outputNativeWindow_);
+		ACaptureSessionOutputContainer_free(captureSessionOutputContainer_);
+		ACaptureSessionOutput_free(sessionOutput_);
+		captureSessionOutputContainer_ = nullptr;
+		sessionOutput_ = nullptr;
+
+		ACameraDevice_close(cameraDevice_);
+		cameraDevice_ = nullptr;
+
+		ACameraManager_delete(cameraManager_);
+		cameraManager_ = nullptr;
+
+		AImageReader_delete(imageReader_);
+		imageReader_ = nullptr;
+	}
+#endif
+
+	CameraMobile::close();
+}
+
+SensorData CameraARCore::captureImage(CameraInfo * info)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return data;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArTrackingState camera_tracking_state;
+	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		ArCamera_getPose(arSession_, ar_camera, arPose_);
+		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+
+		// Get calibration parameters
+		float fx,fy, cx, cy;
+		int32_t width, height;
+		ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &width, &height);
+		UINFO("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
+
+		if(fx > 0 && fy > 0 && width > 0 && height > 0 && cx > 0 && cy > 0)
+		{
+			model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(width, height));
+
+			ArPointCloud * pointCloud = nullptr;
+			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
+
+			ArImage * image = nullptr;
+			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			if(status == AR_SUCCESS)
+			{
+				int64_t timestamp_ns;
+				ArImageFormat format;
+				ArImage_getTimestamp(arSession_, image, &timestamp_ns);
+				ArImage_getFormat(arSession_, image, &format);
+				if(format == AR_IMAGE_FORMAT_YUV_420_888)
+				{
+					int32_t num_planes;
+					int32_t pixel_stride;
+					const uint8_t * plane_data;
+					int32_t data_length;
+					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
+					for(int i=0;i<num_planes; ++i)
+					{
+						ArImage_getPlanePixelStride(arSession_, image, 0, &pixel_stride);
+						//LOGI("Plane %d/%d: w=%d h=%d stride=%d", i+1, num_planes, width, height, pixel_stride);
+					}
+
+					ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
+
+					if(plane_data != nullptr)
+					{
+						double stamp = double(timestamp_ns)/10e8;
+						//LOGI("data_length=%d stamp=%f", data_length, stamp);
+						cv::Mat rgb;
+						cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
+
+						LaserScan scan;
+						if(pointCloud)
+						{
+							int32_t points = 0;
+							ArPointCloud_getNumberOfPoints(arSession_, pointCloud, &points);
+							const float * pointCloudData = 0;
+							ArPointCloud_getData(arSession_, pointCloud, &pointCloudData);
+							LOGI("pointCloudData=%d size=%d", pointCloudData?1:0, points);
+							if(pointCloudData && points>0)
+							{
+								cv::Mat scanData(1, points, CV_32FC4);
+								float * ptr = scanData.ptr<float>();
+								for(unsigned int i=0;i<points; ++i)
+								{
+									cv::Point3f pt(pointCloudData[i*4], pointCloudData[i*4 + 1], pointCloudData[i*4 + 2]);
+									pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
+									ptr[i*4] = pt.x;
+									ptr[i*4 + 1] = pt.y;
+									ptr[i*4 + 2] = pt.z;
+
+									//get color from rgb image
+									cv::Point3f org= pt;
+									pt = util3d::transformPoint(pt, opticalRotationInv);
+									int u,v;
+									model.reproject(pt.x, pt.y, pt.z, u, v);
+									unsigned char r=255,g=255,b=255;
+									if(model.inFrame(u, v))
+									{
+										b=rgb.at<cv::Vec3b>(v,u).val[0];
+										g=rgb.at<cv::Vec3b>(v,u).val[1];
+										r=rgb.at<cv::Vec3b>(v,u).val[2];
+									}
+									*(int*)&ptr[i*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+
+									//confidence
+									//*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
+
+								}
+								scan = LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
+							}
+						}
+						else
+						{
+							LOGI("pointCloud empty");
+						}
+
+						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+					}
+				}
+				else
+				{
+					LOGE("CameraARCore: cannot convert image format %d", format);
+				}
+			}
+			else
+			{
+				LOGE("CameraARCore: failed to get rgb image (status=%d)", (int)status);
+			}
+
+			ArImage_release(image);
+			ArPointCloud_release(pointCloud);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+
+	if(pose.isNull())
+	{
+		LOGE("CameraARCore: Pose is null");
+	}
+	else
+	{
+		this->poseReceived(pose);
+		info->odomPose = pose;
+	}
+	return data;
+
+}
+
+void CameraARCore::capturePoseOnly()
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArTrackingState camera_tracking_state;
+	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		ArCamera_getPose(arSession_, ar_camera, arPose_);
+		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		if(!pose.isNull())
+		{
+			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+			this->poseReceived(pose);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+}
+
+} /* namespace rtabmap */
--- a/app/android/jni/CameraARCore.h
+++ b/app/android/jni/CameraARCore.h
@@ -0,0 +1,104 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAARCORE_H_
+#define CAMERAARCORE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+
+#include <arcore_c_api.h>
+#ifdef DEPTH_TEST
+#include <camera/NdkCameraDevice.h>
+#include <camera/NdkCameraManager.h>
+#include <media/NdkImageReader.h>
+#include <android/native_window.h>
+#endif
+
+namespace rtabmap {
+
+class CameraARCore : public CameraMobile {
+public:
+	CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
+	virtual ~CameraARCore();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+
+#ifdef DEPTH_TEST
+	void imageCallback(AImageReader *reader);
+#endif // DEPTH_TEST
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly();
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	ArSession* arSession_ = nullptr;
+	ArConfig* arConfig_ = nullptr;
+	ArFrame* arFrame_ = nullptr;
+	ArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	ArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	GLuint textureId_;
+	UMutex arSessionMutex_;
+
+#ifdef DEPTH_TEST
+	// Camera variables
+	ACameraDevice* cameraDevice_ = nullptr;
+	ACaptureRequest* captureRequest_ = nullptr;
+	ACameraOutputTarget* cameraOutputTarget_ = nullptr;
+	ACaptureSessionOutput* sessionOutput_ = nullptr;
+	ACaptureSessionOutputContainer* captureSessionOutputContainer_ = nullptr;
+	ACameraCaptureSession* captureSession_ = nullptr;
+	ANativeWindow *outputNativeWindow_ = nullptr;
+
+	ACameraDevice_StateCallbacks deviceStateCallbacks_;
+	ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
+
+	ACameraManager* cameraManager_ = nullptr;
+	AImageReader* imageReader_ = nullptr;
+#endif // DEPTH_TEST
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARCORE_H_ */
--- a/app/android/jni/CameraAREngine.cpp
+++ b/app/android/jni/CameraAREngine.cpp
@@ -0,0 +1,351 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CameraAREngine.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+#include <media/NdkImage.h>
+
+namespace rtabmap {
+
+
+//////////////////////////////
+// CameraAREngine
+//////////////////////////////
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing):
+	CameraMobile(smoothing),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false)
+{
+	glGenTextures(1, &textureId_);
+}
+
+CameraAREngine::~CameraAREngine() {
+	// Disconnect ARCore service
+	close();
+
+	glDeleteTextures(1, &textureId_);
+}
+
+std::string CameraAREngine::getSerial() const
+{
+	return "AREngine";
+}
+
+bool CameraAREngine::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	UScopeMutex lock(arSessionMutex_);
+
+	HwArInstallStatus install_status;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	HwArEnginesApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case HWAR_INSTALL_STATUS_INSTALLED:
+			break;
+		case HWAR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(HwArSession_create(env_, context_, &arSession_) == HWAR_SUCCESS);
+	UASSERT(arSession_);
+
+	HwArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	HwArConfig_setFocusMode(arSession_, arConfig_, HWAR_FOCUS_MODE_FIXED);
+	UASSERT(HwArSession_configure(arSession_, arConfig_) == HWAR_SUCCESS);
+
+	HwArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	HwArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_); // May fail?!
+	//UASSERT(arCameraIntrinsics_);
+
+	HwArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	HwArConfig_setUpdateMode(arSession_, arConfig_, HWAR_UPDATE_MODE_BLOCKING);
+
+	deviceTColorCamera_ = opticalRotation;
+
+	// Required as ArSession_update does some off-screen OpenGL stuff...
+	HwArSession_setCameraTextureName(arSession_, textureId_);
+
+	if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
+	{
+		UERROR("Cannot resume camera!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraAREngine::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		HwArCameraIntrinsics_destroy(arSession_, arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if(arSession_!= nullptr)
+	{
+		HwArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		HwArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		HwArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		HwArPose_destroy(arPose_);
+	}
+	arPose_ = nullptr;
+
+	CameraMobile::close();
+}
+
+SensorData CameraAREngine::captureImage(CameraInfo * info)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
+		LOGE("CameraAREngine::captureImage() ArSession_update error");
+		return data;
+	}
+
+	HwArCamera* ar_camera;
+	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	HwArTrackingState camera_tracking_state;
+	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		HwArCamera_getPose(arSession_, ar_camera, arPose_);
+		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+
+		// Get calibration parameters
+		// FIXME: Hard-coded as getting intrinsics with the api fails
+		float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
+		int32_t camWidth=640, camHeight=480;
+		//HwArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		//HwArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		//HwArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		//HwArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &camWidth, &camHeight);
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, camWidth, camHeight);
+
+		if(fx > 0 && fy > 0 && camWidth > 0 && camHeight > 0 && cx > 0 && cy > 0)
+		{
+			//ArPointCloud * point_cloud;
+			//ArFrame_acquirePointCloud(ar_session_, ar_frame_, &point_cloud);
+
+			HwArImage * image = nullptr;
+			HwArImage * depthImage = nullptr;
+			HwArStatus statusRgb = HwArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			HwArStatus statusDepth = HwArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+			if(statusRgb == HWAR_SUCCESS && statusDepth == HWAR_SUCCESS)
+			{
+				int64_t timestamp_ns;
+				HwArFrame_getTimestamp(arSession_, arFrame_, &timestamp_ns);
+
+				int planeCount;
+				uint8_t *imageData = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				const AImage* ndkImageRGB;
+				HwArImage_getNdkImage(image, &ndkImageRGB);
+
+				AImage_getNumberOfPlanes(ndkImageRGB, &planeCount);
+				AImage_getWidth(ndkImageRGB, &width);
+				AImage_getHeight(ndkImageRGB, &height);
+				AImage_getPlaneRowStride(ndkImageRGB, 0, &stride);
+				AImage_getPlaneData(ndkImageRGB, 0, &imageData, &len);
+				LOGI("RGB: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputRGB;
+				if(imageData != nullptr && len>0)
+				{
+					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, CV_YUV2BGR_NV21);
+				}
+
+				//Depth
+				const AImage* ndkImageDepth;
+				HwArImage_getNdkImage(depthImage, &ndkImageDepth);
+				AImage_getNumberOfPlanes(ndkImageDepth, &planeCount);
+				AImage_getWidth(ndkImageDepth, &width);
+				AImage_getHeight(ndkImageDepth, &height);
+				AImage_getPlaneRowStride(ndkImageDepth, 0, &stride);
+				AImage_getPlaneData(ndkImageDepth, 0, &imageData, &len);
+				LOGI("Depth: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputDepth(height, width, CV_16UC1);
+				uint16_t *dataShort = (uint16_t *)imageData;
+				for (int y = 0; y < outputDepth.rows; ++y)
+				{
+					for (int x = 0; x < outputDepth.cols; ++x)
+					{
+						uint16_t depthSample = dataShort[y*outputDepth.cols + x];
+						uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
+						outputDepth.at<uint16_t>(y,x) = depthRange;
+					}
+				}
+
+				if(!outputRGB.empty() && !outputDepth.empty())
+				{
+					double stamp = double(timestamp_ns)/10e8;
+					CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
+					data = SensorData(outputRGB, outputDepth, model, 0, stamp);
+				}
+			}
+			else
+			{
+				LOGE("CameraAREngine: failed to get rgb image (status=%d %d)", (int)statusRgb, (int)statusDepth);
+			}
+
+			HwArImage_release(image);
+			HwArImage_release(depthImage);
+		}
+		else
+		{
+			LOGE("Invalid intrinsics!");
+		}
+	}
+
+	HwArCamera_release(ar_camera);
+
+	if(pose.isNull())
+	{
+		LOGE("CameraAREngine: Pose is null");
+	}
+	else
+	{
+		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+		this->poseReceived(pose);
+		info->odomPose = pose;
+	}
+	return data;
+
+}
+
+void CameraAREngine::capturePoseOnly()
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return;
+	}
+
+	HwArCamera* ar_camera;
+	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	HwArTrackingState camera_tracking_state;
+	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		HwArCamera_getPose(arSession_, ar_camera, arPose_);
+		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		if(!pose.isNull())
+		{
+			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+			this->poseReceived(pose);
+		}
+	}
+
+	HwArCamera_release(ar_camera);
+}
+
+} /* namespace rtabmap */
--- a/app/android/jni/CameraAREngine.h
+++ b/app/android/jni/CameraAREngine.h
@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAARENGINE_H_
+#define CAMERAARENGINE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+
+#include <huawei_arengine_interface.h>
+
+namespace rtabmap {
+
+class CameraAREngine : public CameraMobile {
+public:
+	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
+	virtual ~CameraAREngine();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly();
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	HwArSession* arSession_ = nullptr;
+	HwArConfig* arConfig_ = nullptr;
+	HwArFrame* arFrame_ = nullptr;
+	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	HwArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	GLuint textureId_;
+	UMutex arSessionMutex_;
+
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARENGINE_H_ */
--- a/app/android/jni/CameraMobile.cpp
+++ b/app/android/jni/CameraMobile.cpp
@@ -0,0 +1,314 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CameraMobile.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+namespace rtabmap {
+
+#define nullptr 0
+
+//////////////////////////////
+// CameraMobile
+//////////////////////////////
+const float CameraMobile::bilateralFilteringSigmaS = 2.0f;
+const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
+
+const rtabmap::Transform CameraMobile::opticalRotation = Transform(
+		0.0f,  0.0f,  1.0f, 0.0f,
+	   -1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f, -1.0f,  0.0f, 0.0f);
+const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
+		0.0f, -1.0f,  0.0f, 0.0f,
+	    0.0f,  0.0f, -1.0f, 0.0f,
+		1.0f,  0.0f,  0.0f, 0.0f);
+
+CameraMobile::CameraMobile(bool smoothing) :
+		Camera(10),
+		deviceTColorCamera_(Transform::getIdentity()),
+		spinOncePreviousStamp_(0.0),
+		previousStamp_(0.0),
+		stampEpochOffset_(0.0),
+		smoothing_(smoothing),
+		colorCameraToDisplayRotation_(ROTATION_0),
+		originUpdate_(false)
+{
+}
+
+CameraMobile::~CameraMobile() {
+	// Disconnect camera service
+	close();
+}
+
+bool CameraMobile::init(const std::string &, const std::string &)
+{
+	deviceTColorCamera_ = opticalRotation;
+	return true;
+}
+
+void CameraMobile::close()
+{
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	originOffset_ = Transform();
+	originUpdate_ = false;
+	pose_ = Transform();
+	data_ = SensorData();
+}
+
+void CameraMobile::resetOrigin()
+{
+	originUpdate_ = true;
+}
+
+void CameraMobile::poseReceived(const Transform & pose)
+{
+	if(!pose.isNull())
+	{
+		// send pose of the camera (without optical rotation)
+		Transform p = pose*deviceTColorCamera_;
+		if(originUpdate_)
+		{
+			originOffset_ = p.translation().inverse();
+			originUpdate_ = false;
+		}
+
+		if(!originOffset_.isNull())
+		{
+			this->post(new PoseEvent(originOffset_*p));
+		}
+		else
+		{
+			this->post(new PoseEvent(p));
+		}
+	}
+}
+
+bool CameraMobile::isCalibrated() const
+{
+	return model_.isValidForProjection();
+}
+
+void CameraMobile::setGPS(const GPS & gps)
+{
+	lastKnownGPS_ = gps;
+}
+
+void CameraMobile::setData(const SensorData & data, const Transform & pose)
+{
+	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	data_ = data;
+	pose_ = pose;
+}
+
+void CameraMobile::addEnvSensor(int type, float value)
+{
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
+void CameraMobile::spinOnce()
+{
+	if(!this->isRunning())
+	{
+		bool ignoreFrame = false;
+		float rate = 10.0f; // maximum 10 FPS for image data
+		double now = UTimer::now();
+		if(rate>0.0f)
+		{
+			if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
+				((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
+			{
+				ignoreFrame = true;
+			}
+		}
+
+		if(!ignoreFrame)
+		{
+			spinOnceFrameRateTimer_.start();
+			spinOncePreviousStamp_ = now;
+			mainLoop();
+		}
+		else
+		{
+			// just send pose
+			capturePoseOnly();
+		}
+	}
+}
+
+void CameraMobile::mainLoopBegin()
+{
+	double t = cameraStartedTime_.elapsed();
+	if(t < 5.0)
+	{
+		uSleep((5.0-t)*1000); // just to make sure that the camera is started
+	}
+}
+
+void CameraMobile::mainLoop()
+{
+	CameraInfo info;
+	SensorData data = this->captureImage(&info);
+
+	if(data.isValid() && !info.odomPose.isNull())
+	{
+		if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
+		{
+			data.setGPS(lastKnownGPS_);
+		}
+		else if(lastKnownGPS_.stamp()>0.0)
+		{
+			LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
+		}
+
+		if(lastEnvSensors_.size())
+		{
+			data.setEnvSensors(lastEnvSensors_);
+			lastEnvSensors_.clear();
+		}
+
+		if(smoothing_ && !data.depthRaw().empty())
+		{
+			//UTimer t;
+			data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
+			//LOGD("Bilateral filtering, time=%fs", t.ticks());
+		}
+
+		// Rotate image depending on the camera orientation
+		if(colorCameraToDisplayRotation_ == ROTATION_90)
+		{
+			cv::Mat rgb, depth;
+			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
+			cv::flip(data.imageRaw(),rgb,1);
+			cv::transpose(rgb,rgbt);
+			rgb = rgbt;
+			cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
+			cv::flip(data.depthRaw(),depth,1);
+			cv::transpose(depth,deptht);
+			depth = deptht;
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_180)
+		{
+			cv::Mat rgb, depth;
+			cv::flip(data.imageRaw(),rgb,1);
+			cv::flip(rgb,rgb,0);
+			cv::flip(data.depthOrRightRaw(),depth,1);
+			cv::flip(depth,depth,0);
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageWidth(), model.imageHeight());
+			model = CameraModel(
+					model.fx(),
+					model.fy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_270)
+		{
+			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
+			cv::transpose(data.imageRaw(),rgb);
+			cv::flip(rgb,rgb,1);
+			cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
+			cv::transpose(data.depthOrRightRaw(),depth);
+			cv::flip(depth,depth,1);
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.cx(),
+					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+		}
+
+		rtabmap::Transform pose = info.odomPose;
+		data.setGroundTruth(Transform());
+
+		// convert stamp to epoch
+		bool firstFrame = previousPose_.isNull();
+		if(firstFrame)
+		{
+			stampEpochOffset_ = UTimer::now()-data.stamp();
+		}
+		data.setStamp(stampEpochOffset_ + data.stamp());
+		OdometryInfo info;
+		if(!firstFrame)
+		{
+			info.interval = data.stamp()-previousStamp_;
+			info.transform = previousPose_.inverse() * pose;
+		}
+		// linear cov = 0.0001
+		info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
+		if(!firstFrame)
+		{
+			// angular cov = 0.000001
+			info.reg.covariance.at<double>(3,3) *= 0.01;
+			info.reg.covariance.at<double>(4,4) *= 0.01;
+			info.reg.covariance.at<double>(5,5) *= 0.01;
+		}
+		LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
+		this->post(new OdometryEvent(data, pose, info));
+		previousPose_ = pose;
+		previousStamp_ = data.stamp();
+	}
+	else if(!this->isKilled())
+	{
+		LOGW("Odometry lost");
+		this->post(new OdometryEvent());
+	}
+}
+
+SensorData CameraMobile::captureImage(CameraInfo * info)
+{
+	if(info)
+	{
+		info->odomPose = pose_;
+	}
+	return data_;
+}
+
+} /* namespace rtabmap */
--- a/app/android/jni/CameraMobile.h
+++ b/app/android/jni/CameraMobile.h
@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAMOBILE_H_
+#define CAMERAMOBILE_H_
+
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include "util.h"
+
+namespace rtabmap {
+
+class CameraInfoEvent: public UEvent
+{
+public:
+	CameraInfoEvent(int type, const std::string & key, const std::string & value) : type_(type), key_(key), value_(value) {}
+	virtual std::string getClassName() const {return "CameraInfoEvent";}
+	int type() const {return type_;}
+	const std::string & key() const {return key_;}
+	const std::string & value() const {return value_;}
+
+private:
+	int type_;
+	std::string key_;
+	std::string value_;
+
+};
+
+class PoseEvent: public UEvent
+{
+public:
+	PoseEvent(const Transform & pose) : pose_(pose) {}
+	virtual std::string getClassName() const {return "PoseEvent";}
+	const Transform & pose() const {return pose_;}
+
+private:
+	Transform pose_;
+};
+
+class CameraMobile : public Camera, public UThread, public UEventsSender {
+public:
+	static const float bilateralFilteringSigmaS;
+	static const float bilateralFilteringSigmaR;
+
+	static const rtabmap::Transform opticalRotation;
+	static const rtabmap::Transform opticalRotationInv;
+
+public:
+	CameraMobile(bool smoothing = false);
+	virtual ~CameraMobile();
+
+	// abstract functions
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // inherited classes should call its parent in their close().
+	virtual std::string getSerial() const {return "CameraMobile";}
+
+	const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
+	void resetOrigin();
+	virtual bool isCalibrated() const;
+
+	void poseReceived(const Transform & pose); // in rtabmap frame
+
+	const CameraModel & getCameraModel() const {return model_;}
+	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
+	void setSmoothing(bool enabled) {smoothing_ = enabled;}
+	void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setGPS(const GPS & gps);
+	void addEnvSensor(int type, float value);
+	void setData(const SensorData & data, const Transform & pose);
+
+	void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly() {}
+
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+protected:
+	CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
+	Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
+	UTimer spinOnceFrameRateTimer_;
+	double spinOncePreviousStamp_;
+
+private:
+	Transform previousPose_;
+	double previousStamp_;
+	UTimer cameraStartedTime_;
+	double stampEpochOffset_;
+	bool smoothing_;
+	ScreenRotation colorCameraToDisplayRotation_;
+	GPS lastKnownGPS_;
+	EnvSensors lastEnvSensors_;
+	Transform originOffset_;
+	bool originUpdate_;
+
+	SensorData data_;
+	Transform pose_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERATANGO_H_ */
--- a/app/android/jni/CameraTango.cpp
+++ b/app/android/jni/CameraTango.cpp
@@ -91,7 +91,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 	if(pose->status_code == TANGO_POSE_VALID)
 	{
 		CameraTango* app = static_cast<CameraTango*>(context);
-		app->poseReceived(app->tangoPoseToTransform(pose));
+		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
 	}
 }

@@ -104,23 +104,15 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-const float CameraTango::bilateralFilteringSigmaS = 2.0f;
-const float CameraTango::bilateralFilteringSigmaR = 0.075f;
-
 CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
-		Camera(0),
+		CameraMobile(smoothing),
 		tango_config_(0),
-		previousStamp_(0.0),
-		stampEpochOffset_(0.0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),
 		rawScanPublished_(publishRawScan),
-		smoothing_(smoothing),
 		cloudStamp_(0),
 		tangoColorType_(0),
-		tangoColorStamp_(0),
-		colorCameraToDisplayRotation_(ROTATION_0),
-		originUpdate_(false)
+		tangoColorStamp_(0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -350,6 +342,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			pose_data.orientation[1],
 			pose_data.orientation[2],
 			pose_data.orientation[3]);
+	deviceTColorCamera_ = rtabmap_world_T_opengl_world * deviceTColorCamera_;

 	// camera intrinsic
 	TangoCameraIntrinsics color_camera_intrinsics;
@@ -408,18 +401,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	model_ = CameraModel(colorCamera_?"color":"fisheye",
 			cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
 			K, D, R, P,
-			tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_); // device to camera optical rotation in rtabmap frame
+			deviceTColorCamera_);

 	if(!colorCamera_)
 	{
 		initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
 	}

-	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
-	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
-
-	cameraStartedTime_.restart();
-
+	LOGI("deviceTColorCameraRtabmap  =%s", deviceTColorCamera_.prettyPrint().c_str());
 	return true;
 }

@@ -433,19 +422,10 @@ void CameraTango::close()
 		TangoService_disconnect();
 		LOGI("TangoService_disconnect() done.");
 	}
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
-	lastKnownGPS_ = GPS();
-	lastEnvSensors_.clear();
-	originOffset_ = Transform();
-	originUpdate_ = false;
-}

-void CameraTango::resetOrigin()
-{
-	originUpdate_ = true;
+	CameraMobile::close();
 }

 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -500,40 +480,9 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	}
 }

-static rtabmap::Transform opticalRotation(
-								1.0f,  0.0f,  0.0f, 0.0f,
-							    0.0f, -1.0f,  0.0f, 0.0f,
-								0.0f,  0.0f, -1.0f, 0.0f);
-void CameraTango::poseReceived(const Transform & pose)
-{
-	if(!pose.isNull())
-	{
-		// send pose of the camera (without optical rotation), not the device
-		Transform p = pose*deviceTColorCamera_*opticalRotation;
-		if(originUpdate_)
-		{
-			originOffset_ = p.translation().inverse();
-			originUpdate_ = false;
-		}
-		if(!originOffset_.isNull())
-		{
-			this->post(new PoseEvent(originOffset_*p));
-		}
-		else
-		{
-			this->post(new PoseEvent(p));
-		}
-	}
-}
-
 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
 {
-	this->post(new CameraTangoEvent(type, key, value));
-}
-
-bool CameraTango::isCalibrated() const
-{
-	return model_.isValidForProjection();
+	this->post(new CameraInfoEvent(type, key, value));
 }

 std::string CameraTango::getSerial() const
@@ -541,16 +490,6 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }

-void CameraTango::setGPS(const GPS & gps)
-{
-	lastKnownGPS_ = gps;
-}
-
-void CameraTango::addEnvSensor(int type, float value)
-{
-	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
-}
-
 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
@@ -594,7 +533,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	else
 	{

-		pose = tangoPoseToTransform(&pose_start_service_T_device);
+		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
 	}

 	return pose;
@@ -610,7 +549,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		if(this->isRunning())
 		{
 			LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
-			this->post(new CameraTangoEvent(0, "CameraTango", "No frames received since 2 seconds."));
+			this->post(new CameraInfoEvent(0, "CameraTango", "No frames received since 2 seconds."));

 			boost::mutex::scoped_lock  lock(dataMutex_);
 			if(!cloud_.empty() && !tangoColor_.empty())
@@ -804,7 +743,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)

 			if(closePoints > 100)
 			{
-				this->post(new CameraTangoEvent(0, "TooClose", ""));
+				this->post(new CameraInfoEvent(0, "TooClose", ""));
 			}

 			if(oi)
@@ -822,78 +761,23 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);

-			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
-
-			// adjust origin
-			if(!originOffset_.isNull())
-			{
-				poseDevice = originOffset_ * poseDevice;
-			}
+			Transform odom = getPoseAtTimestamp(rgbStamp);

 			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
 			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
 			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());

-			//Rotate in RTAB-Map's coordinate
-			Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
+			// adjust origin
+			if(!getOriginOffset().isNull())
+			{
+				odom = getOriginOffset() * odom;
+			}

 			//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
 			//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());

 			Transform scanLocalTransform = model.localTransform();

-			// Rotate image depending on the camera orientation
-			if(colorCameraToDisplayRotation_ == ROTATION_90)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::flip(rgb,rgb,1);
-				cv::transpose(rgb,rgbt);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::flip(depth,depth,1);
-				cv::transpose(depth,deptht);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy(), model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_180)
-			{
-				cv::flip(rgb,rgb,1);
-				cv::flip(rgb,rgb,0);
-				cv::flip(depth,depth,1);
-				cv::flip(depth,depth,0);
-				cv::Size sizet(model.imageWidth(), model.imageHeight());
-				model = CameraModel(
-						model.fx(),
-						model.fy(),
-						model.cx()>0?model.imageWidth()-model.cx():0,
-						model.cy()>0?model.imageHeight()-model.cy():0,
-						model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144*2.0));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_270)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::transpose(rgb,rgbt);
-				cv::flip(rgbt,rgbt,1);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::transpose(depth,deptht);
-				cv::flip(deptht,deptht,1);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy()>0?model.imageHeight()-model.cy():0, model.cx(), model.localTransform()*rtabmap::Transform(0,0,0,0,0,-1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-
-			if(smoothing_)
-			{
-				//UTimer t;
-				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
-				//LOGD("Bilateral filtering, time=%fs", t.ticks());
-			}
-
 			if(rawScanPublished_)
 			{
 				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
@@ -902,22 +786,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			{
 				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
-			data.setGroundTruth(odom);
-
-			if(lastKnownGPS_.stamp() > 0.0 && rgbStamp-lastKnownGPS_.stamp()<1.0)
-			{
-				data.setGPS(lastKnownGPS_);
-			}
-			else if(lastKnownGPS_.stamp()>0.0)
-			{
-				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
-			}
-
-			if(lastEnvSensors_.size())
-			{
-				data.setEnvSensors(lastEnvSensors_);
-				lastEnvSensors_.clear();
-			}
+			info->odomPose = odom;
 		}
 		else
 		{
@@ -928,64 +797,4 @@ SensorData CameraTango::captureImage(CameraInfo * info)

 }

-void CameraTango::mainLoopBegin()
-{
-	double t = cameraStartedTime_.elapsed();
-	if(t < 5.0)
-	{
-		uSleep((5.0-t)*1000); // just to make sure that the camera is started
-	}
-}
-
-void CameraTango::mainLoop()
-{
-	if(tango_config_)
-	{
-		SensorData data = this->captureImage();
-
-		if(!data.groundTruth().isNull())
-		{
-			rtabmap::Transform pose = data.groundTruth();
-			data.setGroundTruth(Transform());
-
-			// convert stamp to epoch
-			bool firstFrame = previousPose_.isNull();
-			if(firstFrame)
-			{
-				stampEpochOffset_ = UTimer::now()-data.stamp();
-			}
-			data.setStamp(stampEpochOffset_ + data.stamp());
-			OdometryInfo info;
-			if(!firstFrame)
-			{
-				info.interval = data.stamp()-previousStamp_;
-				info.transform = previousPose_.inverse() * pose;
-			}
-			// linear cov = 0.0001
-			info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
-			if(!firstFrame)
-			{
-				// angular cov = 0.000001
-				info.reg.covariance.at<double>(3,3) *= 0.01;
-				info.reg.covariance.at<double>(4,4) *= 0.01;
-				info.reg.covariance.at<double>(5,5) *= 0.01;
-			}
-			LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
-			this->post(new OdometryEvent(data, pose, info));
-			previousPose_ = pose;
-			previousStamp_ = data.stamp();
-		}
-		else if(!this->isKilled())
-		{
-			LOGW("Odometry lost");
-			this->post(new OdometryEvent());
-		}
-	}
-	else
-	{
-		UERROR("Camera not initialized, cannot start thread.");
-		this->kill();
-	}
-}
-
 } /* namespace rtabmap */
--- a/app/android/jni/CameraTango.h
+++ b/app/android/jni/CameraTango.h
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CAMERATANGO_H_
 #define CAMERATANGO_H_

+#include "CameraMobile.h"
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
@@ -37,66 +38,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <tango_client_api.h>
 #include <tango_support_api.h>

-class TangoPoseData;
-
 namespace rtabmap {

-class PoseEvent: public UEvent
-{
-public:
-	PoseEvent(const Transform & pose) : pose_(pose) {}
-	virtual std::string getClassName() const {return "PoseEvent";}
-	const Transform & pose() const {return pose_;}
-
-private:
-	Transform pose_;
-};
-
-class CameraTangoEvent: public UEvent
-{
-public:
-	CameraTangoEvent(int type, const std::string & key, const std::string & value) : type_(type), key_(key), value_(value) {}
-	virtual std::string getClassName() const {return "CameraTangoEvent";}
-	int type() const {return type_;}
-	const std::string & key() const {return key_;}
-	const std::string & value() const {return value_;}
-
-private:
-	int type_;
-	std::string key_;
-	std::string value_;
-
-};
-
-class CameraTango : public Camera, public UThread, public UEventsSender {
-public:
-	static const float bilateralFilteringSigmaS;
-	static const float bilateralFilteringSigmaR;
-
+class CameraTango : public CameraMobile {
 public:
 	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	void close(); // close Tango connection
-	void resetOrigin();
-	virtual bool isCalibrated() const;
+	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
-	const CameraModel & getCameraModel() const {return model_;}
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
 	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
-	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
-	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
-	void setGPS(const GPS & gps);
-	void addEnvSensor(int type, float value);

 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
-	void poseReceived(const Transform & pose);
 	void tangoEventReceived(int type, const char * key, const char * value);

 protected:
@@ -105,19 +66,11 @@ protected:
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);

-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-
 private:
 	void * tango_config_;
-	Transform previousPose_;
-	double previousStamp_;
-	UTimer cameraStartedTime_;
-	double stampEpochOffset_;
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	bool smoothing_;
 	cv::Mat cloud_;
 	double cloudStamp_;
 	cv::Mat tangoColor_;
@@ -125,15 +78,8 @@ private:
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	CameraModel model_;
-	Transform deviceTColorCamera_;
-	TangoSupportRotation colorCameraToDisplayRotation_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
-	GPS lastKnownGPS_;
-	EnvSensors lastEnvSensors_;
-	Transform originOffset_;
-	bool originUpdate_;
 };

 } /* namespace rtabmap */
--- a/app/android/jni/RTABMapApp.cpp
+++ b/app/android/jni/RTABMapApp.cpp
--- a/app/android/jni/RTABMapApp.h
+++ b/app/android/jni/RTABMapApp.h
@@ -31,11 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <jni.h>
 #include <memory>

-#include <tango_client_api.h>  // NOLINT
 #include <tango-gl/util.h>

 #include "scene.h"
-#include "CameraTango.h"
+#include "CameraMobile.h"
 #include "util.h"
 #include "ProgressionStatus.h"

@@ -49,39 +48,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class RTABMapApp : public UEventsHandler {
 public:
  // Constructor and deconstructor.
-  RTABMapApp();
+  RTABMapApp(JNIEnv* env, jobject caller_activity);
  ~RTABMapApp();

-  void onCreate(JNIEnv* env, jobject caller_activity);
-
  void setScreenRotation(int displayRotation, int cameraRotation);

  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());

-  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
-
-  // Explicitly reset motion tracking and restart the pipeline.
-  // Note that this will cause motion tracking to re-initialize.
-  void TangoResetMotionTracking();
-
-  // Tango Service point cloud callback function for depth data. Called when new
-  // new point cloud data is available from the Tango Service.
-  //
-  // @param pose: The current point cloud returned by the service,
-  //              caller allocated.
-  void onPointCloudAvailable(const TangoXYZij* xyz_ij);
-
-  // Tango service pose callback function for pose data. Called when new
-  // information about device pose is available from the Tango Service.
-  //
-  // @param pose: The current pose returned by the service, caller allocated.
-  void onPoseAvailable(const TangoPoseData* pose);
-
-  // Tango service event callback function for event data. Called when new events
-  // are available from the Tango Service.
-  //
-  // @param event: Tango event, caller allocated.
-  void onTangoEventAvailable(const TangoEvent* event);
+  bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);

  // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
  void InitializeGLContent();
@@ -92,8 +66,7 @@ class RTABMapApp : public UEventsHandler {
  // Main render loop.
  int Render();

-  // Release all non-OpenGL allocated resources.
-  void onPause();
+  void stopCamera();

  // Set render camera's viewing angle, first person, third person or top down.
  //
@@ -150,7 +123,6 @@ class RTABMapApp : public UEventsHandler {
  void setGPS(const rtabmap::GPS & gps);
  void addEnvSensor(int type, float value);

-  void resetMapping();
  void save(const std::string & databasePath);
  void cancelProcessing();
  bool exportMesh(
@@ -174,21 +146,32 @@ class RTABMapApp : public UEventsHandler {
  bool writeExportedMesh(const std::string & directory, const std::string & name);
  int postProcessing(int approach);

+  void postCameraPoseEvent(
+  		float x, float y, float z, float qx, float qy, float qz, float qw);
+
+  void postOdometryEvent(
+  		float x, float y, float z, float qx, float qy, float qz, float qw,
+  		float fx, float fy, float cx, float cy,
+  		double stamp,
+  		void * rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
+  		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat);
+
 protected:
  virtual bool handleEvent(UEvent * event);

 private:
  rtabmap::ParametersMap getRtabmapParameters();
-  bool smoothMesh(int id, Mesh & mesh);
+  bool smoothMesh(int id, rtabmap::Mesh & mesh);
  void gainCompensation(bool full = false);
  std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
  std::vector<pcl::Vertices> filterPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;

 private:
-  rtabmap::CameraTango * camera_;
+  int cameraDriver_;
+  rtabmap::CameraMobile * camera_;
  rtabmap::RtabmapThread * rtabmapThread_;
  rtabmap::Rtabmap * rtabmap_;
-  LogHandler * logHandler_;
+  rtabmap::LogHandler * logHandler_;

  bool odomCloudShown_;
  bool graphOptimization_;
@@ -212,7 +195,6 @@ class RTABMapApp : public UEventsHandler {

  rtabmap::ParametersMap mappingParameters_;

-  bool paused_;
  bool dataRecorderMode_;
  bool clearSceneOnNextRender_;
  bool openingDatabase_;
@@ -244,14 +226,13 @@ class RTABMapApp : public UEventsHandler {
  Scene main_scene_;

 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
-	std::list<rtabmap::RtabmapEvent*> visLocalizationEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;

 	rtabmap::Transform mapToOdom_;

+	boost::mutex cameraMutex_;
 	boost::mutex rtabmapMutex_;
-	boost::mutex visLocalizationMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
@@ -259,7 +240,7 @@ class RTABMapApp : public UEventsHandler {

 	USemaphore screenshotReady_;

-	std::map<int, Mesh> createdMeshes_;
+	std::map<int, rtabmap::Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;

 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
--- a/app/android/jni/jni_interface.cpp
+++ b/app/android/jni/jni_interface.cpp
@@ -31,8 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <RTABMapApp.h>
 #include <scene.h>

-static RTABMapApp app;
-
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -48,300 +46,634 @@ void GetJStringContent(JNIEnv *AEnv, jstring AStr, std::string &ARes) {
  AEnv->ReleaseStringUTFChars(AStr,s);
 }

-JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onCreate(
-    JNIEnv* env, jobject, jobject activity)
+inline jlong jptr(RTABMapApp *native_computer_vision_application) {
+  return reinterpret_cast<intptr_t>(native_computer_vision_application);
+}
+
+inline RTABMapApp *native(jlong ptr) {
+  return reinterpret_cast<RTABMapApp *>(ptr);
+}
+
+JNIEXPORT jlong JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_createNativeApplication(
+    JNIEnv* env, jclass, jobject activity)
 {
-	return app.onCreate(env, activity);
+	return jptr(new RTABMapApp(env, activity));
+}
+
+JNIEXPORT void Java_com_introlab_rtabmap_RTABMapLib_destroyNativeApplication(
+	JNIEnv *, jclass, jlong native_application) {
+	if(native_application)
+	{
+		delete native(native_application);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(
-    JNIEnv* env, jobject, int displayRotation, int cameraRotation)
+    JNIEnv* env, jclass, jlong native_application, int displayRotation, int cameraRotation)
 {
-	return app.setScreenRotation(displayRotation, cameraRotation);
+	if(native_application)
+	{
+		return native(native_application)->setScreenRotation(displayRotation, cameraRotation);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
-    JNIEnv* env, jobject, jstring databasePath, bool databaseInMemory, bool optimize)
+    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize)
 {
 	std::string databasePathC;
 	GetJStringContent(env,databasePath,databasePathC);
-	return app.openDatabase(databasePathC, databaseInMemory, optimize);
+	if(native_application)
+	{
+		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
 }

 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
-    JNIEnv* env, jobject, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+    JNIEnv* env, jclass, jlong native_application, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
 {
-	std::string databasePathC;
-	GetJStringContent(env,databasePath,databasePathC);
-	std::string databaseSourceC;
-	GetJStringContent(env,databaseSource,databaseSourceC);
-	return app.openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+	if(native_application)
+	{
+		std::string databasePathC;
+		GetJStringContent(env,databasePath,databasePathC);
+		std::string databaseSourceC;
+		GetJStringContent(env,databaseSource,databaseSourceC);
+		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
 }

 JNIEXPORT bool JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
-		JNIEnv* env, jobject, jobject iBinder) {
-  return app.onTangoServiceConnected(env, iBinder);
+Java_com_introlab_rtabmap_RTABMapLib_startCamera(
+		JNIEnv* env, jclass, jlong native_application, jobject iBinder, jobject context, jobject activity, int driver) {
+	if(native_application)
+	{
+		return native(native_application)->startCamera(env, iBinder, context, activity, driver);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_initGlContent(
-    JNIEnv*, jobject) {
-  app.InitializeGLContent();
+    JNIEnv*, jclass, jlong native_application) {
+	if(native_application)
+	{
+		native(native_application)->InitializeGLContent();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setupGraphic(
-    JNIEnv*, jobject, jint width, jint height) {
-  app.SetViewPort(width, height);
+    JNIEnv*, jclass, jlong native_application, jint width, jint height) {
+	if(native_application)
+	{
+		native(native_application)->SetViewPort(width, height);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_render(
-    JNIEnv*, jobject) {
-  return app.Render();
+    JNIEnv*, jclass, jlong native_application) {
+	if(native_application)
+	{
+		return native(native_application)->Render();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
 }

 JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onPause(
-    JNIEnv*, jobject) {
-  app.onPause();
+Java_com_introlab_rtabmap_RTABMapLib_stopCamera(
+    JNIEnv*, jclass, jlong native_application) {
+	if(native_application)
+	{
+		native(native_application)->stopCamera();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setCamera(
-    JNIEnv*, jobject, int camera_index) {
-  using namespace tango_gl;
-  GestureCamera::CameraType cam_type =
-      static_cast<GestureCamera::CameraType>(camera_index);
-  app.SetCameraType(cam_type);
+    JNIEnv*, jclass, jlong native_application, int camera_index) {
+	if(native_application)
+	{
+		using namespace tango_gl;
+		GestureCamera::CameraType cam_type =
+		  static_cast<GestureCamera::CameraType>(camera_index);
+		native(native_application)->SetCameraType(cam_type);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_onTouchEvent(
-    JNIEnv*, jobject, int touch_count, int event, float x0, float y0, float x1,
+    JNIEnv*, jclass, jlong native_application, int touch_count, int event, float x0, float y0, float x1,
    float y1) {
-  using namespace tango_gl;
-  GestureCamera::TouchEvent touch_event =
-      static_cast<GestureCamera::TouchEvent>(event);
-  app.OnTouchEvent(touch_count, touch_event, x0, y0, x1, y1);
+	if(native_application)
+	{
+		using namespace tango_gl;
+		GestureCamera::TouchEvent touch_event =
+		  static_cast<GestureCamera::TouchEvent>(event);
+		native(native_application)->OnTouchEvent(touch_count, touch_event, x0, y0, x1, y1);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setPausedMapping(
-		JNIEnv*, jobject, bool paused)
+		JNIEnv*, jclass, jlong native_application, bool paused)
 {
-	return app.setPausedMapping(paused);
+	if(native_application)
+	{
+		return native(native_application)->setPausedMapping(paused);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setOnlineBlending(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setOnlineBlending(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setOnlineBlending(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMapCloudShown(
-		JNIEnv*, jobject, bool shown)
+		JNIEnv*, jclass, jlong native_application, bool shown)
 {
-	return app.setMapCloudShown(shown);
+	if(native_application)
+	{
+		return native(native_application)->setMapCloudShown(shown);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setOdomCloudShown(
-		JNIEnv*, jobject, bool shown)
+		JNIEnv*, jclass, jlong native_application, bool shown)
 {
-	return app.setOdomCloudShown(shown);
+	if(native_application)
+	{
+		return native(native_application)->setOdomCloudShown(shown);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshRendering(
-		JNIEnv*, jobject, bool enabled, bool withTexture)
+		JNIEnv*, jclass, jlong native_application, bool enabled, bool withTexture)
 {
-	return app.setMeshRendering(enabled, withTexture);
+	if(native_application)
+	{
+		return native(native_application)->setMeshRendering(enabled, withTexture);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setPointSize(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setPointSize(value);
+	if(native_application)
+	{
+		return native(native_application)->setPointSize(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setFOV(
-		JNIEnv*, jobject, float fov)
+		JNIEnv*, jclass, jlong native_application, float fov)
 {
-	return app.setFOV(fov);
+	if(native_application)
+	{
+		return native(native_application)->setFOV(fov);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setOrthoCropFactor(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setOrthoCropFactor(value);
+	if(native_application)
+	{
+		return native(native_application)->setOrthoCropFactor(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGridRotation(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setGridRotation(value);
+	if(native_application)
+	{
+		return native(native_application)->setGridRotation(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLighting(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setLighting(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setLighting(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setBackfaceCulling(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setBackfaceCulling(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setBackfaceCulling(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setWireframe(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setWireframe(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setWireframe(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setLocalizationMode(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setLocalizationMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setTrajectoryMode(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setTrajectoryMode(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setTrajectoryMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGraphOptimization(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setGraphOptimization(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setGraphOptimization(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setNodesFiltering(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setNodesFiltering(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
-		JNIEnv*, jobject, bool visible)
+		JNIEnv*, jclass, jlong native_application, bool visible)
 {
-	return app.setGraphVisible(visible);
+	if(native_application)
+	{
+		return native(native_application)->setGraphVisible(visible);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
-		JNIEnv*, jobject, bool visible)
+		JNIEnv*, jclass, jlong native_application, bool visible)
 {
-	return app.setGridVisible(visible);
+	if(native_application)
+	{
+		return native(native_application)->setGridVisible(visible);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setRawScanSaved(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setRawScanSaved(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setRawScanSaved(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setFullResolution(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setFullResolution(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setSmoothing(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setSmoothing(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setCameraColor(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setCameraColor(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setCameraColor(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setAppendMode(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setAppendMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
-		JNIEnv*, jobject, bool enabled)
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setDataRecorderMode(enabled);
+	if(native_application)
+	{
+		return native(native_application)->setDataRecorderMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setMaxCloudDepth(value);
+	if(native_application)
+	{
+		return native(native_application)->setMaxCloudDepth(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMinCloudDepth(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setMinCloudDepth(value);
+	if(native_application)
+	{
+		return native(native_application)->setMinCloudDepth(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setCloudDensityLevel(
-		JNIEnv*, jobject, int value)
+		JNIEnv*, jclass, jlong native_application, int value)
 {
-	return app.setCloudDensityLevel(value);
+	if(native_application)
+	{
+		return native(native_application)->setCloudDensityLevel(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setMeshAngleTolerance(value);
+	if(native_application)
+	{
+		return native(native_application)->setMeshAngleTolerance(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
-		JNIEnv*, jobject, int value)
+		JNIEnv*, jclass, jlong native_application, int value)
 {
-	return app.setMeshTriangleSize(value);
+	if(native_application)
+	{
+		return native(native_application)->setMeshTriangleSize(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setClusterRatio(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setClusterRatio(value);
+	if(native_application)
+	{
+		return native(native_application)->setClusterRatio(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMaxGainRadius(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setMaxGainRadius(value);
+	if(native_application)
+	{
+		return native(native_application)->setMaxGainRadius(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setRenderingTextureDecimation(
-		JNIEnv*, jobject, int value)
+		JNIEnv*, jclass, jlong native_application, int value)
 {
-	return app.setRenderingTextureDecimation(value);
+	if(native_application)
+	{
+		return native(native_application)->setRenderingTextureDecimation(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setBackgroundColor(
-		JNIEnv*, jobject, float value)
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setBackgroundColor(value);
+	if(native_application)
+	{
+		return native(native_application)->setBackgroundColor(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 JNIEXPORT jint JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
-    JNIEnv* env, jobject, jstring key, jstring value)
+    JNIEnv* env, jclass, jlong native_application, jstring key, jstring value)
 {
-	std::string keyC, valueC;
-	GetJStringContent(env,key,keyC);
-	GetJStringContent(env,value,valueC);
-	return app.setMappingParameter(keyC, valueC);
+	if(native_application)
+	{
+		std::string keyC, valueC;
+		GetJStringContent(env,key,keyC);
+		GetJStringContent(env,value,valueC);
+		return native(native_application)->setMappingParameter(keyC, valueC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGPS(
-		JNIEnv*, jobject,
+		JNIEnv*, jclass, jlong native_application,
 		double stamp,
 		double longitude,
 		double latitude,
@@ -349,49 +681,70 @@ Java_com_introlab_rtabmap_RTABMapLib_setGPS(
 		double accuracy,
 		double bearing)
 {
-	return app.setGPS(rtabmap::GPS(stamp,
-			longitude,
-			latitude,
-			altitude,
-			accuracy,
-			bearing));
+	if(native_application)
+	{
+		return native(native_application)->setGPS(rtabmap::GPS(stamp,
+					longitude,
+					latitude,
+					altitude,
+					accuracy,
+					bearing));
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_addEnvSensor(
-		JNIEnv*, jobject,
+		JNIEnv*, jclass, jlong native_application,
 		int type,
 		float value)
 {
-	return app.addEnvSensor(type, value);
-}
-
-JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
-		JNIEnv*, jobject)
-{
-	return app.resetMapping();
+	if(native_application)
+	{
+		return native(native_application)->addEnvSensor(type, value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_save(
-		JNIEnv* env, jobject, jstring databasePath)
+		JNIEnv* env, jclass, jlong native_application, jstring databasePath)
 {
-	std::string databasePathC;
-	GetJStringContent(env,databasePath,databasePathC);
-	return app.save(databasePathC);
+	if(native_application)
+	{
+		std::string databasePathC;
+		GetJStringContent(env,databasePath,databasePathC);
+		return native(native_application)->save(databasePathC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_cancelProcessing(
-		JNIEnv* env, jobject)
+		JNIEnv* env, jclass, jlong native_application)
 {
-	return app.cancelProcessing();
+	if(native_application)
+	{
+		return native(native_application)->cancelProcessing();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }

 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
-		JNIEnv* env, jobject,
+		JNIEnv* env, jclass, jlong native_application,
 		float cloudVoxelSize,
 		bool regenerateCloud,
 		bool meshing,
@@ -409,49 +762,124 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		int optimizedMinTextureClusterSize,
 		bool blockRendering)
 {
-	return app.exportMesh(
-			cloudVoxelSize,
-			regenerateCloud,
-			meshing,
-			textureSize,
-			textureCount,
-			normalK,
-			optimized,
-			optimizedVoxelSize,
-			optimizedDepth,
-			optimizedMaxPolygons,
-			optimizedColorRadius,
-			optimizedCleanWhitePolygons,
-			optimizedMinClusterSize,
-			optimizedMaxTextureDistance,
-			optimizedMinTextureClusterSize,
-			blockRendering);
+	if(native_application)
+	{
+		return native(native_application)->exportMesh(
+					cloudVoxelSize,
+					regenerateCloud,
+					meshing,
+					textureSize,
+					textureCount,
+					normalK,
+					optimized,
+					optimizedVoxelSize,
+					optimizedDepth,
+					optimizedMaxPolygons,
+					optimizedColorRadius,
+					optimizedCleanWhitePolygons,
+					optimizedMinClusterSize,
+					optimizedMaxTextureDistance,
+					optimizedMinTextureClusterSize,
+					blockRendering);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
 }

 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postExportation(
-		JNIEnv* env, jobject, bool visualize)
+		JNIEnv* env, jclass, jlong native_application, bool visualize)
 {
-	return app.postExportation(visualize);
+	if(native_application)
+	{
+		return native(native_application)->postExportation(visualize);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
 }

 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_writeExportedMesh(
-		JNIEnv* env, jobject, jstring directory, jstring name)
+		JNIEnv* env, jclass, jlong native_application, jstring directory, jstring name)
 {
-	std::string directoryC;
-	GetJStringContent(env,directory,directoryC);
-	std::string nameC;
-	GetJStringContent(env,name,nameC);
-	return app.writeExportedMesh(directoryC, nameC);
+	if(native_application)
+	{
+		std::string directoryC;
+		GetJStringContent(env,directory,directoryC);
+		std::string nameC;
+		GetJStringContent(env,name,nameC);
+		return native(native_application)->writeExportedMesh(directoryC, nameC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
 }


 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
-		JNIEnv* env, jobject, int approach)
+		JNIEnv* env, jclass, jlong native_application, int approach)
 {
-	return app.postProcessing(approach);
+	if(native_application)
+	{
+		return native(native_application)->postProcessing(approach);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postCameraPoseEvent(
+		JNIEnv* env, jclass, jlong native_application,
+		float x, float y, float z, float qx, float qy, float qz, float qw)
+{
+	if(native_application)
+	{
+		native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return;
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
+		JNIEnv* env, jclass, jlong native_application,
+		float x, float y, float z, float qx, float qy, float qz, float qw,
+		float fx, float fy, float cx, float cy,
+		double stamp,
+		jobject rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
+		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat)
+{
+	if(native_application)
+	{
+		void *rgbPtr = env->GetDirectBufferAddress(rgb);
+		void *depthPtr = env->GetDirectBufferAddress(depth);
+		native(native_application)->postOdometryEvent(
+				x,y,z,qx,qy,qz,qw,
+				fx,fy,cx,cy,
+				stamp,
+				rgbPtr, rgbLen, rgbWidth, rgbHeight, rgbFormat,
+				depthPtr, depthLen, depthWidth, depthHeight, depthFormat);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return;
+	}
 }


--- a/app/android/jni/point_cloud_drawable.cpp
+++ b/app/android/jni/point_cloud_drawable.cpp
@@ -318,7 +318,7 @@ PointCloudDrawable::PointCloudDrawable(
 }

 PointCloudDrawable::PointCloudDrawable(
-		const Mesh & mesh,
+		const rtabmap::Mesh & mesh,
 		bool createWireframe) :
 				vertex_buffers_(0),
 				textures_(0),
@@ -525,7 +525,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	nPoints_ = totalPoints;
 }

-void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
+void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;
--- a/app/android/jni/point_cloud_drawable.h
+++ b/app/android/jni/point_cloud_drawable.h
@@ -55,13 +55,13 @@ private:
 		  float gainG = 1.0f,
 		  float gainB = 1.0f);
  PointCloudDrawable(
-    	  const Mesh & mesh,
+    	  const rtabmap::Mesh & mesh,
 		  bool createWireframe = false);
  virtual ~PointCloudDrawable();

  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
-  void updateMesh(const Mesh & mesh, bool createWireframe = false);
+  void updateMesh(const rtabmap::Mesh & mesh, bool createWireframe = false);
  void setPose(const rtabmap::Transform & pose);
  void setVisible(bool visible) {visible_=visible;}
  void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
--- a/app/android/jni/scene.cpp
+++ b/app/android/jni/scene.cpp
@@ -79,7 +79,8 @@ Scene::Scene() :
 		graphVisible_(true),
 		gridVisible_(true),
 		traceVisible_(true),
-		color_camera_to_display_rotation_(ROTATION_0),
+		frustumVisible_(true),
+		color_camera_to_display_rotation_(rtabmap::ROTATION_0),
 		currentPose_(0),
 		graph_shader_program_(0),
 		blending_(true),
@@ -108,6 +109,7 @@ Scene::Scene() :
 Scene::~Scene() {
 	DeleteResources();
 	delete gesture_camera_;
+	delete currentPose_;
 }

 //Should only be called in OpenGL thread!
@@ -126,7 +128,6 @@ void Scene::InitGLContent()
 	trace_ = new tango_gl::Trace();
 	grid_ = new tango_gl::Grid();
 	box_ = new BoundingBoxDrawable();
-	currentPose_ = new rtabmap::Transform();


 	axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
@@ -158,7 +159,6 @@ void Scene::DeleteResources() {
 		delete frustum_;
 		delete trace_;
 		delete grid_;
-		delete currentPose_;
 		delete box_;
 	}

@@ -367,6 +367,10 @@ bool intersectFrustumAABB(
 int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);

+	if(currentPose_ == 0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
 	Eigen::Quaternionf quat = currentPose_->getQuaternionf();
 	glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
@@ -493,7 +497,7 @@ int Scene::Render() {

 	if(!currentPose_->isNull())
 	{
-		if (gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
 		{
 			frustum_->SetPosition(position);
 			frustum_->SetRotation(rotation);
@@ -502,8 +506,13 @@ int Scene::Render() {
 			frustum_->SetScale(kFrustumScale);
 			frustum_->Render(projectionMatrix, viewMatrix);

-			axis_->SetPosition(position);
-			axis_->SetRotation(rotation);
+			rtabmap::Transform cameraFrame = *currentPose_*rtabmap::optical_T_opengl*rtabmap::CameraMobile::opticalRotationInv;
+			glm::vec3 positionCamera(cameraFrame.x(), cameraFrame.y(), cameraFrame.z());
+			Eigen::Quaternionf quatCamera = cameraFrame.getQuaternionf();
+			glm::quat rotationCamera(quatCamera.w(), quatCamera.x(), quatCamera.y(), quatCamera.z());
+
+			axis_->SetPosition(positionCamera);
+			axis_->SetRotation(rotationCamera);
 			axis_->Render(projectionMatrix, viewMatrix);
 		}

@@ -512,11 +521,11 @@ int Scene::Render() {
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+	}

-		if(gridVisible_)
-		{
-			grid_->Render(projectionMatrix, viewMatrix);
-		}
+	if(gridVisible_)
+	{
+		grid_->Render(projectionMatrix, viewMatrix);
 	}

 	if(graphVisible_ && graph_)
@@ -571,8 +580,11 @@ void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {

 void Scene::SetCameraPose(const rtabmap::Transform & pose)
 {
-	UASSERT(currentPose_ != 0);
 	UASSERT(!pose.isNull());
+	if(currentPose_ ==0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	*currentPose_ = pose;
 }

@@ -600,7 +612,7 @@ rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 	{
 		*fov = gesture_camera_->getFOV();
 	}
-	return glmToTransform(gesture_camera_->GetTransformationMatrix());
+	return rtabmap::glmToTransform(gesture_camera_->GetTransformationMatrix());
 }

 void Scene::OnTouchEvent(int touch_count,
@@ -658,6 +670,11 @@ void Scene::setTraceVisible(bool visible)
 	traceVisible_ = visible;
 }

+void Scene::setFrustumVisible(bool visible)
+{
+	frustumVisible_ = visible;
+}
+
 //Should only be called in OpenGL thread!
 void Scene::addMarker(
 		int id,
@@ -728,7 +745,7 @@ void Scene::addCloud(

 void Scene::addMesh(
 		int id,
-		const Mesh & mesh,
+		const rtabmap::Mesh & mesh,
 		const rtabmap::Transform & pose,
 		bool createWireframe)
 {
@@ -847,7 +864,7 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
 	}
 }

-void Scene::updateMesh(int id, const Mesh & mesh)
+void Scene::updateMesh(int id, const rtabmap::Mesh & mesh)
 {
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())
--- a/app/android/jni/scene.h
+++ b/app/android/jni/scene.h
@@ -21,7 +21,7 @@
 #include <memory>
 #include <set>

-#include <tango_client_api.h>  // NOLINT
+#include "CameraMobile.h"
 #include <tango-gl/axis.h>
 #include <tango-gl/camera.h>
 #include <tango-gl/color.h>
@@ -60,7 +60,8 @@ class Scene {
  int getViewPortWidth() const {return screenWidth_;}
  int getViewPortHeight() const {return screenHeight_;}

-  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
+  rtabmap::ScreenRotation getScreenRotation() const {return color_camera_to_display_rotation_;}
+  void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}

  void clear(); // removed all point clouds

@@ -78,7 +79,7 @@ class Scene {
  //         top down
  void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);

-  void SetCameraPose(const rtabmap::Transform & pose);
+  void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
  rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
  rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;

@@ -101,6 +102,7 @@ class Scene {
  void setGraphVisible(bool visible);
  void setGridVisible(bool visible);
  void setTraceVisible(bool visible);
+  void setFrustumVisible(bool visible);

  void addMarker(int id, const rtabmap::Transform & pose);
  void setMarkerPose(int id, const rtabmap::Transform & pose);
@@ -115,7 +117,7 @@ class Scene {
  		  const rtabmap::Transform & pose);
  void addMesh(
  		int id,
-  		const Mesh & mesh,
+  		const rtabmap::Mesh & mesh,
  		const rtabmap::Transform & pose,
 		bool createWireframe = false);

@@ -126,7 +128,7 @@ class Scene {
  bool hasTexture(int id) const;
  std::set<int> getAddedClouds() const;
  void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
-  void updateMesh(int id, const Mesh & mesh);
+  void updateMesh(int id, const rtabmap::Mesh & mesh);
  void updateGains(int id, float gainR, float gainG, float gainB);

  void setBlending(bool enabled) {blending_ = enabled;}
@@ -172,10 +174,11 @@ class Scene {
  bool graphVisible_;
  bool gridVisible_;
  bool traceVisible_;
+  bool frustumVisible_;

  std::map<int, tango_gl::Axis*> markers_;

-  TangoSupportRotation color_camera_to_display_rotation_;
+  rtabmap::ScreenRotation color_camera_to_display_rotation_;

  std::map<int, PointCloudDrawable*> pointClouds_;

--- a/app/android/jni/tango-gl/frustum.cpp
+++ b/app/android/jni/tango-gl/frustum.cpp
@@ -18,11 +18,14 @@
 namespace tango_gl {

 static const float float_vertices[] = {
-    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  1.0f,
-    1.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f, 0.0f,  0.0f,
-    0.0f,  1.0f,  -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
-    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f,
-    -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
+    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f,  -1.0f, -1.0f,
+	-1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f,
+	1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f,
+	-1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};

 Frustum::Frustum() : Line(3.0f, GL_LINES) {
  SetShader();
--- a/app/android/jni/tango-gl/grid.cpp
+++ b/app/android/jni/tango-gl/grid.cpp
@@ -31,13 +31,17 @@ Grid::Grid(float density, int qx, int qy) : Line(1.0f, GL_LINES) {

  // Horizontal line.
  for (int i = 0; i < (qy + 1); i++) {
-    vec_vertices_.push_back(glm::vec3(-width, 0.0f, -height + i * density));
-    vec_vertices_.push_back(glm::vec3(width, 0.0f, -height + i * density));
+	  for (int j = 0; j < (qx + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + j*density, 0.0f, -height + i * density));
+		vec_vertices_.push_back(glm::vec3(-width+ + (j+1)*density, 0.0f, -height + i * density));
+	  }
  }

  for (int i = 0; i < (qx + 1); i++) {
-    vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height));
-    vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, height));
+	  for (int j = 0; j < (qy + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + j*density));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + (j+1)*density));
+	  }
  }
 }
 }  // namespace tango_gl
--- a/app/android/jni/tango-gl/include/tango-gl/util.h
+++ b/app/android/jni/tango-gl/include/tango-gl/util.h
@@ -25,7 +25,6 @@
 #include <android/log.h>
 #include <GLES2/gl2.h>
 #include <GLES2/gl2ext.h>
-#include <tango_support_api.h>

 #include "glm/glm.hpp"
 #include "glm/gtc/matrix_transform.hpp"
@@ -44,6 +43,7 @@
 #define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
 #endif
 #define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
+#define LOGF(...) __android_log_print(ANDROID_LOG_FATAL,LOG_TAG,__VA_ARGS__)

 #ifndef M_PI
 #define M_PI 3.1415926f
@@ -84,32 +84,6 @@ namespace util {

  glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec);

-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @param display: integer value of display orientation, values available
-  // are 0, 1, 2 ,3. Followed by Android display orientation standard:
-  // https://developer.android.com/reference/android/view/Display.html#getRotation()
-  // @param color_camera: integer value of color camera oreintation, values
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      int display_rotation, int color_camera_rotation);
-
-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @param display: the device display orientation.
-  // @param color_camera: integer value of color camera oreintation, values
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      TangoSupportRotation display_rotation, int color_camera_rotation);
-
 }  // namespace util
 }  // namespace tango_gl
 #endif  // TANGO_GL_RENDERER_GL_UTIL
--- a/app/android/jni/tango-gl/util.cpp
+++ b/app/android/jni/tango-gl/util.cpp
@@ -238,23 +238,4 @@ glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
  return glm::vec3(mat * glm::vec4(vec, 1.0f));
 }

-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    int display_rotation, int color_camera_rotation) {
-  TangoSupportRotation r =
-      static_cast<TangoSupportRotation>(display_rotation);
-  return util::GetAndroidRotationFromColorCameraToDisplay(
-      r, color_camera_rotation);
-}
-
-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    TangoSupportRotation display_rotation, int color_camera_rotation) {
-  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
-
-  int ret = static_cast<int>(display_rotation) - color_camera_n;
-  if (ret < 0) {
-    ret += 4;
-  }
-  return static_cast<TangoSupportRotation>(ret % 4);
-}
-
 }  // namespace tango_gl
--- a/app/android/jni/util.h
+++ b/app/android/jni/util.h
@@ -39,6 +39,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/Vertices.h>
 #include <pcl/pcl_base.h>

+namespace rtabmap {
+
 class LogHandler : public UEventsHandler
 {
 public:
@@ -58,19 +60,23 @@ protected:
 			ULogEvent * logEvent = (ULogEvent*)event;
 			if(logEvent->getCode() == ULogger::kDebug)
 			{
-				LOGD(logEvent->getMsg().c_str());
+				LOGD("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() == ULogger::kInfo)
 			{
-				LOGI(logEvent->getMsg().c_str());
+				LOGI("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() == ULogger::kWarning)
 			{
-				LOGW(logEvent->getMsg().c_str());
+				LOGW("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() >= ULogger::kError)
 			{
-				LOGE(logEvent->getMsg().c_str());
+				LOGE("%s", logEvent->getMsg().c_str());
+			}
+			else if(logEvent->getCode() >= ULogger::kFatal)
+			{
+				LOGF("%s", logEvent->getMsg().c_str());
 			}

 		}
@@ -78,6 +84,11 @@ protected:
 	}
 };

+static const rtabmap::Transform optical_T_opengl(
+		1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f, -1.0f,  0.0f, 0.0f,
+		0.0f,  0.0f, -1.0f, 0.0f);
+
 static const rtabmap::Transform opengl_world_T_tango_world(
 		1.0f,  0.0f,  0.0f, 0.0f,
 		0.0f,  0.0f,  1.0f, 0.0f,
@@ -88,20 +99,25 @@ static const rtabmap::Transform rtabmap_world_T_tango_world(
 	    -1.0f, 0.0f, 0.0f, 0.0f,
 		 0.0f, 0.0f, 1.0f, 0.0f);

-static const rtabmap::Transform tango_device_T_rtabmap_device(
+static const rtabmap::Transform tango_device_T_rtabmap_world(
 		 0.0f, -1.0f, 0.0f, 0.0f,
 	     0.0f,  0.0f, 1.0f, 0.0f,
 		-1.0f,  0.0f, 0.0f, 0.0f);

+static const rtabmap::Transform tango_world_T_rtabmap_world(
+		 0.0f, -1.0f, 0.0f, 0.0f,
+	     1.0f, 0.0f, 0.0f, 0.0f,
+		 0.0f, 0.0f, 1.0f, 0.0f);
+
 static const rtabmap::Transform opengl_world_T_rtabmap_world(
 		 0.0f, -1.0f, 0.0f, 0.0f,
 		 0.0f,  0.0f, 1.0f, 0.0f,
 		-1.0f,  0.0f, 0.0f, 0.0f);

-static const rtabmap::Transform rtabmap_device_T_opengl_device(
-		 0.0f, 0.0f, -1.0f, 0.0f,
-		-1.0f, 0.0f,  0.0f, 0.0f,
-		 0.0f, 1.0f,  0.0f, 0.0f);
+static const rtabmap::Transform rtabmap_world_T_opengl_world(
+		 0.0f,  0.0f,-1.0f, 0.0f,
+		-1.0f,  0.0f, 0.0f, 0.0f,
+		 0.0f,  1.0f, 0.0f, 0.0f);

 inline glm::mat4 glmFromTransform(const rtabmap::Transform & transform)
 {
@@ -173,4 +189,81 @@ public:
 	cv::Mat texture;
 };

+typedef enum {
+  /// Not apply any rotation.
+  ROTATION_IGNORED = -1,
+
+  /// 0 degree rotation (natural orientation)
+  ROTATION_0 = 0,
+
+  /// 90 degree rotation.
+  ROTATION_90 = 1,
+
+  /// 180 degree rotation.
+  ROTATION_180 = 2,
+
+  /// 270 degree rotation.
+  ROTATION_270 = 3
+} ScreenRotation;
+
+inline int NormalizedColorCameraRotation(int camera_rotation) {
+  int camera_n = 0;
+  switch (camera_rotation) {
+    case 90:
+      camera_n = 1;
+      break;
+    case 180:
+      camera_n = 2;
+      break;
+    case 270:
+      camera_n = 3;
+      break;
+    default:
+      camera_n = 0;
+      break;
+  }
+  return camera_n;
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @param display: the device display orientation.
+// @param color_camera: integer value of color camera oreintation, values
+// available are 0, 90, 180, 270. Followed by Android camera orientation
+// standard:
+// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
+		ScreenRotation display_rotation, int color_camera_rotation) {
+  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
+
+  int ret = static_cast<int>(display_rotation) - color_camera_n;
+  if (ret < 0) {
+    ret += 4;
+  }
+  return static_cast<ScreenRotation>(ret % 4);
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @param display: integer value of display orientation, values available
+// are 0, 1, 2 ,3. Followed by Android display orientation standard:
+// https://developer.android.com/reference/android/view/Display.html#getRotation()
+// @param color_camera: integer value of color camera oreintation, values
+// available are 0, 90, 180, 270. Followed by Android camera orientation
+// standard:
+// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
+    int display_rotation, int color_camera_rotation) {
+	ScreenRotation r =
+      static_cast<ScreenRotation>(display_rotation);
+  return GetAndroidRotationFromColorCameraToDisplay(
+      r, color_camera_rotation);
+}
+
+}
+
 #endif /* UTIL_H_ */