CloudViewer: fixed build with vtk<6

* Added Kinect for Azure menu option

* Reintegrated Kinect for Azure initialization code, fixed previous GUI commit

* Added ifdefs to fix build

* Added preliminary K4A device support

* Edits to K4A camera implementation

* K4A minor initialization fix

* Typo

* Another initialization fix

* Update CameraRGBD tool to display K4A camera output

* K4A added model parameter so we can create maps

.appveyor.yml

@@ -0,0 +1,137 @@
+
+branches:
+  only:
+    - master
+    - devel
+
+os: Visual Studio 2015
+
+clone_folder: c:\projects\rtabmap
+
+platform: x64
+configuration: Release
+
+init:
+  - cmake --version
+  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
+  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
+
+install:
+  # Qt
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
+  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
+  - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
+  # Openni2
+  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
+  - cmd: OpenNI2.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenNI2:"
+  - ps: "ls \"C:/Program Files/OpenNI2\""
+  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
+  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
+  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
+  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
+  # OpenCV
+  #- ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
+  #- cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
+  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe' -outfile opencv-2.4.13.6-vc14.exe
+  - cmd: opencv-2.4.13.6-vc14.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenCV:"
+  - ps: "ls \"C:/Program Files/opencv/build\""
+  - set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
+  # VTK (including QVTK)
+  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
+  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
+  - ECHO "Installed VTK:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed QHull:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed FLANN:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed Eigen:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe
+  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
+  # zlib
+  - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
+  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
+  - ECHO "Installed zlib:"
+  - ps: "ls \"C:/Program Files/zlib\""
+  - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
+  # OctoMap
+  - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe
+  - cmd: octomap.exe -o"C:\Program Files" -y
+  - ECHO "Installed OctoMap:"
+  - ps: "ls \"C:/Program Files/octomap-distribution\""
+  - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin
+  # CPU-TSDF
+  - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe
+  - cmd: cpu_tsdf.exe -o"C:\Program Files" -y
+  - ECHO "Installed CPU-TSDF:"
+  - ps: "ls \"C:/Program Files/cpu_tsdf\""
+  - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin
+  # Open Chisel
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe
+  - cmd: open_chisel.exe -o"C:\Program Files" -y
+  - ECHO "Installed Open Chisel:"
+  - ps: "ls \"C:/Program Files/open_chisel\""
+  - set PATH=%PATH%;C:\Program Files\open_chisel\bin
+  # cvsba
+  - ps: wget 'https://dl.dropboxusercontent.com/s/4ey8ergerx46zvj/cvsba_x64_vc14.exe?dl=0' -outfile cvsba.exe
+  - cmd: cvsba.exe -o"C:\Program Files" -y
+  - ECHO "Installed cvsba:"
+  - ps: "ls \"C:/Program Files/cvsba\""
+  - set PATH=%PATH%;C:\Program Files\cvsba\bin
+  - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe
+  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
+  - ECHO "Installed yaml-cpp:"
+  - ps: "ls \"C:/Program Files/yaml-cpp\""
+  
+before_build:
+  - cd c:\projects\rtabmap\build
+  - ECHO %PROGRAMFILES%
+  - ECHO %PATH%
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dcvsba_DIR="C:\Program Files\cvsba\lib\cmake" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON ..
+  
+after_build :
+  - cmake --build . --config Release --target package
+
+artifacts:
+  - path: build\RTABMap-*
+  
+notifications:
+  - provider: Email
+    to:
+      - matlabbe@gmail.com
+    on_build_success: false
+    on_build_failure: false
+    on_build_status_changed: true

.gitignore

@@ -1,6 +1,9 @@
 /lib
 .DS_Store
 .settings/language.settings.xml
+.idea/
+.vscode
+cmake-build-debug/
 app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml

.travis.yml

@@ -1,30 +1,57 @@
 sudo: true
-dist: trusty
 language: cpp
+group: deprecated-2017Q3
 
 compiler:
   - gcc
-  - clang
 
-addons:
-  apt:
-    packages:
-      - cmake
-      - libopencv-dev
-      - libqt4-dev
-      - libsqlite3-dev
+matrix:
+  include:
 
-install:
-    - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-    - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-    - sudo apt-get update
-    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev
+    - dist: trusty
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-indigo-rtabmap-ros
+        - sudo apt-get -y remove ros-indigo-rtabmap
 
-script: 
-    - source /opt/ros/indigo/setup.bash
-    - mkdir -p build && cd build
-    - cmake .. 
-    - make
+      script: 
+        - source /opt/ros/indigo/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
+
+    - dist: xenial
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-kinetic-rtabmap-ros
+        - sudo apt-get -y remove ros-kinetic-rtabmap
+
+      script: 
+        - source /opt/ros/kinetic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
+
+    - dist: bionic
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-melodic-rtabmap-ros
+        - sudo apt-get -y remove ros-melodic-rtabmap
+
+      script: 
+        - source /opt/ros/melodic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
 
 notifications:
   email:

README.md

@@ -1,10 +1,27 @@
-rtabmap [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap)
+rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pixel) 
 =======
 
+[![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
+
+[![Release][release-image]][releases]
+[![License][license-image]][license]
+Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
+
+[release-image]: https://img.shields.io/badge/release-0.18.0-green.svg?style=flat
+[releases]: https://github.com/introlab/rtabmap/releases
+
+[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
+[license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
+
 RTAB-Map library and standalone application.
 
 For more information, visit the [RTAB-Map's home page](http://introlab.github.io/rtabmap) or the [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
 
 To use RTAB-Map under ROS, visit the [rtabmap](http://wiki.ros.org/rtabmap) page on the ROS wiki.
 
+### Acknowledgements
+This project is supported by [IntRoLab - Intelligent / Interactive / Integrated / Interdisciplinary Robot Lab](https://introlab.3it.usherbrooke.ca/), Sherbrooke, Québec, Canada.
 
+<a href="https://introlab.3it.usherbrooke.ca/">
+<img src="https://github.com/introlab/16SoundsUSB/blob/master/images/IntRoLab.png" alt="IntRoLab" height="100">
+</a>

RTABMapConfig.cmake.in

@@ -1,4 +1,8 @@
 # - Config file for the RTABMap package
+# Components:
+#  core    (required)
+#  gui     (optional)
+#  utilite (required)
 # It defines the following variables
 #  RTABMap_INCLUDE_DIRS - include directories for RTABMap
 #  RTABMap_LIBRARIES    - libraries to link against
@@ -42,8 +46,18 @@ ENDIF()
 
 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
 
+list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
+set(WITH_GUI ON)
+if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
+   if (${_index} EQUAL -1)
+      set(WITH_GUI OFF)
+   endif()
+endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+
+
 #gui lib (OFF if RTAB-Map is not built with Qt)
-if(@CONF_WITH_GUI@)
+if(@CONF_WITH_GUI@ AND ${WITH_GUI})
    find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    
@@ -59,13 +73,15 @@ if(@CONF_WITH_GUI@)
    ENDIF()
    
    set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
-endif(@CONF_WITH_GUI@)
+elseif(${WITH_GUI})
+   MESSAGE(WARNING "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
+endif()
 
 # Dependencies
-if(@CONF_VTK_QT@)
+if(@CONF_VTK_QT@ AND ${WITH_GUI})
    find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
-endif(@CONF_VTK_QT@)
-set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
+endif(@CONF_VTK_QT@ AND ${WITH_GUI})
+SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
 
 #backward compatibilities
 set(RTABMAP_CORE ${RTABMap_CORE})
@@ -74,3 +90,7 @@ if(RTABMap_GUI)
    set(RTABMAP_GUI ${RTABMap_GUI})
    set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
 endif(RTABMap_GUI)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
+mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)

Version.h.in

@@ -38,20 +38,45 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
 
 @NONFREE@#define RTABMAP_NONFREE
+@OPENCV_DEV@#define RTABMAP_OPENCV_DEV
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
 @GTSAM@#define RTABMAP_GTSAM
+@CERES@#define RTABMAP_CERES
 @VERTIGO@#define RTABMAP_VERTIGO
-@OPENCV3@#define RTABMAP_OPENCV3
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
+@K4W2@#define RTABMAP_K4W2
+@K4A@#define RTABMAP_K4A
 @CVSBA@#define RTABMAP_CVSBA
+@POINTMATCHER@#define RTABMAP_POINTMATCHER
+@FASTCV@#define RTABMAP_FASTCV
+@LOAM@#define RTABMAP_LOAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
 @REALSENSE@#define RTABMAP_REALSENSE
+@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
+@MYNTEYE@#define RTABMAP_MYNTEYE
 @OCTOMAP@#define RTABMAP_OCTOMAP
+@CPUTSDF@#define RTABMAP_CPUTSDF
+@ALICE_VISION@#define RTABMAP_ALICE_VISION
+@OPENCHISEL@#define RTABMAP_OPENCHISEL
+@FOVIS@#define RTABMAP_FOVIS
+@VISO2@#define RTABMAP_VISO2
+@DVO@#define RTABMAP_DVO
+@OKVIS@#define RTABMAP_OKVIS
+@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
+@VINS@#define RTABMAP_VINS
+@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
+@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
+@SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH
+@PYMATCHER@#define RTABMAP_PYMATCHER
+@MADGWICK@#define RTABMAP_MADGWICK
+
 
 #endif /* VERSION_H_ */
 

app/android/.gitignore

@@ -0,0 +1,2 @@
+/bin/
+/gen/

app/android/AndroidManifest.xml.in

@@ -2,32 +2,40 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="29"
+        android:versionCode="72"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
-    <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
-    <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
+    <uses-permission android:name="android.permission.INTERNET" />
+    <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
+    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-permission android:name="android.permission.ACCESS_WIFI_STATE" />
+    <uses-feature android:name="android.hardware.location.gps" />
     <uses-feature android:glEsVersion="0x00020000" />
 
-    <!-- This is the platform API where NativeActivity was introduced. -->
-    <uses-sdk android:minSdkVersion="17" />
+    <!-- This is the platform API where depth16 support in android was introduced. -->
+    <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
 
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 
         android:label="@string/app_name" 
-        android:icon="@drawable/ic_launcher">
+        android:icon="@drawable/ic_launcher"
+        android:debuggable="@ANDROID_DEBUGGABLE@">
         
-        <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
+        <uses-library android:name="com.projecttango.libtango_device2" android:required="false" />
+        <meta-data android:name="com.google.ar.core" android:value="optional" />
+        <meta-data android:name="com.huawei.ar.engine" android:value="optional" />
 
         <!-- Our activity is the built-in NativeActivity framework class.
              This will take care of integrating with our NDK code. -->
         <activity android:name="RTABMapActivity"
                 android:label="@string/app_name"
                 android:launchMode="singleTask"
-                android:screenOrientation="landscape">
+		android:screenOrientation="fullSensor"
+		android:configChanges="orientation|screenSize|keyboardHidden"
+		android:theme="@style/ThemeApp">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
                     android:value="NativeRTABMap" />
@@ -36,6 +44,30 @@
                 <category android:name="android.intent.category.LAUNCHER" />
             </intent-filter>
         </activity>
+
+        <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>
+        <activity android:name="SketchfabActivity" android:label="@string/sketchfab" android:screenOrientation="fullSensor"/>
+    
+        <meta-data
+            android:name="com.google.ar.core.min_apk_version"
+            android:value="191106000" /> <!-- This activity is critical for installing ARCore when it is not already present. -->
+        <activity
+            android:name="com.google.ar.core.InstallActivity"
+            android:configChanges="keyboardHidden|orientation|screenSize"
+            android:excludeFromRecents="true"
+            android:exported="false"
+            android:launchMode="singleTop"
+            android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
+            
+        <provider
+            android:name="android.support.v4.content.FileProvider"
+            android:authorities="com.introlab.rtabmap.provider"
+            android:exported="false"
+            android:grantUriPermissions="true">
+            <meta-data
+                android:name="android.support.FILE_PROVIDER_PATHS"
+                android:resource="@xml/provider_paths"/>
+        </provider>
     </application>
 
 </manifest>

app/android/CMakeLists.txt

@@ -1,4 +1,102 @@
 
+option(WITH_TANGO         "Include Tango support"                ON)
+option(WITH_ARCORE        "Include ARCore support"               ON)
+option(WITH_ARENGINE      "Include AREngine support"             ON)
+option(DISABLE_LOG        "Disable Android logging (should be true in release)"    ON)
+option(DEPTH_TEST         "Enable depth test on ARCore"    OFF)
+
+# Google Tango needs access to system shared 
+# libraries (e.g. libbinder.so) that are not accessible 
+# with android >=24
+IF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    FIND_PACKAGE(Tango QUIET)
+    IF(Tango_FOUND)
+       MESSAGE(STATUS "Found Tango: ${Tango_INCLUDE_DIRS}")
+    ENDIF(Tango_FOUND)
+ENDIF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+
+IF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+    FIND_PACKAGE(ARCore QUIET)
+    IF(ARCore_FOUND)
+       MESSAGE(STATUS "Found ARCore: ${ARCore_INCLUDE_DIRS}")
+    ENDIF(ARCore_FOUND)
+ENDIF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+
+IF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    FIND_PACKAGE(AREngine QUIET)
+    IF(AREngine_FOUND)
+       MESSAGE(STATUS "Found AREngine: ${AREngine_INCLUDE_DIRS}")
+    ENDIF(AREngine_FOUND)
+ENDIF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+
+IF(NOT Tango_FOUND)
+   SET(TANGO "//")
+ENDIF(NOT Tango_FOUND)
+IF(NOT ARCore_FOUND)
+   SET(ARCORE "//")
+ENDIF(NOT ARCore_FOUND)
+IF(NOT AREngine_FOUND)
+   SET(ARENGINE "//")
+ENDIF(NOT AREngine_FOUND)
+
+CONFIGURE_FILE(CameraAvailability.h.in ${CMAKE_CURRENT_SOURCE_DIR}/jni/CameraAvailability.h)
+
+
+IF(DISABLE_LOG)
+  ADD_DEFINITIONS(-DDISABLE_LOG)
+ENDIF(DISABLE_LOG)
+IF(DEPTH_TEST)
+  ADD_DEFINITIONS(-DDEPTH_TEST)
+ENDIF(DEPTH_TEST)
+
+MESSAGE(STATUS "--------------------------------------------")
+MESSAGE(STATUS "Android build info:")
+MESSAGE(STATUS "  DISABLE_LOG    = ${DISABLE_LOG}")
+MESSAGE(STATUS "  DEPTH_TEST     = ${DEPTH_TEST}")
+IF(Tango_FOUND)
+  MESSAGE(STATUS "  With Tango     = YES")
+ELSEIF(NOT WITH_TANGO)
+  MESSAGE(STATUS "  With Tango     = NO (WITH_TANGO=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    MESSAGE(STATUS "  With Tango     = NO (ANDROID_NATIVE_API_LEVEL should be <= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With Tango     = NO (tango not found)")
+  ENDIF()
+ENDIF()
+IF(ARCore_FOUND)
+  MESSAGE(STATUS "  With ARCore    = YES")
+ELSEIF(NOT WITH_ARCORE)
+  MESSAGE(STATUS "  With ARCore    = NO (WITH_ARCORE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 23)
+    MESSAGE(STATUS "  With ARCore    = NO (ANDROID_NATIVE_API_LEVEL should be >= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With ARCore    = NO (ARCore not found)")
+  ENDIF()
+ENDIF()
+IF(AREngine_FOUND)
+  MESSAGE(STATUS "  With AREngine  = YES")
+ELSEIF(NOT WITH_ARENGINE)
+  MESSAGE(STATUS "  With AREngine  = NO (WITH_ARENGINE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    MESSAGE(STATUS "  With AREngine  = NO (ANDROID_NATIVE_API_LEVEL should be >= 24)")
+  ELSE()
+    MESSAGE(STATUS "  With AREngine  = NO (AREngine not found)")
+  ENDIF()
+ENDIF()
+MESSAGE(STATUS "  ANDROID_NATIVE_API_LEVEL       = ${ANDROID_NATIVE_API_LEVEL}")
+MESSAGE(STATUS "  ANDROID_COMPILER_FLAGS_RELEASE = ${ANDROID_COMPILER_FLAGS_RELEASE}")
+MESSAGE(STATUS "  ANDROID_TOOLCHAIN_PREFIX       = ${ANDROID_TOOLCHAIN_PREFIX}")
+
+
+IF(DISABLE_LOG)
+   SET(ANDROID_DEBUGGABLE false)
+ELSE()
+   SET(ANDROID_DEBUGGABLE true)
+ENDIF()
+
 add_subdirectory(jni)
 
 
@@ -16,7 +114,8 @@ endif()
 
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
-  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml")
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
+  @ONLY)
   
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
@@ -32,6 +131,11 @@ configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/ant.properties.in"
   "${CMAKE_CURRENT_BINARY_DIR}/ant.properties"
   @ONLY)
+  
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/custom_rules.xml"
+  "${CMAKE_CURRENT_BINARY_DIR}/custom_rules.xml"
+  COPYONLY)
 
 add_custom_target(NativeRTABMap-ant-configure ALL
   COMMAND "${ANDROID_EXECUTABLE}"

app/android/CameraAvailability.h.in

@@ -0,0 +1,39 @@
+/*
+Copyright (c) 2010-2014, 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 UNIVERTY DE SHERBROOKE 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 CAMERAAVAILABILITY_H_
+#define CAMERAAVAILABILITY_H_
+
+// This is auto-generated!
+
+@TANGO@#define RTABMAP_TANGO
+@ARCORE@#define RTABMAP_ARCORE
+@ARENGINE@#define RTABMAP_ARENGINE
+
+
+#endif /* CAMERAAVAILABILITY_H_ */
+

app/android/ant.properties.in

@@ -1,5 +1,6 @@
 builddir=@CMAKE_CURRENT_BINARY_DIR@
 srcdir=@CMAKE_CURRENT_SOURCE_DIR@
+android.abi=@ANDROID_ABI@
 
 source.dir=${srcdir}/src
 gen.dir=${builddir}/gen

app/android/custom_rules.xml

@@ -0,0 +1,13 @@
+<project>
+    <target name="-pre-build">
+		<copy todir="${jar.libs.dir}">
+			<fileset dir="${srcdir}/libs" includes="**/*.jar" excludes="**/*sources.jar, **/*javadoc.jar" />
+		</copy>
+		<copy todir="${native.libs.dir}/${android.abi}">
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="*.so"/>
+		</copy>
+		<copy todir="${native.libs.dir}">
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
+		</copy>
+	</target>
+</project>

app/android/info.txt.in

@@ -1,8 +1,7 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
 Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
-Copyright 2016<br>
+Copyright 2016-2020<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
 <b>http://introlab.github.io/rtabmap</b><br><br>
 
-

app/android/jni/.gitignore

@@ -0,0 +1 @@
+CameraAvailability.h

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)

app/android/jni/CameraARCore.cpp

@@ -0,0 +1,702 @@
+/*
+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();
+}
+
+LaserScan CameraARCore::scanFromPointCloudData(
+		const float * pointCloudData,
+		int points,
+		const Transform & pose,
+		const CameraModel & model,
+		const cv::Mat & rgb,
+		std::vector<cv::KeyPoint> * kpts,
+		std::vector<cv::Point3f> * kpts3D)
+{
+	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];
+				if(kpts)
+					kpts->push_back(cv::KeyPoint(u,v,3));
+				if(kpts3D)
+					kpts3D->push_back(org);
+			}
+			*(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);
+
+		}
+		return LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
+	}
+	return LaserScan();
+}
+
+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);
+#ifndef DISABLE_LOG
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
+#endif
+
+		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)
+				{
+
+#ifndef DISABLE_LOG
+					int32_t num_planes;
+					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
+					for(int i=0;i<num_planes; ++i)
+					{
+						int32_t pixel_stride;
+						int32_t row_stride;
+						ArImage_getPlanePixelStride(arSession_, image, i, &pixel_stride);
+						ArImage_getPlaneRowStride(arSession_, image, i, &row_stride);
+						LOGI("Plane %d/%d: pixel stride=%d, row stride=%d", i+1, num_planes, pixel_stride, row_stride);
+					}
+#endif
+					const uint8_t * plane_data;
+					const uint8_t * plane_uv_data;
+					int32_t data_length;
+					ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
+					int32_t uv_data_length;
+					ArImage_getPlaneData(arSession_, image, 2, &plane_uv_data, &uv_data_length);
+
+					if(plane_data != nullptr && data_length == height*width)
+					{
+						double stamp = double(timestamp_ns)/10e8;
+#ifndef DISABLE_LOG
+						LOGI("data_length=%d stamp=%f", data_length, stamp);
+#endif
+						cv::Mat rgb;
+						if((long)plane_uv_data-(long)plane_data != data_length)
+						{
+							// The uv-plane is not concatenated to y plane in memory, so concatenate them
+							cv::Mat yuv(height+height/2, width, CV_8UC1);
+							memcpy(yuv.data, plane_data, data_length);
+							memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
+							cv::cvtColor(yuv, rgb, CV_YUV2BGR_NV21);
+						}
+						else
+						{
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
+						}
+
+						std::vector<cv::KeyPoint> kpts;
+						std::vector<cv::Point3f> kpts3;
+						LaserScan scan;
+						if(pointCloud)
+						{
+							int32_t points = 0;
+							ArPointCloud_getNumberOfPoints(arSession_, pointCloud, &points);
+							const float * pointCloudData = 0;
+							ArPointCloud_getData(arSession_, pointCloud, &pointCloudData);
+#ifndef DISABLE_LOG
+							LOGI("pointCloudData=%d size=%d", pointCloudData?1:0, points);
+#endif
+							if(pointCloudData && points>0)
+							{
+								scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3);
+							}
+						}
+						else
+						{
+							LOGI("pointCloud empty");
+						}
+
+						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+						data.setFeatures(kpts,  kpts3, cv::Mat());
+					}
+				}
+				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 */

app/android/jni/CameraARCore.h

@@ -0,0 +1,114 @@
+/*
+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:
+	static LaserScan scanFromPointCloudData(
+			const float * pointCloudData,
+			int points,
+			const Transform & pose,
+			const CameraModel & model,
+			const cv::Mat & rgb,
+			std::vector<cv::KeyPoint> * kpts = 0,
+			std::vector<cv::Point3f> * kpts3D = 0);
+
+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_ */

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 */

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_ */

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 */

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_ */

app/android/jni/CameraTango.cpp

@@ -40,39 +40,49 @@ namespace rtabmap {
 const int kVersionStringLength = 128;
 const int holeSize = 5;
 const float maxDepthError = 0.10;
-const int scanDownsampling = 10;
+const int scanDownsampling = 1;
 
 // Callbacks
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
+	if(app->isRunning() && point_cloud->num_points>0)
+	{
+		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
+	}
 }
 
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	cv::Mat tangoImage;
-	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+	if(app->isRunning())
 	{
-		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
-	}
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
-	{
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-	}
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-	{
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-	}
-	else
-	{
-		LOGE("Not supported color format : %d.", color->format);
-	}
+		cv::Mat tangoImage;
+		if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+		{
+			tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+		}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+		{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+		{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
+		else if(color->format == 35)
+		{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
+		else
+		{
+			LOGE("Not supported color format : %d.", color->format);
+		}
 
-	if(!tangoImage.empty())
-	{
-		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+		if(!tangoImage.empty())
+		{
+			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+		}
 	}
 }
 
@@ -81,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);
 	}
 }
 
@@ -94,12 +104,12 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-CameraTango::CameraTango(int decimation, bool autoExposure) :
-		Camera(0),
+CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
+		CameraMobile(smoothing),
 		tango_config_(0),
-		firstFrame_(true),
+		colorCamera_(colorCamera),
 		decimation_(decimation),
-		autoExposure_(autoExposure),
+		rawScanPublished_(publishRawScan),
 		cloudStamp_(0),
 		tangoColorType_(0),
 		tangoColorStamp_(0)
@@ -112,11 +122,70 @@ CameraTango::~CameraTango() {
 	close();
 }
 
+// Compute fisheye distorted coordinates from undistorted coordinates.
+// The distortion model used by the Tango fisheye camera is called FOV and is
+// described in 'Straight lines have to be straight' by Frederic Devernay and
+// Olivier Faugeras. See https://hal.inria.fr/inria-00267247/document.
+// Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
+void applyFovModel(
+    double xu, double yu, double w, double w_inverse, double two_tan_w_div_two,
+    double* xd, double* yd) {
+  double ru = sqrt(xu * xu + yu * yu);
+  constexpr double epsilon = 1e-7;
+  if (w < epsilon || ru < epsilon) {
+    *xd = xu;
+    *yd = yu ;
+  } else {
+    double rd_div_ru = std::atan(ru * two_tan_w_div_two) * w_inverse / ru;
+    *xd = xu * rd_div_ru;
+    *yd = yu * rd_div_ru;
+  }
+}
+// Compute the warp maps to undistort the Tango fisheye image using the FOV
+// model. See OpenCV documentation for more information on warp maps:
+// http://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html
+// Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
+// @param fisheyeModel the fisheye camera intrinsics.
+// @param mapX the output map for the x direction.
+// @param mapY the output map for the y direction.
+void initFisheyeRectificationMap(
+    const CameraModel& fisheyeModel,
+    cv::Mat & mapX, cv::Mat & mapY) {
+  const double & fx = fisheyeModel.K().at<double>(0,0);
+  const double & fy = fisheyeModel.K().at<double>(1,1);
+  const double & cx = fisheyeModel.K().at<double>(0,2);
+  const double & cy = fisheyeModel.K().at<double>(1,2);
+  const double & w = fisheyeModel.D().at<double>(0,0);
+  mapX.create(fisheyeModel.imageSize(), CV_32FC1);
+  mapY.create(fisheyeModel.imageSize(), CV_32FC1);
+  LOGD("initFisheyeRectificationMap: fx=%f fy=%f, cx=%f, cy=%f, w=%f", fx, fy, cx, cy, w);
+  // Pre-computed variables for more efficiency.
+  const double fy_inverse = 1.0 / fy;
+  const double fx_inverse = 1.0 / fx;
+  const double w_inverse = 1 / w;
+  const double two_tan_w_div_two = 2.0 * std::tan(w * 0.5);
+  // Compute warp maps in x and y directions.
+  // OpenCV expects maps from dest to src, i.e. from undistorted to distorted
+  // pixel coordinates.
+  for(int iu = 0; iu < fisheyeModel.imageHeight(); ++iu) {
+    for (int ju = 0; ju < fisheyeModel.imageWidth(); ++ju) {
+      double xu = (ju - cx) * fx_inverse;
+      double yu = (iu - cy) * fy_inverse;
+      double xd, yd;
+      applyFovModel(xu, yu, w, w_inverse, two_tan_w_div_two, &xd, &yd);
+      double jd = cx + xd * fx;
+      double id = cy + yd * fy;
+      mapX.at<float>(iu, ju) = jd;
+      mapY.at<float>(iu, ju) = id;
+    }
+  }
+}
+
 bool CameraTango::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	close();
 
-	TangoSupport_initializeLibrary();
+	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
 
 	// Connect to Tango
 	LOGI("NativeRTABMap: Setup tango config");
@@ -136,38 +205,15 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
-	// Enable color.
-	ret = TangoConfig_setBool(tango_config_, "config_enable_color_camera", true);
-	if (ret != TANGO_SUCCESS)
+	if(colorCamera_)
 	{
-		LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
-		return false;
-	}
-
-	// disable auto exposure (disabled, seems broken on latest Tango releases)
-	ret = TangoConfig_setBool(tango_config_, "config_color_mode_auto", autoExposure_);
-	if (ret != TANGO_SUCCESS)
-	{
-		LOGE("NativeRTABMap: config_color_mode_auto() failed with error code: %d", ret);
-		//return false;
-	}
-	else
-	{
-		if(!autoExposure_)
+		// Enable color.
+		ret = TangoConfig_setBool(tango_config_, "config_enable_color_camera", true);
+		if (ret != TANGO_SUCCESS)
 		{
-			ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
-			if (ret != TANGO_SUCCESS)
-			{
-				LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret);
-				return false;
-			}
+			LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
+			return false;
 		}
-		bool verifyAutoExposureState;
-		int32_t verifyIso, verifyExp;
-		TangoConfig_getBool( tango_config_, "config_color_mode_auto", &verifyAutoExposureState );
-		TangoConfig_getInt32( tango_config_, "config_color_iso", &verifyIso );
-		TangoConfig_getInt32( tango_config_, "config_color_exp", &verifyExp );
-		LOGI( "NativeRTABMap: config_color autoExposure=%s %d %d", verifyAutoExposureState?"On" : "Off", verifyIso, verifyExp );
 	}
 
 	// Enable depth.
@@ -232,7 +278,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
-	ret = TangoService_connectOnFrameAvailable(TANGO_CAMERA_COLOR, this, onFrameAvailableRouter);
+	ret = TangoService_connectOnFrameAvailable(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, this, onFrameAvailableRouter);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: Failed to connect to color callback with error code: %d", ret);
@@ -281,7 +327,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	//
 	// Get color camera with respect to device transformation matrix.
 	frame_pair.base = TANGO_COORDINATE_FRAME_DEVICE;
-	frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_COLOR;
+	frame_pair.target = colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE;
 	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
 	if (ret != TANGO_SUCCESS)
 	{
@@ -296,31 +342,73 @@ 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;
-	ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics);
+	ret = TangoService_getCameraIntrinsics(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, &color_camera_intrinsics);
 	if (ret != TANGO_SUCCESS)
 	{
 		LOGE("NativeRTABMap: Failed to get the intrinsics for the color camera with error code: %d.", ret);
 		return false;
 	}
-	model_ = CameraModel(
+
+	LOGD("Calibration: fx=%f fy=%f cx=%f cy=%f width=%d height=%d",
 			color_camera_intrinsics.fx,
 			color_camera_intrinsics.fy,
 			color_camera_intrinsics.cx,
 			color_camera_intrinsics.cy,
-			this->getLocalTransform());
-	model_.setImageSize(cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height));
+			color_camera_intrinsics.width,
+			color_camera_intrinsics.height);
 
-	// device to camera optical rotation in rtabmap frame
-	model_.setLocalTransform(tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_);
+	cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+	K.at<double>(0,0) = color_camera_intrinsics.fx;
+	K.at<double>(1,1) = color_camera_intrinsics.fy;
+	K.at<double>(0,2) = color_camera_intrinsics.cx;
+	K.at<double>(1,2) = color_camera_intrinsics.cy;
+	cv::Mat D = cv::Mat::zeros(1, 5, CV_64FC1);
+	LOGD("Calibration type = %d", color_camera_intrinsics.calibration_type);
+	if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_5_PARAMETERS ||
+			color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_EQUIDISTANT)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = color_camera_intrinsics.distortion[2];
+		D.at<double>(0,3) = color_camera_intrinsics.distortion[3];
+		D.at<double>(0,4) = color_camera_intrinsics.distortion[4];
+	}
+	else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_3_PARAMETERS)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = 0.;
+		D.at<double>(0,3) = 0.;
+		D.at<double>(0,4) = color_camera_intrinsics.distortion[2];
+	}
+	else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_2_PARAMETERS)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = 0.;
+		D.at<double>(0,3) = 0.;
+		D.at<double>(0,4) = 0.;
+	}
 
-	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
-	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+	cv::Mat P;
 
-	cameraStartedTime_.restart();
+	LOGD("Distortion params: %f, %f, %f, %f, %f", D.at<double>(0,0), D.at<double>(0,1), D.at<double>(0,2), D.at<double>(0,3), D.at<double>(0,4));
+	model_ = CameraModel(colorCamera_?"color":"fisheye",
+			cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
+			K, D, R, P,
+			deviceTColorCamera_);
 
+	if(!colorCamera_)
+	{
+		initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
+	}
+
+	LOGI("deviceTColorCameraRtabmap  =%s", deviceTColorCamera_.prettyPrint().c_str());
 	return true;
 }
 
@@ -330,48 +418,47 @@ void CameraTango::close()
 	{
 		TangoConfig_free(tango_config_);
 		tango_config_ = nullptr;
+		LOGI("TangoService_disconnect()");
 		TangoService_disconnect();
+		LOGI("TangoService_disconnect() done.");
 	}
-	firstFrame_ = true;
+	fisheyeRectifyMapX_ = cv::Mat();
+	fisheyeRectifyMapY_ = cv::Mat();
+
+	CameraMobile::close();
 }
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 {
-	if(this->isRunning())
+	if(this->isRunning() && !cloud.empty())
 	{
+		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
+
 		UASSERT(cloud.type() == CV_32FC4);
 		boost::mutex::scoped_lock  lock(dataMutex_);
 
-		bool notify = cloud_.empty();
-		cloud_ = cloud.clone();
-		cloudStamp_ = timestamp;
-		LOGD("Depth received! (%d points)", cloud.cols);
-		if(!tangoColor_.empty() && notify)
+		// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
+		// "In the current version of Project Tango Tablet RGB IR camera
+		//  is used for both depth and color images and it can only do one
+		//  or the other for each frame. So in the stream we get 4 RGB frames
+		//  followed by 1 Depth frame resulting in the pattern you observed. This
+		//  is more of a hardware limitation."
+		//
+		//  So, synchronize with the last RGB frame before the Depth is acquired
+		if(!tangoColor_.empty())
 		{
-			LOGD("Cloud: Release semaphore");
-			dataReady_.release();
-		}
-	}
-}
+			double dt = fabs(timestamp - tangoColorStamp_);
 
-void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
-{
-	if(this->isRunning())
-	{
-		boost::mutex::scoped_lock  lock(dataMutex_);
+			//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
 
-		if(!cloud_.empty())
-		{
-			if(!tangoImage.empty())
+			if(dt >= 0.0 && dt < 0.5)
 			{
-				bool notify = tangoColor_.empty();
-				tangoColor_ = tangoImage.clone();
-				tangoColorStamp_ = timestamp;
-				tangoColorType_ = type;
-				LOGD("RGB received!");
-				if(!cloud_.empty() && notify)
+				bool notify = cloud_.empty();
+				cloud_ = cloud.clone();
+				cloudStamp_ = timestamp;
+				if(notify)
 				{
-					LOGD("RGB: Release semaphore");
+					//LOGD("Cloud: Release semaphore");
 					dataReady_.release();
 				}
 			}
@@ -379,27 +466,23 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	}
 }
 
-static rtabmap::Transform opticalRotationTango(
-								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)
+void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
 {
-	if(!pose.isNull() && pose.getNormSquared() < 100000)
+	if(this->isRunning() && !tangoImage.empty())
 	{
-		// send pose of the camera (without optical rotation), not the device
-		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotationTango));
+		//LOGD("RGB received! %fs", timestamp);
+
+		boost::mutex::scoped_lock  lock(dataMutex_);
+
+		tangoColor_ = tangoImage.clone();
+		tangoColorStamp_ = timestamp;
+		tangoColorType_ = type;
 	}
 }
 
 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
@@ -450,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;
@@ -458,7 +541,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 
 SensorData CameraTango::captureImage(CameraInfo * info)
 {
-	LOGI("Capturing image...");
+	//LOGI("Capturing image...");
 
 	SensorData data;
 	if(!dataReady_.acquire(1, 2000))
@@ -466,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())
@@ -503,6 +586,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			tangoColorType_ = 0;
 		}
 
+		LOGD("tangoColorType=%d", tangoColorType);
 		if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 		{
 			cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
@@ -515,6 +599,10 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
 		}
+		else if(tangoColorType == 35)
+		{
+			cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
+		}
 		else
 		{
 			LOGE("Not supported color format : %d.", tangoColorType);
@@ -527,10 +615,22 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		//}
 
 		CameraModel model = model_;
-		if(decimation_ > 1)
+
+		if(colorCamera_)
 		{
-			rgb = util2d::decimate(rgb, decimation_);
-			model = model.scaled(1.0/double(decimation_));
+			if(decimation_ > 1)
+			{
+				rgb = util2d::decimate(rgb, decimation_);
+				model = model.scaled(1.0/double(decimation_));
+			}
+		}
+		else
+		{
+			//UTimer t;
+			cv::Mat rgbRect;
+			cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
+			rgb = rgbRect;
+			//LOGD("Rectification time=%fs", t.ticks());
 		}
 
 		// Querying the depth image's frame transformation based on the depth image's
@@ -543,7 +643,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		Transform colorToDepth;
 		TangoPoseData pose_color_image_t1_T_depth_image_t0;
 		if (TangoSupport_calculateRelativePose(
-				rgbStamp, TANGO_COORDINATE_FRAME_CAMERA_COLOR, cloudStamp,
+				rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
 			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
 			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
 		{
@@ -567,47 +667,90 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			// The Color Camera frame at timestamp t0 with respect to Depth
 			// Camera frame at timestamp t1.
-			LOGI("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+			//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+			LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
 
 			int pixelsSet = 0;
-			depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
-			CameraModel depthModel = model_.scaled(1.0f/8.0f);
-			std::vector<cv::Point3f> scanData(cloud.total());
+			int depthSizeDec = colorCamera_?8:1;
+			depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
+			CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
+			std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
 			int oi=0;
+			int closePoints = 0;
+			float closeROI[4];
+			closeROI[0] = depth.cols/4;
+			closeROI[1] = 3*(depth.cols/4);
+			closeROI[2] = depth.rows/4;
+			closeROI[3] = 3*(depth.rows/4);
+			unsigned short minDepthValue=10000;
 			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
 				float * p = cloud.ptr<float>(0,i);
 				cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
 
-				if(pt.z > 0.0f && i%scanDownsampling == 0)
+				if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
 				{
 					scanData.at(oi++) = pt;
 				}
 
-				int pixel_x, pixel_y;
+				int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
 				// get the coordinate on image plane.
-				pixel_x = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
-				pixel_y = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+				pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
+				pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+				pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
+				pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
 				unsigned short depth_value(pt.z * 1000.0f);
 
-				if(pixel_x>=0 && pixel_x<depth.cols &&
-				   pixel_y>0 && pixel_y<depth.rows &&
+				if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
+				   pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
+				   depth_value < 600)
+				{
+					++closePoints;
+					if(depth_value < minDepthValue)
+					{
+						minDepthValue = depth_value;
+					}
+				}
+
+				bool pixelSet = false;
+				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
 				   depth_value)
 				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y, pixel_x);
+					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
 					if(depthPixel == 0 || depthPixel > depth_value)
 					{
 						depthPixel = depth_value;
-						pixelsSet += 1;
+						pixelSet = true;
 					}
 				}
+				if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+				   pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
+				   depth_value)
+				{
+					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+					if(depthPixel == 0 || depthPixel > depth_value)
+					{
+						depthPixel = depth_value;
+						pixelSet = true;
+					}
+				}
+				if(pixelSet)
+				{
+					pixelsSet += 1;
+				}
+			}
+
+			if(closePoints > 100)
+			{
+				this->post(new CameraInfoEvent(0, "TooClose", ""));
 			}
 
 			if(oi)
 			{
 				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
 			}
-			LOGI("pixels depth set= %d", pixelsSet);
+			//LOGD("pixels depth set= %d", pixelsSet);
 		}
 		else
 		{
@@ -618,20 +761,32 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
 
-			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
+			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());
+			//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());
+			//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());
 
-			data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);
-			data.setGroundTruth(odom);
+			Transform scanLocalTransform = model.localTransform();
+
+			if(rawScanPublished_)
+			{
+				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+			}
+			else
+			{
+				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+			}
+			info->odomPose = odom;
 		}
 		else
 		{
@@ -642,40 +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());
-			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
-			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
-			firstFrame_ = false;
-		}
-		else if(!this->isKilled())
-		{
-			LOGW("Odometry lost");
-			this->post(new OdometryEvent());
-		}
-	}
-	else
-	{
-		UERROR("Camera not initialized, cannot start thread.");
-		this->kill();
-	}
-}
-
 } /* namespace rtabmap */

app/android/jni/CameraTango.h

@@ -28,7 +28,9 @@ 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>
 #include <rtabmap/utilite/USemaphore.h>
 #include <rtabmap/utilite/UEventsSender.h>
@@ -36,55 +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>
-
-class TangoPoseData;
+#include <tango_client_api.h>
+#include <tango_support_api.h>
 
 namespace rtabmap {
 
-class PoseEvent: public UEvent
-{
+class CameraTango : public CameraMobile {
 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:
-	CameraTango(int decimation, bool autoExposure);
+	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
-	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 setAutoExposure(bool enabled) {autoExposure_ = enabled;}
+	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 
 	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:
@@ -93,15 +66,11 @@ protected:
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
 
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-
 private:
 	void * tango_config_;
-	bool firstFrame_;
-	UTimer cameraStartedTime_;
+	bool colorCamera_;
 	int decimation_;
-	bool autoExposure_;
+	bool rawScanPublished_;
 	cv::Mat cloud_;
 	double cloudStamp_;
 	cv::Mat tangoColor_;
@@ -109,8 +78,8 @@ private:
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	CameraModel model_;
-	Transform deviceTColorCamera_;
+	cv::Mat fisheyeRectifyMapX_;
+	cv::Mat fisheyeRectifyMapY_;
 };
 
 } /* namespace rtabmap */

app/android/jni/ProgressionStatus.h

@@ -0,0 +1,124 @@
+/*
+ * ProgressionStatus.h
+ *
+ *  Created on: Feb 28, 2017
+ *      Author: mathieu
+ */
+
+#ifndef APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+#define APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+
+#include <rtabmap/core/ProgressState.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <jni.h>
+
+namespace rtabmap {
+
+class ProgressEvent : public UEvent
+{
+public:
+	ProgressEvent(int count = 1) : count_(count){}
+	virtual std::string getClassName() const {return "ProgressEvent";}
+
+	int count_;
+};
+
+class ProgressionStatus: public ProgressState, public UEventsHandler
+{
+public:
+	ProgressionStatus() : count_(0), max_(100), jvm_(0), rtabmap_(0)
+	{
+		registerToEventsManager();
+	}
+
+	void setJavaObjects(JavaVM * jvm, jobject rtabmap)
+	{
+		jvm_ = jvm;
+		rtabmap_ = rtabmap;
+	}
+
+	void reset(int max)
+	{
+		count_=-1;
+		max_ = max;
+		setCanceled(false);
+
+		increment();
+	}
+
+	void setMax(int max)
+	{
+		max_ = max;
+	}
+	int getMax() const {return max_;}
+
+	void increment(int count = 1) const
+	{
+		UEventsManager::post(new ProgressEvent(count));
+	}
+
+	void finish()
+	{
+
+	}
+
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!isCanceled())
+		{
+			increment();
+		}
+
+		return ProgressState::callback(msg);
+	}
+	virtual ~ProgressionStatus(){}
+
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("ProgressEvent") == 0)
+		{
+			count_ += ((ProgressEvent*)event)->count_;
+			// Call JAVA callback
+			bool success = false;
+			if(jvm_ && rtabmap_)
+			{
+				JNIEnv *env = 0;
+				jint rs = jvm_->AttachCurrentThread(&env, NULL);
+				if(rs == JNI_OK && env)
+				{
+					jclass clazz = env->GetObjectClass(rtabmap_);
+					if(clazz)
+					{
+						jmethodID methodID = env->GetMethodID(clazz, "updateProgressionCallback", "(II)V" );
+						if(methodID)
+						{
+							env->CallVoidMethod(rtabmap_, methodID,
+									count_,
+									max_);
+							success = true;
+						}
+					}
+				}
+				jvm_->DetachCurrentThread();
+			}
+			if(!success)
+			{
+				UERROR("Failed to call rtabmap::updateProgressionCallback");
+			}
+		}
+		return false;
+	}
+
+private:
+	int count_;
+	int max_;
+	JavaVM *jvm_;
+	jobject rtabmap_;
+};
+
+}
+
+
+#endif /* APP_ANDROID_JNI_PROGRESSIONSTATUS_H_ */

app/android/jni/RTABMapApp.h

@@ -31,53 +31,32 @@ 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 <util.h>
+#include "scene.h"
+#include "CameraMobile.h"
+#include "util.h"
+#include "ProgressionStatus.h"
 
 #include <rtabmap/core/RtabmapThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <boost/thread/mutex.hpp>
 #include <pcl/pcl_base.h>
+#include <pcl/TextureMesh.h>
 
 // RTABMapApp handles the application lifecycle and resources.
 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);
 
-  void openDatabase(const std::string & databasePath = "");
+  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 isBuiltWith(int cameraDriver) const;
+  bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
 
   // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
   void InitializeGLContent();
@@ -88,8 +67,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.
   //
@@ -110,90 +88,166 @@ class RTABMapApp : public UEventsHandler {
                     float x0, float y0, float x1, float y1);
 
   void setPausedMapping(bool paused);
+  void setOnlineBlending(bool enabled);
   void setMapCloudShown(bool shown);
   void setOdomCloudShown(bool shown);
   void setMeshRendering(bool enabled, bool withTexture);
+  void setPointSize(float value);
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float value);
+  void setLighting(bool enabled);
+  void setBackfaceCulling(bool enabled);
+  void setWireframe(bool enabled);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
   void setNodesFiltering(bool enabled);
-  void setDriftCorrection(bool enabled);
   void setGraphVisible(bool visible);
   void setGridVisible(bool visible);
-  void setAutoExposure(bool enabled);
+  void setRawScanSaved(bool enabled);
+  void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
+  void setSmoothing(bool enabled);
   void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
-  void setMeshDecimation(int value);
+  void setMinCloudDepth(float value);
+  void setCloudDensityLevel(int value);
   void setMeshAngleTolerance(float value);
   void setMeshTriangleSize(int value);
+  void setClusterRatio(float value);
+  void setMaxGainRadius(float value);
+  void setRenderingTextureDecimation(int value);
+  void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
+  void setGPS(const rtabmap::GPS & gps);
+  void addEnvSensor(int type, float value);
 
-  void resetMapping();
   void save(const std::string & databasePath);
-  bool exportMesh(const std::string & filePath);
+  void cancelProcessing();
+  bool exportMesh(
+		  float cloudVoxelSize,
+		  bool regenerateCloud,
+		  bool meshing,
+		  int textureSize,
+		  int textureCount,
+		  int normalK,
+		  bool optimized,
+		  float optimizedVoxelSize,
+		  int optimizedDepth,
+		  int optimizedMaxPolygons,
+		  float optimizedColorRadius,
+		  bool optimizedCleanWhitePolygons,
+		  int optimizedMinClusterSize,
+		  float optimizedMaxTextureDistance,
+		  int optimizedMinTextureClusterSize,
+		  bool blockRendering);
+  bool postExportation(bool visualize);
+  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 * yPlane, void * uPlane, void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+  		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		float * points, int pointsLen);
+
  protected:
-  virtual void handleEvent(UEvent * event);
+  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_;
   bool nodesFiltering_;
-  bool driftCorrection_;
   bool localizationMode_;
   bool trajectoryMode_;
-  bool autoExposure_;
+  bool rawScanSaved_;
+  bool smoothing_;
+  bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;
   float maxCloudDepth_;
-  int meshDecimation_;
+  float minCloudDepth_;
+  int cloudDensityLevel_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
+  float clusterRatio_;
+  float maxGainRadius_;
+  int renderingTextureDecimation_;
+  float backgroundColor_;
 
   rtabmap::ParametersMap mappingParameters_;
 
-  bool paused_;
   bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
+  bool openingDatabase_;
+  bool exporting_;
+  bool postProcessing_;
   bool filterPolygonsOnNextRender_;
   int gainCompensationOnNextRender_;
   bool bilateralFilteringOnNextRender_;
+  bool takeScreenshotOnNextRender_;
   bool cameraJustInitialized_;
+  int meshDecimation_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
   float renderingTime_;
+  double lastPostRenderEventTime_;
+  double lastPoseEventTime_;
+  std::map<std::string, float> bufferedStatsData_;
+
+  bool visualizingMesh_;
+  bool exportedMeshUpdated_;
+  pcl::TextureMesh::Ptr optMesh_;
+  cv::Mat optTexture_;
+  int optRefId_;
+  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
   Scene main_scene_;
 
-	std::list<rtabmap::Statistics> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 
+	rtabmap::Transform mapToOdom_;
+
+	boost::mutex cameraMutex_;
 	boost::mutex rtabmapMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;
 
-	std::map<int, Mesh> createdMeshes_;
+	USemaphore screenshotReady_;
+
+	std::map<int, rtabmap::Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
+
+	rtabmap::ProgressionStatus progressionStatus_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

app/android/jni/bounding_box_drawable.h

@@ -0,0 +1,131 @@
+/*
+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 BOUNDING_BOX_DRAWABLE_H_
+#define BOUNDING_BOX_DRAWABLE_H_
+
+#include "tango-gl/line.h"
+
+class BoundingBoxDrawable : public tango_gl::Line
+{
+public:
+	BoundingBoxDrawable() :
+		Line(3.0f, GL_LINES)
+	{
+		vec_vertices_.resize(24);
+	}
+
+	void updateVertices(const pcl::PointXYZ & min, const pcl::PointXYZ & max)
+	{
+		int index = 0;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+	}
+
+};
+#endif  // TANGO_GL_LINE_H_

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,232 +46,859 @@ 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_openEmptyDatabase(
-    JNIEnv* env, jobject)
+Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(
+    JNIEnv* env, jclass, jlong native_application, int displayRotation, int cameraRotation)
 {
-	return app.openDatabase();
+	if(native_application)
+	{
+		return native(native_application)->setScreenRotation(displayRotation, cameraRotation);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
 }
 
-JNIEXPORT void JNICALL
+JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
-    JNIEnv* env, jobject, jstring databasePath)
+    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize)
 {
 	std::string databasePathC;
 	GetJStringContent(env,databasePath,databasePathC);
-	return app.openDatabase(databasePathC);
+	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, jclass, jlong native_application, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+{
+	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_isBuiltWith(
+		JNIEnv* env, jclass, jlong native_application, int cameraDriver) {
+	if(native_application)
+	{
+		return native(native_application)->isBuiltWith(cameraDriver);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
+}
+
+JNIEXPORT bool JNICALL
+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*, jclass, jlong native_application, bool 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*, jclass, jlong native_application, float 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*, jclass, jlong native_application, float 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*, jclass, jlong native_application, float 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*, jclass, jlong native_application, float 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*, jclass, jlong native_application, bool 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*, jclass, jlong native_application, bool 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*, jclass, jlong native_application, bool 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);
-}
-JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_setDriftCorrection(
-		JNIEnv*, jobject, bool enabled)
-{
-	return app.setDriftCorrection(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_setAutoExposure(
-		JNIEnv*, jobject, bool enabled)
+Java_com_introlab_rtabmap_RTABMapLib_setRawScanSaved(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
-	return app.setAutoExposure(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*, jclass, jlong native_application, bool 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*, jclass, jlong native_application, bool 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_setMeshDecimation(
-		JNIEnv*, jobject, int value)
+Java_com_introlab_rtabmap_RTABMapLib_setMinCloudDepth(
+		JNIEnv*, jclass, jlong native_application, float value)
 {
-	return app.setMeshDecimation(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*, jclass, jlong native_application, int 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*, jclass, jlong native_application, float 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*, jclass, jlong native_application, float 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*, jclass, jlong native_application, int 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*, jclass, jlong native_application, float 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_resetMapping(
-		JNIEnv*, jobject)
+Java_com_introlab_rtabmap_RTABMapLib_setGPS(
+		JNIEnv*, jclass, jlong native_application,
+		double stamp,
+		double longitude,
+		double latitude,
+		double altitude,
+		double accuracy,
+		double bearing)
 {
-	return app.resetMapping();
+	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*, jclass, jlong native_application,
+		int type,
+		float value)
+{
+	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, jclass, jlong native_application)
+{
+	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, jstring filePath)
+		JNIEnv* env, jclass, jlong native_application,
+		float cloudVoxelSize,
+		bool regenerateCloud,
+		bool meshing,
+		int textureSize,
+		int textureCount,
+		int normalK,
+		bool optimized,
+		float optimizedVoxelSize,
+		int optimizedDepth,
+		int optimizedMaxPolygons,
+		float optimizedColorRadius,
+		bool optimizedCleanWhitePolygons,
+		int optimizedMinClusterSize,
+		float optimizedMaxTextureDistance,
+		int optimizedMinTextureClusterSize,
+		bool blockRendering)
 {
-	std::string filePathC;
-	GetJStringContent(env,filePath,filePathC);
-	return app.exportMesh(filePathC);
+	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, jclass, jlong native_application, bool 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, jclass, jlong native_application, jstring directory, jstring name)
+{
+	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 yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		jobject points, int pointsLen)
+{
+	if(native_application)
+	{
+		void *yPtr = env->GetDirectBufferAddress(yPlane);
+		void *uPtr = env->GetDirectBufferAddress(uPlane);
+		void *vPtr = env->GetDirectBufferAddress(vPlane);
+		void *depthPtr = env->GetDirectBufferAddress(depth);
+		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
+		native(native_application)->postOdometryEvent(
+				x,y,z,qx,qy,qz,qw,
+				fx,fy,cx,cy,
+				stamp,
+				yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
+				depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
+				pointsPtr, pointsLen);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return;
+	}
 }
 
 

app/android/jni/point_cloud_drawable.h

@@ -40,28 +40,41 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 // PointCloudDrawable is responsible for the point cloud rendering.
 class PointCloudDrawable {
+public:
+	static void createShaderPrograms();
+	static void releaseShaderPrograms();
+
+private:
+	static std::vector<GLuint> shaderPrograms_;
+
  public:
   PointCloudDrawable(
-		  GLuint cloudShaderProgram,
-		  GLuint textureShaderProgram,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		  const pcl::IndicesPtr & indices,
-		  float gain);
+		  float gainR = 1.0f,
+		  float gainG = 1.0f,
+		  float gainB = 1.0f);
   PointCloudDrawable(
-  		  GLuint cloudShaderProgram,
-  		  GLuint textureShaderProgram,
-    	  const Mesh & mesh,
-		  const cv::Mat & texture);
+    	  const rtabmap::Mesh & mesh,
+		  bool createWireframe = false);
   virtual ~PointCloudDrawable();
 
-  void updatePolygons(const std::vector<pcl::Vertices> & polygons);
-  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
-  void updateMesh(const Mesh & mesh, const cv::Mat & texture);
+  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 rtabmap::Mesh & mesh, bool createWireframe = false);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
-  rtabmap::Transform getPose() const {return glmToTransform(pose_);}
+  void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
+  rtabmap::Transform getPose() const {return pose_;}
+  const glm::mat4 & getPoseGl() const {return poseGl_;}
   bool isVisible() const {return visible_;}
+  bool hasMesh() const {return polygons_.size()!=0;}
   bool hasTexture() const {return textures_ != 0;}
+  float getMinHeight() const {return minHeight_;}
+  const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
+  const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
+  const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
+  const pcl::PointXYZ & aabbMaxWorld() const {return aabbMaxWorld_;}
 
   // Update current point cloud data.
   //
@@ -69,22 +82,61 @@ class PointCloudDrawable {
   // @param view_mat: view matrix from current render camera.
   // @param model_mat: model matrix for this point cloud frame.
   // @param vertices: all vertices in this point cloud frame.
-  void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f, bool textureRendering = false);
+  void Render(
+		  const glm::mat4 & projectionMatrix,
+		  const glm::mat4 & viewMatrix,
+		  bool meshRendering = true,
+		  float pointSize = 3.0f,
+		  bool textureRendering = false,
+		  bool lighting = true,
+		  float distanceToCamSqr = 0.0f,
+		  const GLuint & depthTexture = 0,
+		  int screenWidth = 0,     // nonnull if depthTexture>0
+		  int screenHeight = 0,    // nonnull if depthTexture>0
+		  float nearClipPlane = 0, // nonnull if depthTexture>0
+		  float farClipPlane = 0,  // nonnull if depthTexture>0
+		  bool packDepthToColorChannel = false,
+		  bool wireFrame = false) const;
+
+ private:
+  template<class PointT>
+  void updateAABBMinMax(const PointT & pt, pcl::PointXYZ & min, pcl::PointXYZ & max)
+  {
+	  if(pt.x<min.x) min.x = pt.x;
+	  if(pt.y<min.y) min.y = pt.y;
+	  if(pt.z<min.z) min.z = pt.z;
+	  if(pt.x>max.x) max.x = pt.x;
+	  if(pt.y>max.y) max.y = pt.y;
+	  if(pt.z>max.z) max.z = pt.z;
+  }
+  void updateAABBWorld(const rtabmap::Transform & pose);
 
  private:
   // Vertex buffer of the point cloud geometry.
   GLuint vertex_buffers_;
   GLuint textures_;
-  std::vector<GLushort> polygons_;
+  std::vector<GLuint> polygons_;
+  std::vector<GLuint> polygonsLowRes_;
+  std::vector<GLuint> polygonLines_;
+  std::vector<GLuint> polygonLinesLowRes_;
+  std::vector<GLuint> verticesLowRes_;
+  std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
-  glm::mat4 pose_;
+  rtabmap::Transform pose_;
+  glm::mat4 poseGl_;
   bool visible_;
-  std::vector<int> organizedToDenseIndices_;
+  bool hasNormals_;
+  std::vector<unsigned int> organizedToDenseIndices_;
+  float minHeight_; // odom frame
 
-  GLuint cloud_shader_program_;
-  GLuint texture_shader_program_;
+  float gainR_;
+  float gainG_;
+  float gainB_;
 
-  float gain_;
+  pcl::PointXYZ aabbMinModel_;
+  pcl::PointXYZ aabbMaxModel_;
+  pcl::PointXYZ aabbMinWorld_;
+  pcl::PointXYZ aabbMaxWorld_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

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>
@@ -37,6 +37,7 @@
 
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
+#include <bounding_box_drawable.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -56,6 +57,11 @@ class Scene {
 
   // Setup GL view port.
   void SetupViewPort(int w, int h);
+  int getViewPortWidth() const {return screenWidth_;}
+  int getViewPortHeight() const {return screenHeight_;}
+
+  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
 
@@ -73,9 +79,9 @@ 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) const;
+  rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;
 
   // Touch event passed from android activity. This function only support two
   // touches.
@@ -96,6 +102,13 @@ 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);
+  bool hasMarker(int id) const;
+  void removeMarker(int id);
+  std::set<int> getAddedMarkers() const;
 
   void addCloud(
   		  int id,
@@ -104,24 +117,40 @@ class Scene {
   		  const rtabmap::Transform & pose);
   void addMesh(
   		int id,
-  		const Mesh & mesh,
-		const cv::Mat & texture,
-  		const rtabmap::Transform & pose);
+  		const rtabmap::Mesh & mesh,
+  		const rtabmap::Transform & pose,
+		bool createWireframe = false);
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
   bool hasCloud(int id) const;
+  bool hasMesh(int id) const;
   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, const cv::Mat & texture);
+  void updateMesh(int id, const rtabmap::Mesh & mesh);
+  void updateGains(int id, float gainR, float gainG, float gainB);
 
+  void setBlending(bool enabled) {blending_ = enabled;}
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float angleDeg);
+  void setLighting(bool enabled) {lighting_ = enabled;}
+  void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
+  void setWireframe(bool enabled) {wireFrame_ = enabled;}
+  void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
+  void setGridColor(float r, float g, float b);
 
+  bool isBlending() const {return blending_;}
+  bool isMapRendering() const {return mapRendering_;}
   bool isMeshRendering() const {return meshRendering_;}
   bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
+  float getPointSize() const {return pointSize_;}
+  bool isLighting() const {return lighting_;}
+  bool isBackfaceCulling() const {return backfaceCulling_;}
 
  private:
   // Camera object that allows user to use touch input to interact with.
@@ -136,26 +165,46 @@ class Scene {
   // Ground grid.
   tango_gl::Grid* grid_;
 
+  // Bounding box
+  BoundingBoxDrawable * box_;
+
   // Trace of pose data.
   tango_gl::Trace* trace_;
   GraphDrawable * graph_;
   bool graphVisible_;
   bool gridVisible_;
   bool traceVisible_;
+  bool frustumVisible_;
+
+  std::map<int, tango_gl::Axis*> markers_;
+
+  rtabmap::ScreenRotation color_camera_to_display_rotation_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
 
   rtabmap::Transform * currentPose_;
 
   // Shader to display point cloud.
-  GLuint cloud_shader_program_;
-  GLuint texture_mesh_shader_program_;
   GLuint graph_shader_program_;
 
+  bool blending_;
   bool mapRendering_;
   bool meshRendering_;
   bool meshRenderingTexture_;
   float pointSize_;
+  bool boundingBoxRendering_;
+  bool lighting_;
+  bool backfaceCulling_;
+  bool wireFrame_;
+  float r_;
+  float g_;
+  float b_;
+  GLuint fboId_;
+  GLuint depthTexture_;
+  GLsizei screenWidth_;
+  GLsizei screenHeight_;
+  bool doubleTapOn_;
+  cv::Point2f doubleTapPos_;
 };
 
 #endif  // TANGO_POINT_CLOUD_SCENE_H_

app/android/jni/tango-gl/camera.cpp

@@ -22,8 +22,13 @@ namespace tango_gl {
 Camera::Camera() {
   field_of_view_ = 45.0f * DEGREE_2_RADIANS;
   aspect_ratio_ = 4.0f / 3.0f;
-  near_clip_plane_ = 0.1f;
-  far_clip_plane_ = 100.0f;
+  width_ = 800.0f;
+  height_ = 600.0f;
+  near_clip_plane_ = 0.2f;
+  far_clip_plane_ = 1000.0f;
+  ortho_ = false;
+  orthoScale_ = 2.0f;
+  orthoCropFactor_ = -1.0f;
 }
 
 glm::mat4 Camera::GetViewMatrix() {
@@ -31,18 +36,29 @@ glm::mat4 Camera::GetViewMatrix() {
 }
 
 glm::mat4 Camera::GetProjectionMatrix() {
-  return glm::perspective(field_of_view_, aspect_ratio_,
-                          near_clip_plane_, far_clip_plane_);
+	if(ortho_)
+	{
+		return glm::ortho(-orthoScale_*aspect_ratio_, orthoScale_*aspect_ratio_, -orthoScale_, orthoScale_, orthoScale_ + orthoCropFactor_, far_clip_plane_);
+	}
+  return glm::perspective(field_of_view_, aspect_ratio_, near_clip_plane_, far_clip_plane_);
 }
 
-void Camera::SetAspectRatio(float aspect_ratio) {
-  aspect_ratio_ = aspect_ratio;
+void Camera::SetWindowSize(float width, float height) {
+	width_ = width;
+	height_ = height;
+  aspect_ratio_ = width/height;
 }
 
 void Camera::SetFieldOfView(float fov) {
   field_of_view_ = fov * DEGREE_2_RADIANS;
 }
 
+void Camera::SetNearFarClipPlanes(const float near, const float far)
+{
+	near_clip_plane_ = near;
+	far_clip_plane_ = far;
+}
+
 Camera::~Camera() {
 }
 

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();

app/android/jni/tango-gl/gesture_camera.cpp

@@ -62,68 +62,83 @@ GestureCamera::GestureCamera() :
 GestureCamera::~GestureCamera() { delete cam_parent_transform_; }
 
 void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
-                                 float y0, float x1, float y1) {
-  if (camera_type_ == kFirstPerson) {
-    return;
-  }
+		float y0, float x1, float y1) {
 
-  if (touch_count == 1) {
-    switch (event) {
-      case kTouch0Down: {
-        cam_start_angle_ = cam_cur_angle_;
+	if (camera_type_!=kFirstPerson && touch_count == 1) {
+		switch (event) {
+		case kTouch0Down: {
+			cam_start_angle_ = cam_cur_angle_;
 
-        touch0_start_position_.x = x0;
-        touch0_start_position_.y = y0;
-        break;
-      }
-      case kTouchMove: {
-        float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
-        float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
+			touch0_start_position_.x = x0;
+			touch0_start_position_.y = y0;
+			break;
+		}
+		case kTouchMove: {
+			glm::vec2 offset;
 
-        cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
-        cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
-        StartCameraToCurrentTransform();
-        break;
-      }
-      default: { break; }
-    }
-  }
-  if (touch_count == 2) {
-    switch (event) {
-      case kTouch1Down: {
-        float abs_x = x0 - x1;
-        float abs_y = y0 - y1;
-        start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
-        cam_start_dist_ = GetPosition().z;
+			float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
+			float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
 
-        // center touch
-        touch0_start_position_.x = (x0+x1)/2.0f;
-        touch0_start_position_.y = (y0+y1)/2.0f;
-        break;
-      }
-      case kTouchMove: {
-        float abs_x = x0 - x1;
-        float abs_y = y0 - y1;
-        float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
+			if(camera_type_!=kTopOrtho)
+				cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
+			cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
 
-        cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
-                                  kCamViewMinDist, kCamViewMaxDist);
+			StartCameraToCurrentTransform();
 
-        glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
-        glm::vec2 offset;
-        offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
-        offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
-        touch0_start_position_ = touch_center_position;
+			break;
+		}
+		default: { break; }
+		}
+	}
+	if (touch_count == 2) {
+		switch (event) {
+		case kTouch1Down: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
+			cam_start_dist_ = GetPosition().z;
+			cam_start_fov_ = this->getFOV();
 
-        StartCameraToCurrentTransform();
+			// center touch
+			touch0_start_position_.x = (x0+x1)/2.0f;
+			touch0_start_position_.y = (y0+y1)/2.0f;
+			break;
+		}
+		case kTouchMove: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
 
-        anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			if(camera_type_ == kFirstPerson)
+			{
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
+			}
+			else
+			{
+				cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+								kCamViewMinDist, kCamViewMaxDist);
 
-        break;
-      }
-      default: { break; }
-    }
-  }
+				this->SetOrthoMode(camera_type_ == kTopOrtho);
+				if(camera_type_ == kTopOrtho)
+				{
+					this->SetOrthoScale(cam_cur_dist_);
+				}
+
+				glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
+				glm::vec2 offset;
+				offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
+				offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
+				touch0_start_position_ = touch_center_position;
+
+				StartCameraToCurrentTransform();
+
+				anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			}
+			break;
+		}
+		default: { break; }
+		}
+	}
 }
 
 Segment GestureCamera::GetSegmentFromTouch(float normalized_x,
@@ -164,6 +179,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
   camera_type_ = camera_index;
   switch (camera_index) {
     case kFirstPerson:
+    	SetOrthoMode(false);
       SetFieldOfView(kLowFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
@@ -177,20 +193,22 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       break;
     case kThirdPerson:
     case kThirdPersonFollow:
-      SetFieldOfView(kHighFov);
+    	SetOrthoMode(false);
+    	SetFieldOfView(kHighFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 4.0f;
+      cam_cur_angle_.x = -M_PI / 6.0f;
       cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;
     case kTopDown:
-      SetFieldOfView(kHighFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      SetOrthoMode(false);
+      SetFieldOfView(kHighFov);
       cam_cur_dist_ = kTopDownCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
       cam_cur_angle_.x = -M_PI / 2.0f;
@@ -198,6 +216,19 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;
+    case kTopOrtho:
+	  SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+	  SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	  SetOrthoMode(true);
+	  SetOrthoScale(kTopDownCameraDist);
+	  SetOrthoCropFactor(-1.0f);
+	  cam_cur_dist_ = kTopDownCameraDist;
+	  anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+	  cam_cur_angle_.x = -M_PI / 2.0f;
+	  cam_cur_angle_.y = 0.0f;
+	  cam_cur_target_rot_ = glm::quat(1,0,0,0);
+	  StartCameraToCurrentTransform();
+	  break;
     default:
       break;
   }

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

app/android/jni/tango-gl/include/tango-gl/camera.h

@@ -27,11 +27,17 @@ class Camera : public Transform {
   Camera& operator=(const Camera&) = delete;
   ~Camera();
 
-  void SetAspectRatio(const float aspect_ratio);
+  void SetWindowSize(const float width, const float height);
   void SetFieldOfView(const float fov);
+  void SetOrthoMode(bool enabled) {ortho_ = enabled;}
+  void SetOrthoScale(float scale) {orthoScale_ = scale;}
+  void SetOrthoCropFactor(float value) {orthoCropFactor_ = value;}
+  void SetNearFarClipPlanes(const float near, const float far);
 
   glm::mat4 GetViewMatrix();
   glm::mat4 GetProjectionMatrix();
+  float getNearClipPlane() const {return near_clip_plane_;}
+  float getFarClipPlane() const {return far_clip_plane_;}
 
   /**
    * Create an OpenGL perspective matrix from window size, camera intrinsics, and clip settings.
@@ -52,7 +58,12 @@ class Camera : public Transform {
  protected:
   float field_of_view_;
   float aspect_ratio_;
+  float width_;
+  float height_;
   float near_clip_plane_, far_clip_plane_;
+  bool ortho_;
+  float orthoScale_;
+  float orthoCropFactor_;
 };
 }  // namespace tango_gl
 #endif  // TANGO_GL_CAMERA_H_

app/android/jni/tango-gl/include/tango-gl/gesture_camera.h

@@ -29,7 +29,8 @@ class GestureCamera : public Camera {
     kFirstPerson = 0,
     kThirdPersonFollow = 1,
     kTopDown = 2,
-    kThirdPerson = 3
+	kTopOrtho = 3,
+    kThirdPerson = 4
   };
 
   enum TouchEvent {
@@ -55,6 +56,11 @@ class GestureCamera : public Camera {
                               float touch_range);
 
   void SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation);
+  void SetAnchorOffset(const glm::vec3& pos) {anchor_offset_ = pos;}
+  const glm::vec3& GetAnchorOffset() const {return anchor_offset_;}
+
+  void SetCameraDistance(float cameraDistance) {cam_cur_dist_ = cameraDistance;}
+  float GetCameraDistance() const {return cam_cur_dist_;}
 
   // Set camera type, set render camera's parent position and rotation.
   void SetCameraType(CameraType camera_index);
@@ -75,6 +81,7 @@ class GestureCamera : public Camera {
   glm::quat cam_cur_target_rot_;
 
   float cam_start_dist_;
+  float cam_start_fov_;
   float cam_cur_dist_;
   glm::vec3 anchor_offset_;
 

app/android/jni/tango-gl/include/tango-gl/util.h

@@ -33,10 +33,17 @@
 #include "glm/gtx/matrix_decompose.hpp"
 
 #define LOG_TAG "rtabmap"
+#ifdef DISABLE_LOG
+#define LOGD(...) ;
+#define LOGI(...) ;
+#define LOGW(...) ;
+#else
 #define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
 #define LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
 #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
@@ -76,6 +83,7 @@ namespace util {
                         const glm::vec3& start, const glm::vec3& end);
 
   glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec);
+
 }  // namespace util
 }  // namespace tango_gl
 #endif  // TANGO_GL_RENDERER_GL_UTIL

app/android/jni/tango-gl/util.cpp

@@ -19,6 +19,27 @@
 
 namespace tango_gl {
 
+namespace {
+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;
+}
+}  // annonymous namespace
+
 void util::CheckGlError(const char* operation) {
   for (GLint error = glGetError(); error; error = glGetError()) {
     LOGE("after %s() glError (0x%x)\n", operation, error);

app/android/jni/third-party/lib/.gitignore

@@ -0,0 +1 @@
+ 

app/android/jni/util.h

@@ -39,44 +39,56 @@ 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:
 	LogHandler()
 	{
-		ULogger::setLevel(ULogger::kWarning);
-		ULogger::setEventLevel(ULogger::kWarning);
+		ULogger::setLevel(ULogger::kDebug);
+		ULogger::setEventLevel(ULogger::kDebug);
 		ULogger::setPrintThreadId(true);
 
 		registerToEventsManager();
 	}
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("ULogEvent") == 0)
 		{
 			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());
 			}
 
 		}
+		return false;
 	}
 };
 
+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,
@@ -87,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)
 {
@@ -143,15 +160,110 @@ inline rtabmap::Transform glmToTransform(const glm::mat4 & mat)
 	return transform;
 }
 
-struct Mesh
+class Mesh
 {
+public:
+	Mesh() :
+		cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
+		normals(new pcl::PointCloud<pcl::Normal>),
+		indices(new std::vector<int>),
+		visible(true)
+	{
+		gains[0] = gains[1] = gains[2] = 1.0f;
+	}
+
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
+	pcl::PointCloud<pcl::Normal>::Ptr normals;
 	pcl::IndicesPtr indices;
 	std::vector<pcl::Vertices> polygons;
+	std::vector<pcl::Vertices> polygonsLowRes;
 	rtabmap::Transform pose; // in rtabmap coordinates
 	bool visible;
 	rtabmap::CameraModel cameraModel;
-	float gain;
+	double gains[3]; // RGB gains
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+    	std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
+#else
+    	std::vector<Eigen::Vector2f> texCoords;
+#endif
+	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_ */

app/android/libs/.gitignore

@@ -0,0 +1 @@
+*.jar

app/android/res/drawable/custom_seekbar.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<layer-list xmlns:android="http://schemas.android.com/apk/res/android" >
+    <item>
+        <shape>
+            <gradient
+                android:endColor="#00ffffff"
+                android:startColor="#ff686868"
+                android:useLevel="false" />
+        </shape>
+    </item>
+
+</layer-list>

app/android/res/layout/activity_rtabmap.xml

@@ -15,259 +15,155 @@
 -->
 <RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
     android:layout_width="wrap_content"
-    android:layout_height="wrap_content" >
+    android:layout_height="wrap_content">
 
     <android.opengl.GLSurfaceView
         android:id="@+id/gl_surface_view"
         android:layout_width="fill_parent"
         android:layout_height="fill_parent"
-        android:layout_gravity="top" />
-
-   <LinearLayout
-        android:id="@+id/debug_layout"
+        android:layout_gravity="top"/>
+    
+    <RelativeLayout 
         android:layout_width="wrap_content"
-        android:layout_height="wrap_content"
-        android:layout_alignParentLeft="true"
-        android:layout_alignParentTop="true"
-        android:orientation="vertical"
-        android:paddingLeft="5dp" >
-
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/status" />
-
-            <TextView
-                android:id="@+id/status"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-               
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/nodes" />
-
-            <TextView
-                android:id="@+id/nodes"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
+        android:layout_height="match_parent"
+        android:fitsSystemWindows="true">
         
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/words" />
-
-            <TextView
-                android:id="@+id/words"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/memory" />
-
-            <TextView
-                android:id="@+id/memory"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/db_size" />
-
-            <TextView
-                android:id="@+id/db_size"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-         <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/points" />
-
-            <TextView
-                android:id="@+id/points"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/polygons" />
-
-            <TextView
-                android:id="@+id/polygons"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-         <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/update_time" />
-
-            <TextView
-                android:id="@+id/update_time"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/features" />
-
-            <TextView
-                android:id="@+id/features"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/total_loop" />
-
-            <TextView
-                android:id="@+id/total_loop"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/inliers" />
-
-            <TextView
-                android:id="@+id/inliers"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/hypothesis" />
-
-            <TextView
-                android:id="@+id/hypothesis"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/fps" />
-
-            <TextView
-                android:id="@+id/fps"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-           
-    </LinearLayout>
-
-    <Button
-        android:id="@+id/first_person_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/third_person_button"
-        android:layout_alignLeft="@+id/third_person_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:text="@string/first_person" />
-
-    <Button
-        android:id="@+id/top_down_button"
+    <ImageButton
+        android:id="@+id/start_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
         android:layout_alignParentBottom="true"
-        android:layout_alignParentRight="true"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:text="@string/top_down" />
+        android:layout_centerHorizontal="true"
+        android:src="@drawable/start96"
+        android:background="@null"
+        android:layout_marginBottom="10dp" />
 
-    <Button
-        android:id="@+id/third_person_button"
+    <ImageButton
+        android:id="@+id/stop_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/top_down_button"
+        android:layout_alignParentBottom="true"
+        android:layout_centerHorizontal="true"
+        android:src="@drawable/stop96"
+        android:background="@null"
+        android:layout_marginBottom="10dp" />
+    
+    <ToggleButton
+        android:id="@+id/wireframe_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/light_button"
+        android:layout_alignLeft="@+id/light_button"
         android:layout_alignParentRight="true"
         android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
-        android:text="@string/third_person" />
+        android:textOff="@string/wireframe"
+        android:textOn="@string/wireframe" />
+    
+    <ToggleButton
+        android:id="@+id/light_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/backface_button"
+        android:layout_alignLeft="@+id/backface_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="5dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/light_off"
+        android:textOn="@string/light_on" />
+    
+    <ToggleButton
+        android:id="@+id/backface_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/camera_button"
+        android:layout_alignRight="@+id/camera_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="15dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/backface_off"
+        android:textOn="@string/backface_on" />
+ 
+    <com.introlab.rtabmap.NDSpinner
+        android:id="@+id/camera_button"
+        android:layout_width="140dp"
+        android:layout_height="40dp"
+        android:layout_alignParentBottom="true"
+        android:layout_alignParentRight="true"
+        android:layout_marginRight="5dp"
+        android:layout_marginBottom="10dp"
+        android:paddingRight="5dp"
+        android:paddingBottom="10dp"
+        android:text="@string/camera_button"
+        android:spinnerMode="dropdown"/>
 
+
+    <Button
+        android:id="@+id/button_shareToSketchfab"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:layout_marginRight="5dp"
+        android:layout_marginTop="10dp"
+        android:layout_alignParentRight="true"
+        android:layout_below="@+id/pause_button"
+        android:text="@string/share_to_sketchfab" />
+
+    <Button
+        android:id="@+id/button_saveOnDevice"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:layout_alignRight="@+id/button_shareToSketchfab"
+        android:layout_below="@+id/button_shareToSketchfab"
+        android:text="@string/save_to_file" />
+
+    <Button
+        android:id="@+id/close_visualization_button"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/camera_button"
+        android:layout_centerHorizontal="true"
+        android:text="@string/close_visualization" />
+
+    <SeekBar
+        android:id="@+id/seekBar_ortho_cut"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:rotation="270"
+        android:layout_above="@+id/light_button"
+        android:layout_alignParentLeft="true"
+        android:layout_gravity="center"
+        android:layout_marginLeft="-50dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+    
+    <SeekBar
+        android:id="@+id/seekBar_grid"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentLeft="true"
+        android:layout_alignBottom="@+id/camera_button"
+        android:layout_marginLeft="35dp"
+        android:paddingBottom="10dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+
+    <Button
+        android:id="@+id/button_library"
+        android:layout_width="match_parent"
+        android:layout_height="100dp"
+        android:layout_centerHorizontal="true"
+        android:layout_gravity="top"
+        android:layout_marginTop="100dp"
+        android:text="@string/library" />
+
+    <Button
+        android:id="@+id/button_new_scan"
+        android:layout_width="match_parent"
+        android:layout_height="100dp"
+        android:layout_alignLeft="@+id/button_library"
+        android:layout_below="@+id/button_library"
+        android:text="@string/new_scan" />
+
+</RelativeLayout>        
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -0,0 +1,375 @@
+<PreferenceScreen xmlns:android="http://schemas.android.com/apk/res/android">
+    
+    <ListPreference 
+            android:key="@string/pref_key_camera_driver"
+    		android:title="@string/pref_title_camera_driver"
+    		android:summary="@string/pref_summary_camera_driver"
+   			android:entries="@array/pref_camera_driver_keys"
+   			android:entryValues="@array/pref_camera_driver_values"
+   			android:defaultValue="@string/pref_default_camera_driver"/>
+    
+    <PreferenceCategory
+        android:title="@string/pref_title_rendering">
+        <ListPreference 
+            android:key="@string/pref_key_density"
+    		android:title="@string/pref_title_density"
+    		android:summary="@string/pref_summary_density"
+   			android:entries="@array/pref_density_keys"
+   			android:entryValues="@array/pref_density_values"
+   			android:defaultValue="@string/pref_default_density"/>
+        <ListPreference 
+            android:key="@string/pref_key_depth"
+    		android:title="@string/pref_title_depth"
+    		android:summary="@string/pref_summary_depth"
+   			android:entries="@array/pref_depth_keys"
+   			android:entryValues="@array/pref_depth_values"
+   			android:defaultValue="@string/pref_default_depth"/>
+        <ListPreference 
+            android:key="@string/pref_key_min_depth"
+    		android:title="@string/pref_title_min_depth"
+    		android:summary="@string/pref_summary_min_depth"
+   			android:entries="@array/pref_min_depth_keys"
+   			android:entryValues="@array/pref_min_depth_values"
+   			android:defaultValue="@string/pref_default_min_depth"/>
+        <ListPreference
+            android:key="@string/pref_key_point_size"
+    		android:title="@string/pref_title_point_size"
+    		android:summary="@string/pref_summary_point_size"
+   			android:entries="@array/pref_point_size_values"
+   			android:entryValues="@array/pref_point_size_values"
+   			android:defaultValue="@string/pref_default_point_size"/>
+        <ListPreference 
+            android:key="@string/pref_key_angle"
+    		android:title="@string/pref_title_angle"
+    		android:summary="@string/pref_summary_angle"
+   			android:entries="@array/pref_angle_keys"
+   			android:entryValues="@array/pref_angle_values"
+   			android:defaultValue="@string/pref_default_angle"/>
+        <ListPreference 
+            android:key="@string/pref_key_triangle"
+    		android:title="@string/pref_title_triangle"
+    		android:summary="@string/pref_summary_triangle"
+   			android:entries="@array/pref_triangle_keys"
+   			android:entryValues="@array/pref_triangle_values"
+   			android:defaultValue="@string/pref_default_triangle"/>
+        <ListPreference 
+            android:key="@string/pref_key_rendering_texture_decimation"
+    		android:title="@string/pref_title_rendering_texture_decimation"
+    		android:summary="@string/pref_summary_rendering_texture_decimation"
+   			android:entries="@array/pref_rendering_texture_decimation_keys"
+   			android:entryValues="@array/pref_rendering_texture_decimation_values"
+   			android:defaultValue="@string/pref_default_rendering_texture_decimation"/>
+        <ListPreference 
+            android:key="@string/pref_key_background_color"
+    		android:title="@string/pref_title_background_color"
+    		android:summary="@string/pref_summary_background_color"
+   			android:entries="@array/pref_background_color_keys"
+   			android:entryValues="@array/pref_background_color_values"
+   			android:defaultValue="@string/pref_default_background_color"/>
+        <com.introlab.rtabmap.CustomSwitchPreference
+            android:key="@string/pref_key_blending"
+            android:title="@string/pref_title_blending"
+            android:summary="@string/pref_summary_blending"
+            android:defaultValue="@string/pref_default_blending"/>
+        <com.introlab.rtabmap.CustomSwitchPreference
+            android:key="@string/pref_key_nodes_filtering"
+            android:title="@string/pref_title_nodes_filtering"
+            android:summary="@string/pref_summary_nodes_filtering"
+            android:defaultValue="@string/pref_default_nodes_filtering"/>
+    </PreferenceCategory>
+ 	 	
+    <PreferenceCategory
+        android:title="@string/pref_title_general">
+        
+        <PreferenceScreen
+            android:key="pref_button_mapping"
+            android:title="@string/pref_title_mapping_sub"
+            android:summary="@string/pref_summary_mapping"
+            android:persistent="false">
+                    
+		    <com.introlab.rtabmap.CustomSwitchPreference
+	            android:key="@string/pref_key_append"
+	            android:title="@string/pref_title_append"
+	            android:summary="@string/pref_summary_append"
+	            android:defaultValue="@string/pref_default_append"/>
+		    <com.introlab.rtabmap.CustomSwitchPreference
+	            android:key="@string/pref_key_resolution"
+	            android:title="@string/pref_title_resolution"
+	            android:summary="@string/pref_summary_resolution"
+	            android:defaultValue="@string/pref_default_resolution"/>
+		    <com.introlab.rtabmap.CustomSwitchPreference
+		        android:key="@string/pref_key_smoothing"
+		        android:title="@string/pref_title_smoothing"
+		        android:summary="@string/pref_summary_smoothing"
+		        android:defaultValue="@string/pref_default_smoothing"/>
+		    <com.introlab.rtabmap.CustomSwitchPreference
+		        android:key="@string/pref_key_fisheye"
+		        android:title="@string/pref_title_fisheye"
+		        android:summary="@string/pref_summary_fisheye"
+		        android:defaultValue="@string/pref_default_fisheye"/>
+	  
+		     <PreferenceCategory
+	        	android:title="@string/pref_title_mapping_core">
+		        <ListPreference
+		            android:key="@string/pref_key_update_rate"
+		            android:title="@string/pref_title_update_rate"
+		            android:summary="@string/pref_summary_update_rate"
+		            android:entries="@array/pref_update_rate_keys"
+		   			android:entryValues="@array/pref_update_rate_values"
+		   			android:defaultValue="@string/pref_default_update_rate"/>
+		        <ListPreference
+		            android:key="@string/pref_key_max_speed"
+		            android:title="@string/pref_title_max_speed"
+		            android:summary="@string/pref_summary_max_speed"
+		            android:entries="@array/pref_max_speed_keys"
+		   			android:entryValues="@array/pref_max_speed_values"
+		   			android:defaultValue="@string/pref_default_max_speed"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_time_thr"
+		    		android:title="@string/pref_title_time_thr"
+		    		android:summary="@string/pref_summary_time_thr"
+		   			android:entries="@array/pref_time_thr_keys"
+		   			android:entryValues="@array/pref_time_thr_values"
+		   			android:defaultValue="@string/pref_default_time_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_mem_thr"
+		    		android:title="@string/pref_title_mem_thr"
+		    		android:summary="@string/pref_summary_mem_thr"
+		   			android:entries="@array/pref_mem_thr_keys"
+		   			android:entryValues="@array/pref_mem_thr_values"
+		   			android:defaultValue="@string/pref_default_mem_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_loop_thr"
+		    		android:title="@string/pref_title_loop_thr"
+		    		android:summary="@string/pref_summary_loop_thr"
+		   			android:entries="@array/pref_loop_thr_keys"
+		   			android:entryValues="@array/pref_loop_thr_values"
+		   			android:defaultValue="@string/pref_default_loop_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_sim_thr"
+		    		android:title="@string/pref_title_sim_thr"
+		    		android:summary="@string/pref_summary_sim_thr"
+		   			android:entries="@array/pref_sim_thr_keys"
+		   			android:entryValues="@array/pref_sim_thr_values"
+		   			android:defaultValue="@string/pref_default_sim_thr"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_min_inliers"
+		    		android:title="@string/pref_title_min_inliers"
+		    		android:summary="@string/pref_summary_min_inliers"
+		   			android:entries="@array/pref_min_inliers_keys"
+		   			android:entryValues="@array/pref_min_inliers_values"
+		   			android:defaultValue="@string/pref_default_min_inliers"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_error"
+		    		android:title="@string/pref_title_opt_error"
+		    		android:summary="@string/pref_summary_opt_error"
+		   			android:entries="@array/pref_opt_error_keys"
+		   			android:entryValues="@array/pref_opt_error_values"
+		   			android:defaultValue="@string/pref_default_opt_error"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_features_voc"
+		    		android:title="@string/pref_title_features_voc"
+		    		android:summary="@string/pref_summary_features_voc"
+		   			android:entries="@array/pref_features_voc_keys"
+		   			android:entryValues="@array/pref_features_voc_values"
+		   			android:defaultValue="@string/pref_default_features_voc"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_features"
+		    		android:title="@string/pref_title_features"
+		    		android:summary="@string/pref_summary_features"
+		   			android:entries="@array/pref_features_keys"
+		   			android:entryValues="@array/pref_features_values"
+		   			android:defaultValue="@string/pref_default_features"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_features_type"
+		    		android:title="@string/pref_title_features_type"
+		    		android:summary="@string/pref_summary_features_type"
+		   			android:entries="@array/pref_features_type_keys"
+		   			android:entryValues="@array/pref_features_type_values"
+		   			android:defaultValue="@string/pref_default_features_type"/>
+		        <ListPreference 
+		            android:key="@string/pref_key_optimizer"
+		    		android:title="@string/pref_title_optimizer"
+		    		android:summary="@string/pref_summary_optimizer"
+		   			android:entries="@array/pref_optimizer_keys"
+		   			android:entryValues="@array/pref_optimizer_values"
+		   			android:defaultValue="@string/pref_default_optimizer"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference 
+		            android:key="@string/pref_key_optimize_end"
+		    		android:title="@string/pref_title_optimize_end"
+		    		android:summary="@string/pref_summary_optimize_end"
+		   			android:defaultValue="@string/pref_default_optimize_end"/>
+		   		<ListPreference 
+		            android:key="@string/pref_key_marker_detection"
+		    		android:title="@string/pref_title_marker_detection"
+		    		android:summary="@string/pref_summary_marker_detection"
+		   			android:entries="@array/pref_marker_detection_keys"
+		   			android:entryValues="@array/pref_marker_detection_values"
+		   			android:defaultValue="@string/pref_default_marker_detection"/>
+		   		<ListPreference 
+		            android:key="@string/pref_key_marker_detection_depth_error"
+		    		android:title="@string/pref_title_marker_detection_depth_error"
+		    		android:summary="@string/pref_summary_marker_detection_depth_error"
+		   			android:entries="@array/pref_marker_detection_depth_error_keys"
+		   			android:entryValues="@array/pref_marker_detection_depth_error_values"
+		   			android:defaultValue="@string/pref_default_marker_detection_depth_error"/>
+		    </PreferenceCategory>
+	        <PreferenceCategory
+	        	android:title="@string/pref_title_mapping_database">
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_keep_all_db"
+		            android:title="@string/pref_title_keep_all_db"
+		            android:summary="@string/pref_summary_keep_all_db"
+		            android:defaultValue="@string/pref_default_keep_all_db"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_raw_scan_saved"
+		            android:title="@string/pref_title_raw_scan_saved"
+		            android:summary="@string/pref_summary_raw_scan_saved"
+		            android:defaultValue="@string/pref_default_raw_scan_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_gps_saved"
+		            android:title="@string/pref_title_gps_saved"
+		            android:summary="@string/pref_summary_gps_saved"
+		            android:defaultValue="@string/pref_default_gps_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_env_sensors_saved"
+		            android:title="@string/pref_title_env_sensors_saved"
+		            android:summary="@string/pref_summary_env_sensors_saved"
+		            android:defaultValue="@string/pref_default_env_sensors_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_db_in_memory"
+		            android:title="@string/pref_title_db_in_memory"
+		            android:summary="@string/pref_summary_db_in_memory"
+		            android:defaultValue="@string/pref_default_db_in_memory"/>
+		    </PreferenceCategory>
+	    </PreferenceScreen>
+    
+	    <PreferenceScreen
+	            android:key="pref_button_export"
+	            android:title="@string/pref_title_export_sub"
+	            android:summary="@string/pref_summary_export"
+	            android:persistent="false">
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_cloud_voxel"
+	    		android:title="@string/pref_title_cloud_voxel"
+	    		android:summary="@string/pref_summary_cloud_voxel"
+	   			android:entries="@array/pref_cloud_voxel_keys"
+	   			android:entryValues="@array/pref_cloud_voxel_values"
+	   			android:defaultValue="@string/pref_default_cloud_voxel"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_texture_size"
+	    		android:title="@string/pref_title_texture_size"
+	    		android:summary="@string/pref_summary_texture_size"
+	   			android:entries="@array/pref_texture_size_keys"
+	   			android:entryValues="@array/pref_texture_size_values"
+	   			android:defaultValue="@string/pref_default_texture_size"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_texture_count"
+	    		android:title="@string/pref_title_texture_count"
+	    		android:summary="@string/pref_summary_texture_count"
+	   			android:entries="@array/pref_texture_count_keys"
+	   			android:entryValues="@array/pref_texture_count_values"
+	   			android:defaultValue="@string/pref_default_texture_count"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_normal_k"
+	    		android:title="@string/pref_title_normal_k"
+	    		android:summary="@string/pref_summary_normal_k"
+	   			android:entries="@array/pref_normal_k_values"
+	   			android:entryValues="@array/pref_normal_k_values"
+	   			android:defaultValue="@string/pref_default_normal_k"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_max_texture_distance"
+	    		android:title="@string/pref_title_max_texture_distance"
+	    		android:summary="@string/pref_summary_max_texture_distance"
+	   			android:entries="@array/pref_max_texture_distance_keys"
+	   			android:entryValues="@array/pref_max_texture_distance_values"
+	   			android:defaultValue="@string/pref_default_max_texture_distance"/>
+	        
+	        <ListPreference 
+	            android:key="@string/pref_key_min_texture_cluster_size"
+	    		android:title="@string/pref_title_min_texture_cluster_size"
+	    		android:summary="@string/pref_summary_min_texture_cluster_size"
+	   			android:entries="@array/pref_min_texture_cluster_size_keys"
+	   			android:entryValues="@array/pref_min_texture_cluster_size_values"
+	   			android:defaultValue="@string/pref_default_min_texture_cluster_size"/>
+	        
+		    <com.introlab.rtabmap.CustomSwitchPreference
+	            android:key="@string/pref_key_block_render"
+	            android:title="@string/pref_title_block_render"
+	            android:summary="@string/pref_summary_block_render"
+	            android:defaultValue="@string/pref_default_block_render"/>
+		    
+		    <PreferenceCategory
+	        android:title="@string/pref_title_optimized">
+	        
+	        
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_depth"
+		    		android:title="@string/pref_title_opt_depth"
+		    		android:summary="@string/pref_summary_opt_depth"
+		   			android:entries="@array/pref_opt_depth_keys"
+		   			android:entryValues="@array/pref_opt_depth_values"
+		   			android:defaultValue="@string/pref_default_opt_depth"/>
+		        
+
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_color_radius"
+		    		android:title="@string/pref_title_opt_color_radius"
+		    		android:summary="@string/pref_summary_opt_color_radius"
+		   			android:entries="@array/pref_opt_color_radius_keys"
+		   			android:entryValues="@array/pref_opt_color_radius_values"
+		   			android:defaultValue="@string/pref_default_opt_color_radius"/>
+		        
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_opt_clean_white"
+		            android:title="@string/pref_title_opt_clean_white"
+		            android:summary="@string/pref_summary_opt_clean_white"
+		            android:defaultValue="@string/pref_default_opt_clean_white"/>
+		        
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_min_cluster_size"
+		    		android:title="@string/pref_title_opt_min_cluster_size"
+		    		android:summary="@string/pref_summary_opt_min_cluster_size"
+		   			android:entries="@array/pref_opt_min_cluster_size_keys"
+		   			android:entryValues="@array/pref_opt_min_cluster_size_values"
+		   			android:defaultValue="@string/pref_default_opt_min_cluster_size"/>
+		        
+		       </PreferenceCategory>
+	 	</PreferenceScreen>
+	 	<ListPreference 
+            android:key="@string/pref_key_gain_max_radius"
+    		android:title="@string/pref_title_gain_max_radius"
+    		android:summary="@string/pref_summary_gain_max_radius"
+   			android:entries="@array/pref_gain_max_radius_keys"
+   			android:entryValues="@array/pref_gain_max_radius_values"
+   			android:defaultValue="@string/pref_default_gain_max_radius"/>
+        <ListPreference 
+            android:key="@string/pref_key_cluster_ratio"
+    		android:title="@string/pref_title_cluster_ratio"
+    		android:summary="@string/pref_summary_cluster_ratio"
+   			android:entries="@array/pref_cluster_ratio_keys"
+   			android:entryValues="@array/pref_cluster_ratio_values"
+   			android:defaultValue="@string/pref_default_cluster_ratio"/>
+        <com.introlab.rtabmap.CustomSwitchPreference 
+            android:key="@string/pref_key_notification_sound"
+    		android:title="@string/pref_title_notification_sound"
+    		android:summary="@string/pref_summary_notification_sound"
+   			android:defaultValue="@string/pref_default_notification_sound"/>
+    </PreferenceCategory>
+	<PreferenceCategory
+        android:title="@string/pref_title_presets">
+        <Preference android:title="@string/pref_title_open_button"
+	                android:key="@string/pref_key_open_button"/>
+        <Preference android:title="@string/pref_title_save_button"
+	                android:key="@string/pref_key_save_button"/>
+        <Preference android:title="@string/pref_title_remove_button"
+	                android:key="@string/pref_key_remove_button"/>
+	    <Preference android:title="@string/pref_title_reset_button"
+	                android:key="@string/pref_key_reset_button"/>
+    </PreferenceCategory>
+</PreferenceScreen>

app/android/res/layout/activity_sketchfab.xml

@@ -0,0 +1,72 @@
+<?xml version="1.0" encoding="utf-8"?>
+<ScrollView xmlns:android="http://schemas.android.com/apk/res/android" 
+    android:layout_width="match_parent"
+    android:layout_height="match_parent">
+<LinearLayout android:layout_width="match_parent"
+              android:layout_height="match_parent"
+              android:orientation="vertical"
+              android:focusableInTouchMode="true" >
+              
+    <TextView android:id="@+id/text"
+              android:layout_width="wrap_content"
+              android:layout_height="wrap_content"
+              android:text="Model Name*:" />
+
+    <EditText
+        android:id="@+id/editText_filename"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:imeOptions="flagNoExtractUi"
+        android:ems="10" />
+
+    <TextView
+        android:id="@+id/textView1"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:text="Description:" />
+
+    <EditText
+        android:id="@+id/editText_description"
+        android:ems="10"
+        android:lines="4" 
+	    android:minLines="2" 
+	    android:gravity="top|left" 
+	    android:maxLines="8" 
+	    android:layout_weight="1" 
+	    android:layout_height="wrap_content" 
+	    android:layout_width="fill_parent" 
+	    android:scrollbars="vertical"
+	    android:imeOptions="flagNoExtractUi"
+        android:inputType="textMultiLine" >
+
+        <requestFocus />
+    </EditText>
+
+    <TextView
+        android:id="@+id/textView2"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:text="Tags:" />
+
+    <EditText
+        android:id="@+id/editText_tags"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:imeOptions="flagNoExtractUi"
+        android:ems="10" />
+
+    <CheckBox
+        android:id="@+id/checkBox_draft"
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:checked="true"
+        android:text="Draft Mode" />
+
+    <Button
+        android:id="@+id/button_ok"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:text="Upload" />
+
+</LinearLayout>
+</ScrollView>

app/android/res/layout/auth_dialog.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+    android:layout_width="match_parent"
+    android:layout_height="match_parent"
+    android:orientation="vertical" >
+      <WebView
+        android:layout_width="wrap_content"
+        android:layout_height="wrap_content"
+        android:id="@+id/webv"/>
+
+</LinearLayout>

app/android/res/layout/database_list.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+android:layout_width="match_parent"
+android:layout_height="wrap_content"
+android:background="#fff">
+
+<ImageView
+android:id="@+id/imageView"
+android:layout_width="@dimen/image_width"
+android:layout_height="@dimen/image_width"
+android:layout_marginLeft="10dp"
+android:padding="5dp"
+android:src="@drawable/ic_launcher" />
+
+<TextView
+android:id="@+id/textView"
+android:layout_width="wrap_content"
+android:layout_height="wrap_content"
+android:layout_gravity="center"
+android:text="Demo"
+android:textColor="#000" />
+</LinearLayout>

app/android/res/menu/optionmenu.xml

@@ -1,19 +1,41 @@
 <?xml version="1.0" encoding="utf-8"?>
 <menu xmlns:android="http://schemas.android.com/apk/res/android">
-    
-    <group android:id="@+id/group_pause" android:checkableBehavior="all">
-        <item android:id="@+id/pause" android:checked="false" android:title="Pause"/>
-    </group>
-    
+        
     <group android:id="@+id/group_actions">
-       <item android:id="@+id/post_processing" android:title="Post-Processing...">
+       <group android:checkableBehavior="single">
+	      <item android:id="@+id/point_cloud" android:title="Point Cloud" />
+		  <item android:id="@+id/mesh" android:title="Mesh" />
+		  <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
+	   </group>
+	   
+       <item android:id="@+id/new_scan" android:title="New Scan"/>
+       <item android:id="@+id/open" android:title="Library" android:showAsAction="ifRoom"/>
+	   <item android:id="@+id/save" android:title="Save"/>
+                     
+       <item android:id="@+id/export" android:title="Export">
+          <menu>
+              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
+                  <menu>
+                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
+                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
+                  </menu>
+              </item>
+	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
+	              <menu>
+		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
+			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
+		          </menu>
+	          </item>
+          </menu>
+       </item>
+       
+        <item android:id="@+id/post_processing" android:title="Optimize">
           <menu>
              <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
              <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
 	            <menu>
 	              <item android:id="@+id/global_graph_optimization" android:title="Global Graph Optimization" />
 	              <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
-	              <item android:id="@+id/icp_refining" android:title="ICP Refining" />
 	              <item android:id="@+id/gain_compensation_fast" android:title="Adjust Colors (Fast)" />
 	              <item android:id="@+id/gain_compensation_full" android:title="Adjust Colors (Full)" />
 	              <item android:id="@+id/bilateral_filtering" android:title="Mesh Smoothing" />
@@ -23,35 +45,12 @@
 	         </item>
           </menu>
        </item>
-       <item android:id="@+id/open" android:title="Open"/>
-       <item android:id="@+id/export" android:title="Export...">
-          <menu>
-	         <group android:id="@+id/group_export">
-	            <item android:id="@+id/export_ply" android:title="Mesh (.ply)" />
-	            <item android:id="@+id/export_obj" android:title="Mesh with texture (*.obj)" />
-	         </group>   
-          </menu>
-       </item>
-       <item android:id="@+id/save" android:title="Save"/>
-       <item android:id="@+id/reset" android:title="Reset"/>
        
-       <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options...">
+       <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
+            <item android:id="@+id/status" android:checked="true" android:title="Status" />
             <item android:id="@+id/debug" android:checked="false" android:title="Debug" />
-            <item android:id="@+id/menu_rendering" android:checkable="false" android:title="Mesh Rendering..." >
-           			<menu>
-           			   <group android:checkableBehavior="single">
-	           			   <item android:id="@+id/point_cloud" android:title="Point Cloud" />
-				           <item android:id="@+id/mesh" android:title="Mesh" />
-				           <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
-			           </group>
-			           <item android:id="@+id/mesh_decimation" android:checkable="false" android:title="Mesh Decimation..." />
-			           <item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
-            		   <item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
-            		   <item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />
-			       </menu>
-            </item>
             <item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
             <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
             <item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
@@ -60,30 +59,17 @@
         </group>   
         </menu>
 	    </item>
-	    <item android:id="@+id/menu_mapping_settings" android:title="Mapping Options...">
+	    <item android:id="@+id/modes" android:title="Modes...">
 	       <menu >
 	       <group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all">
 	           <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
 	           <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
-	           <item android:id="@+id/append" android:checked="true" android:title="Append Mode" />
-	           <item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
-	           <item android:id="@+id/drift_correction" android:checked="false" android:title="Drift Correction" />
-	           <item android:id="@+id/auto_exposure" android:checked="true" android:title="Auto Exposure" />
-	           <item android:id="@+id/resolution" android:checked="false" android:title="HD Mode" />
 	           <item android:id="@+id/data_recorder" android:checked="false" android:title="Data Recorder Mode" />
-	           <item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters...">
-		           <menu >
-			           <item android:id="@+id/update_rate" android:title="Map Update Rate..." />
-			           <item android:id="@+id/time_threshold" android:title="Time Threshold..." />
-			           <item android:id="@+id/loop_threshold" android:title="Loop Closure Threshold..." />
-			           <item android:id="@+id/optimize_error" android:title="Max Optimization Error..." />
-			           <item android:id="@+id/features" android:title="Max Features Extracted..." />
-			       </menu>
-			    </item>
 	       </group>   
 	       </menu>
 	   </item>
-       
+	   <item android:id="@+id/settings" android:title="Settings"/>
+       <item android:id="@+id/resume" android:title="Resume"/>
        <item android:id="@+id/about" android:title="About"/>
     </group>
 </menu>

app/android/res/values/arrays.xml

@@ -0,0 +1,9 @@
+
+    <resources>
+        <string-array name="camera_view_array">
+        <item>First View</item>
+        <item>Third-P. View</item>
+        <item>Top View</item>
+        <item>Ortho View</item>
+    </string-array>
+    </resources>

app/android/res/values/dimens.xml

@@ -0,0 +1,3 @@
+<resources>
+    <dimen name="image_width">150dp</dimen>
+</resources>

app/android/res/values/strings.xml

@@ -1,25 +1,962 @@
 <?xml version="1.0" encoding="utf-8"?>
 <resources>
+
     <string name="app_name">RTAB-Map</string>
     <string name="sys_name">RTAB-Map</string>
-    <string name="menu_name">Real-Time Appearance-Based Mapping</string>
+    <string name="menu_name">RTAB-Map</string>
+    <string name="settings">Settings</string>
+    <string name="sketchfab">Upload to Sketchfab&#8230;</string>
     <string name="status">"Status: "</string>
     <string name="words">"Words: "</string>
-    <string name="first_person">First</string>
-    <string name="third_person">Third</string>
-    <string name="top_down">Top</string>
+    <string name="camera_button">First View</string>
+    <string name="library">Library</string>
+    <string name="new_scan">New Scan</string>
+    <string name="pause">Stop</string>
+    <string name="resume">Start</string>
+    <string name="backface_on">Backface</string>
+    <string name="backface_off">Backface</string>
+    <string name="light_on">Lighting</string>
+    <string name="light_off">Lighting</string>
+    <string name="wireframe">Wireframe</string>
+    <string name="close_visualization">Close Visualization</string>
+    <string name="save_to_file">Export to File&#8230;</string>
+    <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
     <string name="start">Start</string>
     <string name="nodes">"Nodes (WM): "</string>
     <string name="points">"Number of points: "</string>
     <string name="update_time">"Update time (ms): "</string>
     <string name="loop_closure">"Loop closure ID: "</string>
-    <string name="db_size">"Database (MB): "</string>
+    <string name="free_memory">"Free Memory (MB): "</string>
+    <string name="database_size">"Database (MB): "</string>
     <string name="total_loop">"Loop closures: "</string>
     <string name="inliers">"Inliers: "</string>
     <string name="features">"Features: "</string>
+    <string name="rehearsal">"Rehearsal (%): "</string>
     <string name="polygons">"Polygons: "</string>
-    <string name="memory">"Memory (MB): "</string>
-    <string name="hypothesis">"Hypothesis: "</string>
+    <string name="memory">"Used Memory (MB): "</string>
+    <string name="hypothesis">"Hypothesis (%): "</string>
     <string name="fps">"FPS (rendering): "</string>
+    <string name="distance">"Distance travelled: "</string>
+    <string name="gps">"GPS (long,lat,alt,bearing,err): "</string>
+    <string name="env_sensors">"Sensors: "</string>
+    <string name="time">"Time: "</string>
     
+    <!-- Preference keys: BEGIN -->
+    <string name="pref_key_tags">pref_key_tags</string>
+    <string name="pref_default_tags">rtabmap 3dscan tango</string>
+    <string name="pref_key_rendering">pref_key_rendering</string>
+    <string name="pref_default_rendering">2</string>
+    <string name="pref_key_open_button">pref_key_open_button</string>
+    <string name="pref_key_save_button">pref_key_save_button</string>
+    <string name="pref_key_remove_button">pref_key_remove_button</string>
+    <string name="pref_key_reset_button">pref_key_reset_button</string>
+    <string name="pref_key_density">pref_key_density</string>
+    <string name="pref_default_density">1</string>
+    <string name="pref_key_min_depth">pref_key_min_depth</string>
+    <string name="pref_default_min_depth">0</string>
+    <string name="pref_key_depth">pref_key_depth</string>
+    <string name="pref_default_depth">2.5</string>
+    <string name="pref_key_point_size">pref_key_point_size</string>
+    <string name="pref_default_point_size">10</string>
+    <string name="pref_key_angle">pref_key_angle</string>
+    <string name="pref_default_angle">20</string>
+    <string name="pref_key_triangle">pref_key_triangle</string>
+    <string name="pref_default_triangle">2</string>
+    <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
+    <string name="pref_default_rendering_texture_decimation">4</string>
+    <string name="pref_key_blending">pref_key_blending</string>
+    <string name="pref_default_blending">false</string>
+    <string name="pref_key_background_color">pref_key_background_color</string>
+    <string name="pref_default_background_color">0.2</string>
+    <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>
+    <string name="pref_default_nodes_filtering">false</string>
+    <string name="pref_key_append">pref_key_append</string>
+    <string name="pref_default_append">true</string>
+    <string name="pref_key_resolution">pref_key_resolution</string>
+    <string name="pref_default_resolution">false</string>
+    <string name="pref_key_smoothing">pref_key_smoothing</string>
+    <string name="pref_default_smoothing">true</string>
+    <string name="pref_key_fisheye">pref_key_fisheye</string>
+    <string name="pref_default_fisheye">false</string>
+    
+    <string name="pref_key_camera_driver">pref_key_camera_driver</string>
+    <string name="pref_default_camera_driver">0</string>
+    <string name="pref_key_update_rate">pref_key_update_rate</string>
+    <string name="pref_default_update_rate">1</string>
+    <string name="pref_key_max_speed">pref_key_max_speed</string>
+    <string name="pref_default_max_speed">0</string>
+    <string name="pref_key_time_thr">pref_key_time_thr</string>
+    <string name="pref_default_time_thr">1000</string>
+    <string name="pref_key_mem_thr">pref_key_mem_thr</string>
+    <string name="pref_default_mem_thr">0</string>
+    <string name="pref_key_loop_thr">pref_key_loop_thr</string>
+    <string name="pref_default_loop_thr">0.11</string>
+    <string name="pref_key_sim_thr">pref_key_sim_thr</string>
+    <string name="pref_default_sim_thr">0.3</string>
+    <string name="pref_key_min_inliers">pref_key_min_inliers</string>
+    <string name="pref_default_min_inliers">25</string>
+    <string name="pref_key_opt_error">pref_key_opt_error</string>
+    <string name="pref_default_opt_error">3</string>
+    <string name="pref_key_features_voc">pref_key_features_voc</string>
+    <string name="pref_default_features_voc">200</string>
+    <string name="pref_key_features">pref_key_features</string>
+    <string name="pref_default_features">400</string>
+    <string name="pref_key_features_type">pref_key_features_type</string>
+    <string name="pref_default_features_type">6</string>
+    <string name="pref_key_optimizer">pref_key_optimizer</string>
+    <string name="pref_default_optimizer">2</string>
+    <string name="pref_key_optimize_end">pref_key_optimize_end</string>
+    <string name="pref_default_optimize_end">true</string>
+    <string name="pref_key_marker_detection">pref_key_marker_detection</string>
+    <string name="pref_default_marker_detection">-1</string>
+    <string name="pref_key_marker_detection_depth_error">pref_key_marker_detection_depth_error</string>
+    <string name="pref_default_marker_detection_depth_error">0.1</string>
+    <string name="pref_key_keep_all_db">pref_key_keep_all_db</string>
+    <string name="pref_default_keep_all_db">true</string>
+    <string name="pref_key_raw_scan_saved">pref_key_raw_scan_saved</string>
+    <string name="pref_default_raw_scan_saved">false</string>
+    <string name="pref_key_gps_saved">pref_key_gps_saved</string>
+    <string name="pref_default_gps_saved">false</string>
+    <string name="pref_key_env_sensors_saved">pref_key_env_sensors_saved</string>
+    <string name="pref_default_env_sensors_saved">false</string>
+    <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
+    <string name="pref_default_db_in_memory">false</string>
+    
+    <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
+    <string name="pref_default_cloud_voxel">0.01</string>
+    <string name="pref_key_texture_size">pref_key_texture_size</string>
+    <string name="pref_default_texture_size">4096</string>
+    <string name="pref_key_texture_count">pref_key_texture_count</string>
+    <string name="pref_default_texture_count">1</string>
+    <string name="pref_key_normal_k">pref_key_normal_k</string>
+    <string name="pref_default_normal_k">18</string>
+    <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
+    <string name="pref_default_max_texture_distance">3</string>
+    <string name="pref_key_min_texture_cluster_size">pref_key_min_texture_cluster_size</string>
+    <string name="pref_default_min_texture_cluster_size">50</string>
+    <string name="pref_key_block_render">pref_key_block_render</string>
+    <string name="pref_default_block_render">false</string>
+    <string name="pref_key_opt_depth">pref_key_opt_depth</string>
+    <string name="pref_default_opt_depth">0</string>
+    <string name="pref_key_opt_color_radius">pref_key_opt_color_radius</string>
+    <string name="pref_default_opt_color_radius">0.05</string>
+    <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>
+    <string name="pref_default_opt_clean_white">true</string>
+    <string name="pref_key_opt_min_cluster_size">pref_key_opt_min_cluster_size</string>
+    <string name="pref_default_opt_min_cluster_size">0</string>
+    <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
+    <string name="pref_default_gain_max_radius">0.02</string>
+    <string name="pref_key_cluster_ratio">pref_key_cluster_ratio</string>
+    <string name="pref_default_cluster_ratio">0.05</string>
+    <string name="pref_key_notification_sound">pref_key_notification_sound</string>
+    <string name="pref_default_notification_sound">true</string>
+    <!-- Preference keys: END -->
+
+    <string name="pref_title_rendering">Rendering</string>
+    <string name="pref_title_density">Point Cloud Density</string>
+    <string name="pref_summary_density">Decrease density to reduce rendering time and memory. Tip: To apply a different density to current map: save the map, change density and re-open the same map to regenerate the point clouds at this density.</string>
+    <string name="pref_title_angle">Mesh Angle Tolerance</string>
+    <string name="pref_summary_angle">Minimum polygon angle. Increase to force scanning perpendicular to surfaces.</string>
+    <string name="pref_title_triangle">Mesh Triangle Size</string>
+    <string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string>
+    <string name="pref_title_rendering_texture_decimation">Texture Resolution</string>
+    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string>
+    <string name="pref_title_min_depth">Min Depth</string>
+    <string name="pref_summary_min_depth">Points under the minimum depth are not rendered.</string>
+    <string name="pref_title_depth">Max Depth</string>
+    <string name="pref_summary_depth">Points over the maximum depth are not rendered.</string>
+    <string name="pref_title_point_size">Point Size</string>
+    <string name="pref_summary_point_size">Size of the points when rendering only the point cloud.</string>
+    <string name="pref_title_blending">Blending</string>
+    <string name="pref_summary_blending">Blend close surfaces together to get more smooth colors on overlapping surfaces. May decrease rendering frame rate.</string>
+    <string name="pref_title_background_color">Background Color</string>
+    <string name="pref_summary_background_color"></string>
+    <string name="pref_title_nodes_filtering">Nodes Filtering</string>
+    <string name="pref_summary_nodes_filtering">Render only the newest point cloud of a loop closure.</string>
+
+    <string-array name="pref_density_keys">
+        <item>"Maximum"</item>
+        <item>"High"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_density_values">
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+    </string-array>
+    <string-array name="pref_min_depth_keys">
+        <item>"0 m"</item>
+        <item>"0.3 m"</item>
+        <item>"0.5 m"</item>
+        <item>"0.75 m"</item>
+        <item>"1 m"</item>
+        <item>"1.5 m"</item>
+        <item>"2 m"</item>
+        <item>"2.5 m"</item>
+        <item>"3 m"</item>
+    </string-array>
+    <string-array name="pref_min_depth_values">
+        <item>"0"</item>
+        <item>"0.3"</item>
+        <item>"0.5"</item>
+        <item>"0.75"</item>
+        <item>"1"</item>
+        <item>"1.5"</item>
+        <item>"2"</item>
+        <item>"2.5"</item>
+        <item>"3"</item>
+    </string-array>
+    <string-array name="pref_depth_keys">
+        <item>"No Limit"</item>
+        <item>"5 m"</item>
+        <item>"4.5 m"</item>
+        <item>"4 m"</item>
+        <item>"3.5 m"</item>
+        <item>"3 m"</item>
+        <item>"2.5 m"</item>
+        <item>"2 m"</item>
+        <item>"1.5 m"</item>
+        <item>"1 m"</item>
+    </string-array>
+    <string-array name="pref_depth_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4.5"</item>
+        <item>"4"</item>
+        <item>"3.5"</item>
+        <item>"3"</item>
+        <item>"2.5"</item>
+        <item>"2"</item>
+        <item>"1.5"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_point_size_values">
+        <item>"50"</item>
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"15"</item>
+        <item>"10"</item>
+        <item>"5"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_angle_keys">
+        <item>"60 deg"</item>
+        <item>"45 deg"</item>
+        <item>"35 deg"</item>
+        <item>"30 deg"</item>
+        <item>"25 deg"</item>
+        <item>"20 deg"</item>
+        <item>"15 deg"</item>
+        <item>"10 deg"</item>
+        <item>"5 deg"</item>
+    </string-array>
+    <string-array name="pref_angle_values">
+        <item>"60"</item>
+        <item>"45"</item>
+        <item>"35"</item>
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"20"</item>
+        <item>"15"</item>
+        <item>"10"</item>
+        <item>"5"</item>
+    </string-array>
+    <string-array name="pref_triangle_keys">
+        <item>"6 pix"</item>
+        <item>"5 pix"</item>
+        <item>"4 pix"</item>
+        <item>"3 pix"</item>
+        <item>"2 pix"</item>
+    </string-array>
+    <string-array name="pref_triangle_values">
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+    </string-array>
+    <string-array name="pref_rendering_texture_decimation_keys">
+        <item>"Maximum"</item>
+        <item>"High"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_rendering_texture_decimation_values">
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"4"</item>
+        <item>"8"</item>
+    </string-array>
+    <string-array name="pref_background_color_keys">
+        <item>"White"</item>
+        <item>"0.9"</item>
+        <item>"Light Gray"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"Gray"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"Dark Gray"</item>
+        <item>"0.1"</item>
+        <item>"Black"</item>
+    </string-array>
+    <string-array name="pref_background_color_values">
+        <item>"1.0"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.0"</item>
+    </string-array>
+
+    <string name="pref_title_mapping_sub">Mapping&#8230;</string>
+    <string name="pref_title_mapping">Mapping</string>
+    <string name="pref_summary_mapping">Advanced mapping parameters for fine tuning.</string>
+    <string name="pref_title_mapping_core">Core</string>
+    <string name="pref_title_mapping_database">Database</string>
+    <string name="pref_title_camera_driver">Camera Driver</string>
+    <string name="pref_summary_camera_driver">AR sdk use for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
+    <string name="pref_title_append">Append Mode</string>
+    <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
+    <string name="pref_title_resolution">HD Mode</string>
+    <string name="pref_summary_resolution">Save HD images of the color camera if you want very detailed textures. More memory will be required.</string>
+    <string name="pref_title_smoothing">Smoothing</string>
+    <string name="pref_summary_smoothing">Smooth the point clouds.</string>
+    <string name="pref_title_fisheye">Fish Eye Camera</string>
+    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
+    <string name="pref_title_update_rate">Update Rate</string>
+    <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
+    <string name="pref_title_max_speed">Maximum Motion Speed</string>
+    <string name="pref_summary_max_speed">Images taken when the camera is moving too fast are ignored to avoid blurry textures.</string>
+    <string name="pref_title_time_thr">Time Limit</string>
+    <string name="pref_summary_time_thr">Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates.</string>
+    <string name="pref_title_mem_thr">Memory Limit</string>
+    <string name="pref_summary_mem_thr">Maximum nodes kept in working memory.</string>
+    <string name="pref_title_loop_thr">Loop Closure Threshold</string>
+    <string name="pref_summary_loop_thr">Threshold at which loop closure hypotheses are accepted. Higher means more robust to false loop closures while rejecting more good loop closures.</string>
+    <string name="pref_title_sim_thr">Similarity Threshold</string>
+    <string name="pref_summary_sim_thr">Threshold at which consecutive images are considered the same, so the corresponding node\'s weight is increased. The background turns dark blue when this happens.</string>
+    <string name="pref_title_min_inliers">Min Inliers</string>
+    <string name="pref_summary_min_inliers">Minimum visual inliers to accept a loop closure.</string>
+    <string name="pref_title_opt_error">Max Optimization Error</string>
+    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this factor of the link\'s variance.</string>
+    <string name="pref_title_features_voc">Max Features Extracted (Vocabulary)</string>
+    <string name="pref_summary_features_voc">Extracting more features per image would result in better loop closure hypotheses but more processing time is required.</string>
+    <string name="pref_title_features">Max Features Extracted (Loop Closure)</string>
+    <string name="pref_summary_features">Extracting more features per image would result in better loop closure transforms but more processing time is required.</string>
+    <string name="pref_title_features_type">Feature Type</string>
+    <string name="pref_summary_features_type">BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
+    <string name="pref_title_optimizer">Graph Optimizer</string>
+    <string name="pref_summary_optimizer">Graph optimization approach.</string>
+    <string name="pref_title_optimize_end">Optimization from Graph End</string>
+    <string name="pref_summary_optimize_end">The map\'s graph is optimized from the last node. The map is moved when a loop closure happens instead of jumping the current pose back to localized area. </string>
+    <string name="pref_title_marker_detection">ArUco Marker Detection</string>
+    <string name="pref_summary_marker_detection">ArUco markers can be detected for localization and graph optimization.</string>
+    <string name="pref_title_marker_detection_depth_error">Marker Depth Error Estimation</string>
+    <string name="pref_summary_marker_detection_depth_error">Size of markers are automatically initialized on the first marker seen. All markers should have the same size. This value is the maximum depth error to do the initialization to get accurate size of the tag. The lower it is, the more perpendicular the camera should be from the marker to do initialization, but size estimated would be more accurate.</string>
+    <string name="pref_title_keep_all_db">Save All Frames in Database</string>
+    <string name="pref_summary_keep_all_db">Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop.</string>
+    <string name="pref_title_raw_scan_saved">Save Raw Scan</string>
+    <string name="pref_summary_raw_scan_saved">Save raw point clouds in database.</string>
+    <string name="pref_title_gps_saved">Save GPS</string>
+    <string name="pref_summary_gps_saved">Save GPS to database.</string>
+    <string name="pref_title_env_sensors_saved">Save Environmental Sensors</string>
+    <string name="pref_summary_env_sensors_saved">Save Wifi strength, temperature, air pressure, light intensity and relative humidity to database.</string>
+    <string name="pref_title_db_in_memory">Database In Memory</string>
+    <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
+
+    <string-array name="pref_camera_driver_keys">
+        <item>"Google Tango NDK"</item>
+        <item>"ARCore NDK"</item>
+        <item>"AREngine NDK"</item>
+        <item>"ARCore Java"</item>
+    </string-array>
+    <string-array name="pref_camera_driver_values">
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+    </string-array>
+    <string-array name="pref_update_rate_keys">
+        <item>"Max"</item>
+        <item>"5 Hz"</item>
+        <item>"4 Hz"</item>
+        <item>"3 Hz"</item>
+        <item>"2 Hz"</item>
+        <item>"1 Hz"</item>
+        <item>"0.5 Hz"</item>
+    </string-array>
+    <string-array name="pref_update_rate_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0.5"</item>
+    </string-array>
+    <string-array name="pref_max_speed_keys">
+        <item>"No Limit"</item>
+        <item>"High"</item>
+        <item>"Medium"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_max_speed_values">
+        <item>"0"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+    </string-array>
+    <string-array name="pref_time_thr_keys">
+        <item>"No Limit"</item>
+        <item>"1500 ms"</item>
+        <item>"1400 ms"</item>
+        <item>"1300 ms"</item>
+        <item>"1200 ms"</item>
+        <item>"1100 ms"</item>
+        <item>"1000 ms"</item>
+        <item>"900 ms"</item>
+        <item>"800 ms"</item>
+        <item>"700 ms"</item>
+        <item>"600 ms"</item>
+        <item>"500 ms"</item>
+        <item>"400"</item>
+    </string-array>
+    <string-array name="pref_time_thr_values">
+        <item>"0"</item>
+        <item>"1500"</item>
+        <item>"1400"</item>
+        <item>"1300"</item>
+        <item>"1200"</item>
+        <item>"1100"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+    </string-array>
+    <string-array name="pref_mem_thr_keys">
+        <item>"No Limit"</item>
+        <item>"500 nodes"</item>
+        <item>"400 nodes"</item>
+        <item>"300 nodes"</item>
+        <item>"200 nodes"</item>
+        <item>"100 nodes"</item>
+        <item>"50 nodes"</item>
+    </string-array>
+    <string-array name="pref_mem_thr_values">
+        <item>"0"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"50"</item>
+    </string-array>
+    <string-array name="pref_loop_thr_keys">
+        <item>"0.90"</item>
+        <item>"0.80"</item>
+        <item>"0.70"</item>
+        <item>"0.60"</item>
+        <item>"0.50"</item>
+        <item>"0.40"</item>
+        <item>"0.30"</item>
+        <item>"0.20"</item>
+        <item>"0.15"</item>
+        <item>"0.11"</item>
+        <item>"0.10"</item>
+    </string-array>
+    <string-array name="pref_loop_thr_values">
+        <item>"0.90"</item>
+        <item>"0.80"</item>
+        <item>"0.70"</item>
+        <item>"0.60"</item>
+        <item>"0.50"</item>
+        <item>"0.40"</item>
+        <item>"0.30"</item>
+        <item>"0.20"</item>
+        <item>"0.15"</item>
+        <item>"0.11"</item>
+        <item>"0.10"</item>
+    </string-array>
+    <string-array name="pref_sim_thr_keys">
+        <item>"Disabled"</item>
+        <item>"0.60"</item>
+        <item>"0.50"</item>
+        <item>"0.40"</item>
+        <item>"0.30"</item>
+        <item>"0.20"</item>
+        <item>"0.10"</item>
+    </string-array>
+    <string-array name="pref_sim_thr_values">
+        <item>"0"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+    </string-array>
+    <string-array name="pref_min_inliers_keys">
+        <item>"35"</item>
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"20"</item>
+        <item>"15"</item>
+        <item>"10"</item>
+    </string-array>
+    <string-array name="pref_min_inliers_values">
+        <item>"35"</item>
+        <item>"30"</item>
+        <item>"25"</item>
+        <item>"20"</item>
+        <item>"15"</item>
+        <item>"10"</item>
+    </string-array>
+    <string-array name="pref_opt_error_keys">
+        <item>"10x"</item>
+        <item>"9x"</item>
+        <item>"8x"</item>
+        <item>"7x"</item>
+        <item>"6x"</item>
+        <item>"5x"</item>
+        <item>"4x"</item>
+        <item>"3x"</item>
+        <item>"2x"</item>
+        <item>"1x"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_opt_error_values">
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0"</item>
+    </string-array>
+    <string-array name="pref_features_voc_keys">
+        <item>"No Limit"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_features_voc_values">
+        <item>"0"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"-1"</item>
+    </string-array>
+    <string-array name="pref_features_keys">
+        <item>"No Limit"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+    </string-array>
+    <string-array name="pref_features_values">
+        <item>"0"</item>
+        <item>"1000"</item>
+        <item>"900"</item>
+        <item>"800"</item>
+        <item>"700"</item>
+        <item>"600"</item>
+        <item>"500"</item>
+        <item>"400"</item>
+        <item>"300"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+    </string-array>
+    <string-array name="pref_features_type_keys">
+        <item>"BRIEF"</item>
+        <item>"FREAK"</item>
+    </string-array>
+    <string-array name="pref_features_type_values">
+        <item>"6"</item>
+        <item>"5"</item>
+    </string-array>
+    <string-array name="pref_optimizer_keys">
+        <item>"GTSAM"</item>
+        <item>"g2o"</item>
+        <item>"TORO"</item>
+    </string-array>
+    <string-array name="pref_optimizer_values">
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_keys">
+        <item>"Disabled"</item>
+        <item>"4X4_50"</item>
+        <item>"4X4_100"</item>
+        <item>"4X4_250"</item>
+        <item>"4X4_1000"</item>
+        <item>"5X5_50"</item>
+        <item>"5X5_100"</item>
+        <item>"5X5_250"</item>
+        <item>"5X5_1000"</item>
+        <item>"6X6_50"</item>
+        <item>"6X6_100"</item>
+        <item>"6X6_250"</item>
+        <item>"6X6_1000"</item>
+        <item>"7X7_50"</item>
+        <item>"7X7_100"</item>
+        <item>"7X7_250"</item>
+        <item>"7X7_1000"</item>
+        <item>"ARUCO_ORIGINAL"</item>
+        <item>"APRILTAG_16h5"</item>
+        <item>"APRILTAG_25h9"</item>
+        <item>"APRILTAG_36h10"</item>
+        <item>"APRILTAG_36h11"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+        <item>"4"</item>
+        <item>"5"</item>
+        <item>"6"</item>
+        <item>"7"</item>
+        <item>"8"</item>
+        <item>"9"</item>
+        <item>"10"</item>
+        <item>"11"</item>
+        <item>"12"</item>
+        <item>"13"</item>
+        <item>"14"</item>
+        <item>"15"</item>
+        <item>"16"</item>
+        <item>"17"</item>
+        <item>"18"</item>
+        <item>"19"</item>
+        <item>"20"</item>
+    </string-array>
+    
+    <string-array name="pref_marker_detection_depth_error_keys">
+        <item>"1 cm"</item>
+        <item>"2 cm"</item>
+        <item>"3 cm"</item>
+        <item>"4 cm"</item>
+        <item>"5 cm"</item>
+        <item>"6 cm"</item>
+        <item>"7 cm"</item>
+        <item>"8 cm"</item>
+        <item>"9 cm"</item>
+        <item>"10 cm"</item>
+        <item>"15 cm"</item>
+        <item>"20 cm"</item>
+        <item>"30 cm"</item>
+        <item>"40 cm"</item>
+        <item>"50 cm"</item>
+        <item>"100 cm"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_depth_error_values">
+        <item>"0.01"</item>
+        <item>"0.02"</item>
+        <item>"0.03"</item>
+        <item>"0.04"</item>
+        <item>"0.05"</item>
+        <item>"0.06"</item>
+        <item>"0.07"</item>
+        <item>"0.08"</item>
+        <item>"0.09"</item>
+        <item>"0.1"</item>
+        <item>"0.15"</item>
+        <item>"0.20"</item>
+        <item>"0.30"</item>
+        <item>"0.40"</item>
+        <item>"0.50"</item>
+        <item>"1"</item>
+    </string-array>
+
+    <string name="pref_title_export_sub">Exporting&#8230;</string>
+    <string name="pref_title_export">Exporting</string>
+    <string name="pref_summary_export">Advanced parameters used when exporting the map.</string>
+    <string name="pref_title_cloud_voxel">Voxel Size</string>
+    <string name="pref_summary_cloud_voxel">If you don\'t need a very precise point cloud, you can set this to reduce the output point cloud size. This is also used for optimized mesh.</string>
+    <string name="pref_title_texture_size">Texture Size</string>
+    <string name="pref_summary_texture_size">If the map is large, you may want to increase this to maximize the texture resolution.</string>
+    <string name="pref_title_texture_count">Maximum Output Textures</string>
+    <string name="pref_summary_texture_count">Note that all textures are still merged into one for visualization, but exported in multiple files.</string>
+    <string name="pref_title_normal_k">Normal K</string>
+    <string name="pref_summary_normal_k">K-nearest neighbors used for normal computation when a mesh is created.</string>
+    <string name="pref_title_max_texture_distance">Max Texture Distance</string>
+    <string name="pref_summary_max_texture_distance">Maximum distance from a camera for polygons to be textured by this camera.</string>
+    <string name="pref_title_min_texture_cluster_size">Min Texture Cluster Size</string>
+    <string name="pref_summary_min_texture_cluster_size">Minimum polygon cluster size to be textured by a camera. This helps to filter sparse textured polygons.</string>
+    <string name="pref_title_block_render">Block Rendering Thread While Exporting</string>
+    <string name="pref_summary_block_render">This decreases exporting time, but freezes rendering while exporting. This also clears temporary the rendered clouds/meshes from memory during exporting, this can be useful to avoid out of memory errors.</string>
+
+    <string-array name="pref_cloud_voxel_keys">
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.02 m"</item>
+        <item>"0.01 m"</item>
+        <item>"0.005 m"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_cloud_voxel_values">
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.02"</item>
+        <item>"0.01"</item>
+        <item>"0.005"</item>
+        <item>"0"</item>
+    </string-array>
+    <string-array name="pref_texture_size_keys">
+        <item>"16384x16384"</item>
+        <item>"8192x8192"</item>
+        <item>"4096x4096"</item>
+        <item>"2048x2048"</item>
+        <item>"1024x1024"</item>
+    </string-array>
+    <string-array name="pref_texture_size_values">
+        <item>"16384"</item>
+        <item>"8192"</item>
+        <item>"4096"</item>
+        <item>"2048"</item>
+        <item>"1024"</item>
+    </string-array>
+    <string-array name="pref_texture_count_keys">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_texture_count_values">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_normal_k_values">
+        <item>"30"</item>
+        <item>"24"</item>
+        <item>"18"</item>
+        <item>"12"</item>
+        <item>"6"</item>
+    </string-array>
+    <string-array name="pref_max_texture_distance_keys">
+        <item>"No Limit"</item>
+        <item>"5 m"</item>
+        <item>"4.5 m"</item>
+        <item>"4 m"</item>
+        <item>"3.5 m"</item>
+        <item>"3 m"</item>
+        <item>"2.5 m"</item>
+        <item>"2 m"</item>
+    </string-array>
+    <string-array name="pref_max_texture_distance_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4.5"</item>
+        <item>"4"</item>
+        <item>"3.5"</item>
+        <item>"3"</item>
+        <item>"2.5"</item>
+        <item>"2"</item>
+    </string-array>
+    <string-array name="pref_min_texture_cluster_size_keys">
+        <item>"1000"</item>
+        <item>"500"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"50"</item>
+        <item>"10"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_min_texture_cluster_size_values">
+        <item>"1000"</item>
+        <item>"500"</item>
+        <item>"200"</item>
+        <item>"100"</item>
+        <item>"50"</item>
+        <item>"10"</item>
+        <item>"0"</item>
+    </string-array>
+    
+    <string name="pref_title_optimized">Optimized</string>
+    <string name="pref_title_opt_voxel">Voxel Size</string>
+    <string name="pref_summary_opt_voxel">Increasing this can reduce reconstruction time at the cost of less geometry precision.</string>
+    <string name="pref_title_opt_depth">Reconstruction Depth</string>
+    <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth). \"Auto\" means that depth is chosen so that polygon size is just under 3 cm.</string>
+    <string name="pref_title_opt_color_radius">Color Radius</string>
+    <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
+    <string name="pref_title_opt_clean_white">Clean Mesh</string>
+    <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
+	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>
+    <string name="pref_title_opt_min_cluster_size">Polygon Filtering</string>
+    
+    <string-array name="pref_opt_depth_keys">
+        <item>"Auto"</item>
+        <item>"12"</item>
+        <item>"11"</item>
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+    </string-array>
+    <string-array name="pref_opt_depth_values">
+        <item>"0"</item>
+        <item>"12"</item>
+        <item>"11"</item>
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+    </string-array>
+    <string-array name="pref_opt_color_radius_keys">
+        <item>"Nearest"</item>
+        <item>"2 m"</item>
+        <item>"1 m"</item>
+        <item>"0.5 m"</item>
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.025"</item>
+        <item>"0.02"</item>
+        <item>"0.015"</item>
+        <item>"0.01"</item>
+        <item>"Disabled"</item>
+    </string-array>
+    <string-array name="pref_opt_color_radius_values">
+        <item>"0"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0.5"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.025"</item>
+        <item>"0.02"</item>
+        <item>"0.015"</item>
+        <item>"0.01"</item>
+        <item>"-1"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_keys">
+        <item>"Only biggest cluster kept"</item>
+        <item>"Keep all polygons"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+    </string-array>
+
+    <string name="pref_title_general">General</string>
+    <string name="pref_title_gain_max_radius">Color Correction Radius</string>
+    <string name="pref_summary_gain_max_radius">Radius used to find pixel correspondences for Adjust Colors optimization.</string>
+    <string name="pref_title_cluster_ratio">Noise Filtering Ratio</string>
+    <string name="pref_summary_cluster_ratio">Polygon clusters with size smaller than this ratio of the largest cluster are removed by the Noise Filtering optimization.</string>
+    <string name="pref_title_notification_sound">Notification Sound</string>
+    <string name="pref_summary_notification_sound">After saving database or preparing data to export, a notification sound is played.</string>
+
+    <string-array name="pref_gain_max_radius_keys">
+        <item>"0.3 m"</item>
+        <item>"0.2 m"</item>
+        <item>"0.1 m"</item>
+        <item>"0.05 m"</item>
+        <item>"0.025 m"</item>
+        <item>"0.02 m"</item>
+        <item>"0.015 m"</item>
+        <item>"0.01 m"</item>
+    </string-array>
+    <string-array name="pref_gain_max_radius_values">
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.025"</item>
+        <item>"0.02"</item>
+        <item>"0.015"</item>
+        <item>"0.01"</item>
+    </string-array>
+    <string-array name="pref_cluster_ratio_keys">
+        <item>"Keep Largest Only"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.01"</item>
+    </string-array>
+    <string-array name="pref_cluster_ratio_values">
+        <item>"1.0"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.01"</item>
+    </string-array>
+
+    <string name="pref_title_presets">Presets</string>
+    <string name="pref_title_open_button">Open</string>
+    <string name="pref_title_save_button">Save</string>
+    <string name="pref_title_remove_button">Remove</string>
+    <string name="pref_title_reset_button">Restore All Default Settings</string>
+    <string name="title_activity_sketchfab">SketchfabActivity</string>
+    <string name="hello_world">Hello world!</string>
+    <string name="action_settings">Settings</string>
+
 </resources>

app/android/res/values/styles.xml

@@ -0,0 +1,10 @@
+<resources>
+    <style name="ThemeActionBar" parent="@android:style/Widget.DeviceDefault.ActionBar.Solid">
+        <item name="android:background">#20000000</item>
+    </style>
+
+    <style name="ThemeApp" parent="@android:style/Theme.DeviceDefault">
+        <item name="android:actionBarStyle">@style/ThemeActionBar</item>
+        <item name="android:windowActionBarOverlay">true</item>
+    </style>
+</resources>

app/android/res/xml/provider_paths.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<paths xmlns:android="http://schemas.android.com/apk/res/android">
+    <external-path name="external_files" path="."/>
+</paths>

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

@@ -0,0 +1,588 @@
+package com.introlab.rtabmap;
+
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.EnumSet;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+import com.google.ar.core.Camera;
+import com.google.ar.core.CameraIntrinsics;
+import com.google.ar.core.Config;
+import com.google.ar.core.Frame;
+import com.google.ar.core.ImageMetadata;
+import com.google.ar.core.PointCloud;
+import com.google.ar.core.Pose;
+import com.google.ar.core.Session;
+import com.google.ar.core.SharedCamera;
+import com.google.ar.core.TrackingState;
+import com.google.ar.core.exceptions.CameraNotAvailableException;
+import com.google.ar.core.exceptions.NotYetAvailableException;
+import com.google.ar.core.exceptions.UnavailableException;
+
+import android.content.Context;
+import android.graphics.ImageFormat;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCaptureSession;
+import android.hardware.camera2.CameraCharacteristics;
+import android.hardware.camera2.CameraDevice;
+import android.hardware.camera2.CameraManager;
+import android.hardware.camera2.CaptureFailure;
+import android.hardware.camera2.CaptureRequest;
+import android.hardware.camera2.TotalCaptureResult;
+import android.media.Image;
+import android.opengl.GLES20;
+import android.opengl.GLSurfaceView;
+import android.os.Handler;
+import android.os.HandlerThread;
+import android.support.annotation.NonNull;
+import android.util.Log;
+import android.view.Surface;
+
+public class ARCoreSharedCamera {
+
+	public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
+
+
+	private static RTABMapActivity mActivity;
+	public ARCoreSharedCamera(RTABMapActivity c) {
+		mActivity = c;
+	}
+
+	// Depth TOF Image.
+	// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
+	private static final int DEPTH_WIDTH = 240;
+	private static final int DEPTH_HEIGHT = 180;
+
+	// GL Surface used to draw camera preview image.
+	public GLSurfaceView surfaceView;
+
+	// ARCore session that supports camera sharing.
+	private Session sharedSession;
+
+	// Camera capture session. Used by both non-AR and AR modes.
+	private CameraCaptureSession captureSession;
+
+	// Reference to the camera system service.
+	private CameraManager cameraManager;
+
+	// Camera device. Used by both non-AR and AR modes.
+	private CameraDevice cameraDevice;
+
+	// Looper handler thread.
+	private HandlerThread backgroundThread;
+	// Looper handler.
+	private Handler backgroundHandler;
+
+	// ARCore shared camera instance, obtained from ARCore session that supports sharing.
+	private SharedCamera sharedCamera;
+
+	// Camera ID for the camera used by ARCore.
+	private String cameraId;
+	
+	private AtomicBoolean mReady = new AtomicBoolean(false);
+
+	// Camera preview capture request builder
+	private CaptureRequest.Builder previewCaptureRequestBuilder;
+
+	private int cameraTextureId = -1;
+
+	// Image reader that continuously processes CPU images.
+	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
+	private boolean mTOFAvailable = false;
+	
+	public boolean isDepthSupported() {return mTOFAvailable;}
+
+	// Camera device state callback.
+	private final CameraDevice.StateCallback cameraDeviceCallback =
+			new CameraDevice.StateCallback() {
+		@Override
+		public void onOpened(@NonNull CameraDevice cameraDevice) {
+			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " opened.");
+			ARCoreSharedCamera.this.cameraDevice = cameraDevice;
+			createCameraPreviewSession();
+		}
+
+		@Override
+		public void onClosed(@NonNull CameraDevice cameraDevice) {
+			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
+			ARCoreSharedCamera.this.cameraDevice = null;
+		}
+
+		@Override
+		public void onDisconnected(@NonNull CameraDevice cameraDevice) {
+			Log.w(TAG, "Camera device ID " + cameraDevice.getId() + " disconnected.");
+			cameraDevice.close();
+			ARCoreSharedCamera.this.cameraDevice = null;
+		}
+
+		@Override
+		public void onError(@NonNull CameraDevice cameraDevice, int error) {
+			Log.e(TAG, "Camera device ID " + cameraDevice.getId() + " error " + error);
+			cameraDevice.close();
+			ARCoreSharedCamera.this.cameraDevice = null;
+		}
+	};
+
+	// Repeating camera capture session state callback.
+	CameraCaptureSession.StateCallback cameraCaptureCallback =
+			new CameraCaptureSession.StateCallback() {
+
+		// Called when the camera capture session is first configured after the app
+		// is initialized, and again each time the activity is resumed.
+		@Override
+		public void onConfigured(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session configured.");
+			captureSession = session;
+			setRepeatingCaptureRequest();
+		}
+
+		@Override
+		public void onSurfacePrepared(
+				@NonNull CameraCaptureSession session, @NonNull Surface surface) {
+			Log.d(TAG, "Camera capture surface prepared.");
+		}
+
+		@Override
+		public void onReady(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session ready.");
+		}
+
+		@Override
+		public void onActive(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session active.");
+			resumeARCore();
+		}
+
+		@Override
+		public void onClosed(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session closed.");
+		}
+
+		@Override
+		public void onConfigureFailed(@NonNull CameraCaptureSession session) {
+			Log.e(TAG, "Failed to configure camera capture session.");
+		}
+	};
+
+	// Repeating camera capture session capture callback.
+	private final CameraCaptureSession.CaptureCallback captureSessionCallback =
+			new CameraCaptureSession.CaptureCallback() {
+
+		@Override
+		public void onCaptureCompleted(
+				@NonNull CameraCaptureSession session,
+				@NonNull CaptureRequest request,
+				@NonNull TotalCaptureResult result) {
+			Log.i(TAG, "onCaptureCompleted");
+		}
+
+		//@Override // android 23 
+		public void onCaptureBufferLost(
+				@NonNull CameraCaptureSession session,
+				@NonNull CaptureRequest request,
+				@NonNull Surface target,
+				long frameNumber) {
+			Log.e(TAG, "onCaptureBufferLost: " + frameNumber);
+		}
+
+		@Override
+		public void onCaptureFailed(
+				@NonNull CameraCaptureSession session,
+				@NonNull CaptureRequest request,
+				@NonNull CaptureFailure failure) {
+			Log.e(TAG, "onCaptureFailed: " + failure.getFrameNumber() + " " + failure.getReason());
+		}
+
+		@Override
+		public void onCaptureSequenceAborted(
+				@NonNull CameraCaptureSession session, int sequenceId) {
+			Log.e(TAG, "onCaptureSequenceAborted: " + sequenceId + " " + session);
+		}
+	};
+
+	private void resumeARCore() {
+		// Ensure that session is valid before triggering ARCore resume. Handles the case where the user
+		// manually uninstalls ARCore while the app is paused and then resumes.
+		if (sharedSession == null) {
+			return;
+		}
+
+		try {
+			Log.i(TAG, "Resume ARCore.");
+			// Resume ARCore.
+			sharedSession.resume();
+			// Set capture session callback while in AR mode.
+			sharedCamera.setCaptureCallback(captureSessionCallback, backgroundHandler);
+		} catch (CameraNotAvailableException e) {
+			Log.e(TAG, "Failed to resume ARCore session", e);
+			return;
+		}
+
+	}
+
+
+	// Called when starting non-AR mode or switching to non-AR mode.
+	// Also called when app starts in AR mode, or resumes in AR mode.
+	private void setRepeatingCaptureRequest() {
+		try {
+			captureSession.setRepeatingRequest(
+					previewCaptureRequestBuilder.build(), captureSessionCallback, backgroundHandler);
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "Failed to set repeating request", e);
+		}
+	}
+
+
+	private void createCameraPreviewSession() {
+		Log.e(TAG, "createCameraPreviewSession: " + "starting camera preview session.");
+		try {
+			// Note that isGlAttached will be set to true in AR mode in onDrawFrame().
+			sharedSession.setCameraTextureName(cameraTextureId);
+
+			// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
+			previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
+
+			// Build surfaces list, starting with ARCore provided surfaces.
+			List<Surface> surfaceList = sharedCamera.getArCoreSurfaces();
+			Log.e(TAG, " createCameraPreviewSession: " + "surfaceList: sharedCamera.getArCoreSurfaces(): " + surfaceList.size());
+
+			// Add a CPU image reader surface. On devices that don't support CPU image access, the image
+			// may arrive significantly later, or not arrive at all.
+			if (mTOFAvailable) surfaceList.add(mTOFImageReader.imageReader.getSurface());
+			// Surface list should now contain three surfacemReadymReadys:
+			// 0. sharedCamera.getSurfaceTexture()
+			// 1. …
+			// 2. depthImageReader.getSurface()
+
+			// Add ARCore surfaces and CPU image surface targets.
+			for (Surface surface : surfaceList) {
+				previewCaptureRequestBuilder.addTarget(surface);
+			}
+
+			// Wrap our callback in a shared camera callback.
+			CameraCaptureSession.StateCallback wrappedCallback = sharedCamera.createARSessionStateCallback(cameraCaptureCallback, backgroundHandler);
+
+			// Create camera capture session for camera preview using ARCore wrapped callback.
+			cameraDevice.createCaptureSession(surfaceList, wrappedCallback, backgroundHandler);
+			
+			mReady.set(true);
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "CameraAccessException", e);
+		}
+
+
+	}
+
+	// Start background handler thread, used to run callbacks without blocking UI thread.
+	private void startBackgroundThread() {
+		backgroundThread = new HandlerThread("sharedCameraBackground");
+		backgroundThread.start();
+		backgroundHandler = new Handler(backgroundThread.getLooper());
+		mTOFImageReader.startBackgroundThread();
+	}
+
+	// Stop background handler thread.
+	private void stopBackgroundThread() {
+		if (backgroundThread != null) {
+			backgroundThread.quitSafely();
+			try {
+				backgroundThread.join();
+				backgroundThread = null;
+				backgroundHandler = null;
+			} catch (InterruptedException e) {
+				Log.e(TAG, "Interrupted while trying to join background handler thread", e);
+			}
+		}
+		mTOFImageReader.stopBackgroundThread();
+	}
+	
+	private long mPreviousTime = 0;
+
+	// Perform various checks, then open camera device and create CPU image reader.
+	public boolean openCamera() {
+
+		close();
+		
+		startBackgroundThread();
+		
+		mPreviousTime = System.currentTimeMillis();
+
+		if(cameraTextureId == -1)
+		{
+			int[] textures = new int[1];
+			GLES20.glGenTextures(1, textures, 0);
+			cameraTextureId = textures[0];
+		}
+
+		Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
+		// Don't open camera if already opened.
+		if (cameraDevice != null) {
+			return false;
+		}
+
+		if (sharedSession == null) {
+			try {
+				// Create ARCore session that supports camera sharing.
+				sharedSession = new Session(mActivity, EnumSet.of(Session.Feature.SHARED_CAMERA));
+			} catch (UnavailableException e) {
+				Log.e(TAG, "Failed to create ARCore session that supports camera sharing", e);
+				return false;
+			}
+
+			// Enable auto focus mode while ARCore is running.
+			Config config = sharedSession.getConfig();
+			config.setFocusMode(Config.FocusMode.FIXED);
+			config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
+			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
+			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
+			//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
+			sharedSession.configure(config);
+
+		}
+
+		// Store the ARCore shared camera reference.
+		sharedCamera = sharedSession.getSharedCamera();
+		// Store the ID of the camera used by ARCore.
+		cameraId = sharedSession.getCameraConfig().getCameraId();
+		initCamera(mActivity, cameraId, 1);
+		ArrayList<String> resolutions;
+
+		mTOFAvailable = false;
+
+		resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
+		if (resolutions != null) {
+			for( String temp : resolutions) {
+				Log.e(TAG + "DEPTH16 resolution: ", temp);
+			};
+			if (resolutions.size()>0) mTOFAvailable = true;
+		}
+
+		// Color CPU Image.
+		// Use the currently configured CPU image size.
+		//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
+
+		if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
+
+		// When ARCore is running, make sure it also updates our CPU image surface.
+		if (mTOFAvailable) {
+			sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
+		}
+
+		try {
+
+			// Wrap our callback in a shared camera callback.
+			CameraDevice.StateCallback wrappedCallback = sharedCamera.createARDeviceStateCallback(cameraDeviceCallback, backgroundHandler);
+
+			// Store a reference to the camera system service.
+			cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
+
+			// Get the characteristics for the ARCore camera.
+			//CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(this.cameraId);
+
+			// Open the camera device using the ARCore wrapped callback.
+			cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
+			
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "Failed to open camera", e);
+			return false;
+		} catch (IllegalArgumentException e) {
+			Log.e(TAG, "Failed to open camera", e);
+			return false;
+		} catch (SecurityException e) {
+			Log.e(TAG, "Failed to open camera", e);
+			return false;
+		} 
+
+		Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
+		return true;
+	}
+
+
+	// Close the camera device.
+	public void close() {
+		
+		if (sharedSession != null)  {
+			sharedSession.pause();
+		}
+
+		if (captureSession != null) {
+			captureSession.close();
+			captureSession = null;
+		}
+		if (cameraDevice != null) {
+			cameraDevice.close();
+		}
+
+		if (mTOFImageReader.imageReader != null) {
+			mTOFImageReader.imageReader.close();
+			mTOFImageReader.imageReader = null;
+		}
+
+		if(cameraTextureId>=0)
+		{
+			GLES20.glDeleteTextures(1, new int[] {cameraTextureId}, 0);
+		}
+		
+		stopBackgroundThread();
+	}
+
+	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
+
+	// Draw frame when in AR mode. Called on the GL thread.
+	public void updateGL() throws CameraNotAvailableException {
+
+		if(!mReady.get())
+		{
+			return;
+		}
+		
+		if (mTOFAvailable && mTOFImageReader.frameCount == 0) return;
+		
+		// Perform ARCore per-frame update.
+		Frame frame = null;
+		try {
+			frame = sharedSession.update();
+		} catch (Exception e) {
+			e.printStackTrace();
+			return;
+		}
+
+		Camera camera = null;
+		if (frame != null) {
+			camera = frame.getCamera();
+		}else
+		{
+			return;
+		}
+
+		if (camera == null) return;
+		// If not tracking, don't draw 3D objects.
+		if (camera.getTrackingState() == TrackingState.PAUSED) return;
+
+		if (frame.getTimestamp() != 0) {
+			
+			Pose pose = camera.getPose();
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
+			
+			int rateMs = 100; // send images at most 10 Hz
+			if(System. currentTimeMillis() - mPreviousTime < rateMs)
+			{
+				return;
+			}
+			mPreviousTime = System. currentTimeMillis();
+			
+			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
+			try{
+				Image image = frame.acquireCameraImage();
+				PointCloud cloud = frame.acquirePointCloud();
+				FloatBuffer points = cloud.getPoints();
+
+				if (image.getFormat() != ImageFormat.YUV_420_888) {
+					throw new IllegalArgumentException(
+							"Expected image in YUV_420_888 format, got format " + image.getFormat());
+				}
+				
+				if(!RTABMapActivity.DISABLE_LOG)
+				{
+					for(int i =0;i<image.getPlanes().length;++i)
+					{
+						Log.d(TAG, String.format("Plane[%d] pixel stride = %d,  row stride = %d", i, image.getPlanes()[i].getPixelStride(), image.getPlanes()[i].getRowStride()));
+					}
+				}
+				
+				float[] fl = intrinsics.getFocalLength();
+				float[] pp = intrinsics.getPrincipalPoint();
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("fx=%f fy=%f cx=%f cy=%f", fl[0], fl[1], pp[0], pp[1]));
+				
+				ByteBuffer y = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+				ByteBuffer u = image.getPlanes()[1].getBuffer().asReadOnlyBuffer();
+				ByteBuffer v = image.getPlanes()[2].getBuffer().asReadOnlyBuffer();
+
+				double stamp = (double)image.getTimestamp()/10e8;
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
+						image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
+
+				if(mTOFAvailable)
+				{
+					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
+					
+					RTABMapLib.postOdometryEvent(
+							RTABMapActivity.nativeApplication,
+							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], stamp, 
+							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							points, points.limit()/4);
+				}
+				else
+				{
+					ByteBuffer bb = ByteBuffer.allocate(0);
+					RTABMapLib.postOdometryEvent(
+							RTABMapActivity.nativeApplication,
+							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], stamp, 
+							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							bb, 0, 0, 0, ImageFormat.DEPTH16,
+							points, points.limit()/4);
+				}
+				
+				image.close();
+				cloud.close();
+				
+			} catch (NotYetAvailableException e) {
+
+			}
+		}
+	}
+
+	/********************************************************************************************************************* */
+	/*************************************************** End ************************************************************* */
+	/********************************************************************************************************************* */
+
+
+	public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
+		Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
+
+		ArrayList<String> output = new ArrayList<String>();
+		try {
+			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
+			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
+
+			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(imageFormat)) {
+				output.add(s.getWidth() + "x" + s.getHeight());
+			}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
+		return output;
+	}
+
+
+	public void initCamera (Context context, String cameraId,int index){
+		boolean ok = false;
+		try {
+			int current = 0;
+			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
+			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
+			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.DEPTH16)) {
+				ok = true;
+				if (current == index)
+					break;
+				else ;
+				current++;
+			}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
+		if (!ok) {
+			Log.e(TAG + " initCamera", "Depth sensor not found!");
+		}
+	}
+
+
+}

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

@@ -8,11 +8,13 @@ import java.io.BufferedReader;
 import java.io.IOException;
 import java.io.InputStream;
 import java.io.InputStreamReader;
+
 import android.app.Dialog;
 import android.content.Context;
 import android.graphics.Color;
 import android.os.Bundle;
 import android.text.Html;
+import android.text.method.LinkMovementMethod;
 import android.text.util.Linkify;
 import android.widget.TextView;
 
@@ -33,8 +35,10 @@ public class AboutDialog extends Dialog{
 		setContentView(R.layout.about);
 		TextView tv = (TextView)findViewById(R.id.info_text);
 		tv.setText(Html.fromHtml(readRawTextFile(R.raw.info)));
+		tv.setMovementMethod(LinkMovementMethod.getInstance());
 		tv.setLinkTextColor(Color.WHITE);
 		Linkify.addLinks(tv, Linkify.ALL);
+		tv.append(Html.fromHtml("<b><a href=http://introlab.github.io/rtabmap/index.html#privacy-policy>Privacy Policy</a></b>"));
 	}
 	
 	public static String readRawTextFile(int id) {

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

@@ -0,0 +1,82 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.preference.SwitchPreference;
+import android.util.AttributeSet;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.Switch;
+
+/**
+ * 
+ * @author mathieu
+ * Bug fix of switch preferences changing states 
+ * when scrolling on Jelly Bean: 
+ * https://issuetracker.google.com/issues/36941388#comment4
+ */
+
+public class CustomSwitchPreference extends SwitchPreference { 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     * @param defStyle Theme attribute defining the default style options 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs, int defStyle) { 
+        super(context, attrs, defStyle); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs) { 
+        super(context, attrs); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with default style options. 
+     * 
+     * @param context The Context that will style this preference 
+     */ 
+    public CustomSwitchPreference(Context context) { 
+        super(context, null); 
+    } 
+
+    @Override 
+    protected void onBindView(View view) { 
+        // Clean listener before invoke SwitchPreference.onBindView 
+        ViewGroup viewGroup= (ViewGroup)view; 
+        clearListenerInViewGroup(viewGroup); 
+        super.onBindView(view); 
+    } 
+
+    /** 
+     * Clear listener in Switch for specify ViewGroup. 
+     * 
+     * @param viewGroup The ViewGroup that will need to clear the listener. 
+     */ 
+    private void clearListenerInViewGroup(ViewGroup viewGroup) { 
+        if (null == viewGroup) { 
+            return; 
+        } 
+
+        int count = viewGroup.getChildCount(); 
+        for(int n = 0; n < count; ++n) { 
+            View childView = viewGroup.getChildAt(n); 
+            if(childView instanceof Switch) { 
+                final Switch switchView = (Switch) childView; 
+                switchView.setOnCheckedChangeListener(null); 
+                return; 
+            } else if (childView instanceof ViewGroup){ 
+                ViewGroup childGroup = (ViewGroup)childView; 
+                clearListenerInViewGroup(childGroup); 
+            } 
+        } 
+    } 
+
+}

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

@@ -0,0 +1,102 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.database.Cursor;
+import android.database.sqlite.SQLiteDatabase;
+import android.graphics.Bitmap;
+import android.graphics.BitmapFactory;
+import android.graphics.drawable.Drawable;
+import android.util.Log;
+import android.view.LayoutInflater;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.BaseAdapter;
+import android.widget.ImageView;
+import android.widget.LinearLayout;
+import android.widget.SimpleAdapter;
+
+import java.io.ByteArrayInputStream;
+import java.io.InputStream;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+public class DatabaseListArrayAdapter extends SimpleAdapter {
+	LayoutInflater inflater;
+	Context context;
+	ArrayList<HashMap<String, String>> arrayList;
+	int imageWidth;
+
+	public DatabaseListArrayAdapter(Context context, ArrayList<HashMap<String, String>> data, int resource, String[] from, int[] to) {
+		super(context, data, resource, from, to);
+		this.context = context;
+		this.arrayList = data;
+		this.imageWidth = (int)context.getResources().getDimension(R.dimen.image_width);
+		inflater.from(context);
+	}
+
+	@Override
+	public View getView(final int position, View convertView, ViewGroup parent) {
+		View view = super.getView(position, convertView, parent);
+		ImageView imageView = (ImageView) view.findViewById(R.id.imageView);
+		
+		boolean imageSet = false;
+		String path = this.arrayList.get(position).get("path");
+		if(!path.isEmpty())
+		{
+			SQLiteDatabase db = null;
+			try {
+				db = SQLiteDatabase.openDatabase(path, null, SQLiteDatabase.OPEN_READONLY);
+				
+				// get version
+				Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);    
+		        if(c1.moveToFirst()) {
+		        	String version = c1.getString(c1.getColumnIndex("version"));
+		        	Log.i(RTABMapActivity.TAG, "Version="+version);
+		        	if(Util.versionCompare(version, "0.12.0") >= 0) {
+		        		Cursor c2 = db.rawQuery("SELECT preview_image FROM Admin WHERE preview_image is not null", null); 
+		        		if(c2.moveToFirst()) {
+		        			Log.i(RTABMapActivity.TAG, "Found image preview for db " + path);
+
+		        			byte[] bytes = c2.getBlob(c2.getColumnIndex("preview_image"));
+		        			ByteArrayInputStream inputStream = new ByteArrayInputStream(bytes);
+		        			Bitmap bitmap = BitmapFactory.decodeStream(inputStream);
+		        			imageView.setImageBitmap(bitmap);
+		        			imageSet = true;
+		        		}
+		        		else {
+		        			Log.i(RTABMapActivity.TAG, "Not found image preview for db " + path);
+		        		}
+		        	}
+		        	else {
+		        		Log.i(RTABMapActivity.TAG, "Too old database for preview image, path = " + path);
+		        	}
+		        }
+		        else {
+		        	Log.e(RTABMapActivity.TAG, "Failed getting version from database");
+		        }
+	
+	        } catch (Exception e) {
+	            Log.e(RTABMapActivity.TAG, e.getMessage());
+	        }
+			finally {
+				if(db != null && db.isOpen()) {
+	            	db.close();
+	            }
+			}
+		}
+		else
+		{
+			Log.e(RTABMapActivity.TAG, "Database path empty for item " + position);
+		}
+		
+		if(!imageSet)
+		{
+			Drawable myDrawable = context.getResources().getDrawable(R.drawable.ic_launcher);
+			imageView.setImageDrawable(myDrawable);
+		}
+		LinearLayout.LayoutParams layoutParams = new LinearLayout.LayoutParams(imageWidth,imageWidth/(!imageSet?2:1));
+		imageView.setLayoutParams(layoutParams);
+		return view;
+	}
+
+}

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

@@ -0,0 +1,148 @@
+package com.introlab.rtabmap;
+ 
+import java.io.BufferedReader;
+ 
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.io.InputStreamReader;
+import java.io.OutputStream;
+import java.io.OutputStreamWriter;
+import java.io.PrintWriter;
+import java.net.HttpURLConnection;
+import java.net.URL;
+import java.net.URLConnection;
+import java.util.ArrayList;
+import java.util.List;
+ 
+/**
+ * This utility class provides an abstraction layer for sending multipart HTTP
+ * POST requests to a web server.
+ * @author www.codejava.net
+ * http://www.codejava.net/java-se/networking/upload-files-by-sending-multipart-request-programmatically
+ *
+ */
+public class MultipartUtility {
+    private final String boundary;
+    private static final String LINE_FEED = "\r\n";
+    private HttpURLConnection httpConn;
+    private String charset;
+    private OutputStream outputStream;
+    private PrintWriter writer;
+ 
+    /**
+     * This constructor initializes a new HTTP POST request with content type
+     * is set to multipart/form-data
+     * @param requestURL
+     * @param charset
+     * @throws IOException
+     */
+    public MultipartUtility(String requestURL, String token, String charset)
+            throws IOException {
+        this.charset = charset;
+         
+        // creates a unique boundary based on time stamp
+        boundary = "===" + System.currentTimeMillis() + "===";
+         
+        URL url = new URL(requestURL);
+        httpConn = (HttpURLConnection) url.openConnection();
+        httpConn.setUseCaches(false);
+        httpConn.setDoOutput(true); // indicates POST method
+        httpConn.setDoInput(true);
+        httpConn.setRequestProperty("Content-Type", "multipart/form-data; boundary=" + boundary);
+        httpConn.setRequestProperty("Authorization", "Bearer " + token);
+        outputStream = httpConn.getOutputStream();
+        writer = new PrintWriter(new OutputStreamWriter(outputStream, charset),
+                true);
+    }
+ 
+    /**
+     * Adds a form field to the request
+     * @param name field name
+     * @param value field value
+     */
+    public void addFormField(String name, String value) {
+        writer.append("--" + boundary).append(LINE_FEED);
+        writer.append("Content-Disposition: form-data; name=\"" + name + "\"").append(LINE_FEED);
+        writer.append("Content-Type: text/plain; charset=" + charset).append(LINE_FEED);
+        writer.append(LINE_FEED);
+        writer.append(value).append(LINE_FEED);
+        writer.flush();
+    }
+ 
+    /**
+     * Adds a upload file section to the request
+     * @param fieldName name attribute in <input type="file" name="..." />
+     * @param uploadFile a File to be uploaded
+     * @throws IOException
+     */
+    public void addFilePart(String fieldName, File uploadFile)
+            throws IOException {
+        String fileName = uploadFile.getName();
+        writer.append("--" + boundary).append(LINE_FEED);
+        writer.append(
+                "Content-Disposition: form-data; name=\"" + fieldName
+                        + "\"; filename=\"" + fileName + "\"")
+                .append(LINE_FEED);
+        writer.append(
+                "Content-Type: "
+                        + URLConnection.guessContentTypeFromName(fileName))
+                .append(LINE_FEED);
+        writer.append("Content-Transfer-Encoding: binary").append(LINE_FEED);
+        writer.append(LINE_FEED);
+        writer.flush();
+ 
+        FileInputStream inputStream = new FileInputStream(uploadFile);
+        byte[] buffer = new byte[4096];
+        int bytesRead = -1;
+        while ((bytesRead = inputStream.read(buffer)) != -1) {
+            outputStream.write(buffer, 0, bytesRead);
+        }
+        outputStream.flush();
+        inputStream.close();
+         
+        writer.append(LINE_FEED);
+        writer.flush();    
+    }
+ 
+    /**
+     * Adds a header field to the request.
+     * @param name - name of the header field
+     * @param value - value of the header field
+     */
+    public void addHeaderField(String name, String value) {
+        writer.append(name + ": " + value).append(LINE_FEED);
+        writer.flush();
+    }
+     
+    /**
+     * Completes the request and receives response from the server.
+     * @return a list of Strings as response in case the server returned
+     * status OK, otherwise an exception is thrown.
+     * @throws IOException
+     */
+    public List<String> finish() throws IOException {
+        List<String> response = new ArrayList<String>();
+ 
+        writer.append(LINE_FEED).flush();
+        writer.append("--" + boundary + "--").append(LINE_FEED);
+        writer.close();
+ 
+        // checks server's status code first
+        int status = httpConn.getResponseCode();
+        if (status == 201) {
+            BufferedReader reader = new BufferedReader(new InputStreamReader(
+                    httpConn.getInputStream()));
+            String line = null;
+            while ((line = reader.readLine()) != null) {
+                response.add(line);
+            }
+            reader.close();
+            httpConn.disconnect();
+        } else {
+            throw new IOException("Server returned non-OK status: " + status + ", msg: " + httpConn.getResponseMessage());
+        }
+ 
+        return response;
+    }
+}

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

@@ -0,0 +1,43 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.util.AttributeSet;
+import android.widget.Spinner;
+
+
+/** Spinner extension that calls onItemSelected even when the selection is the same as its previous value 
+Author: Mattia Ruggiero 
+https://stackoverflow.com/questions/5335306/how-can-i-get-an-event-in-android-spinner-when-the-current-selected-item-is-sele
+*/
+public class NDSpinner extends Spinner {
+
+    public NDSpinner(Context context)
+    { super(context); }
+
+    public NDSpinner(Context context, AttributeSet attrs)
+    { super(context, attrs); }
+
+    public NDSpinner(Context context, AttributeSet attrs, int defStyle)
+    { super(context, attrs, defStyle); }
+
+    @Override 
+    public void setSelection(int position, boolean animate) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position, animate);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            //getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    } 
+
+    @Override
+    public void setSelection(int position) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    }
+
+}

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

@@ -0,0 +1,79 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package com.introlab.rtabmap;
+
+import android.Manifest;
+
+import android.app.Activity;
+import android.content.Intent;
+import android.content.pm.PackageManager;
+import android.net.Uri;
+import android.provider.Settings;
+import android.support.v4.app.ActivityCompat;
+import android.support.v4.content.ContextCompat;
+
+/** Helper to ask camera permission. */
+public class PermissionHelper {
+	public static final int CAMERA_CODE = 0;
+	public static final int READ_EXTERNAL_STORAGE_CODE = 1;
+	public static final int WRITE_EXTERNAL_STORAGE_CODE = 2;
+	public static final int INTERNET_CODE = 3;
+	public static final int ACCESS_NETWORK_STATE_CODE = 4;
+	public static final int ACCESS_FINE_LOCATION_CODE = 5;
+	public static final int ACCESS_WIFI_STATE_CODE = 6;
+
+	/** Check to see we have the necessary permissions for this app. */
+	public static boolean hasPermission(Activity activity, String permission) {
+		return ContextCompat.checkSelfPermission(activity, permission) == PackageManager.PERMISSION_GRANTED;
+	}
+
+	/** Check to see we have the necessary permissions for this app, and ask for them if we don't. */
+	public static void requestPermission(Activity activity, String permission) {
+		int requestCode = -1;
+		if (permission == Manifest.permission.CAMERA) {
+			requestCode = CAMERA_CODE;
+		} else if(permission == Manifest.permission.READ_EXTERNAL_STORAGE) {
+			requestCode = READ_EXTERNAL_STORAGE_CODE;
+		} else if(permission == Manifest.permission.WRITE_EXTERNAL_STORAGE) {
+			requestCode = WRITE_EXTERNAL_STORAGE_CODE;
+		} else if(permission == Manifest.permission.INTERNET) {
+			requestCode = INTERNET_CODE;
+		} else if(permission == Manifest.permission.ACCESS_NETWORK_STATE) {
+			requestCode = ACCESS_NETWORK_STATE_CODE;
+		} else if(permission == Manifest.permission.ACCESS_FINE_LOCATION) {
+			requestCode = ACCESS_FINE_LOCATION_CODE;
+		} else if(permission == Manifest.permission.ACCESS_WIFI_STATE) {
+			requestCode = ACCESS_WIFI_STATE_CODE;
+		}
+		if(requestCode >=0)
+		{
+			ActivityCompat.requestPermissions(
+					activity, new String[] {permission}, requestCode);
+		}
+	}
+
+	/** Check to see if we need to show the rationale for this permission. */
+	public static boolean shouldShowRequestPermissionRationale(Activity activity, String permission) {
+		return ActivityCompat.shouldShowRequestPermissionRationale(activity, permission);
+	}
+
+	/** Launch Application Setting to grant permission. */
+	public static void launchPermissionSettings(Activity activity) {
+		Intent intent = new Intent();
+		intent.setAction(Settings.ACTION_APPLICATION_DETAILS_SETTINGS);
+		intent.setData(Uri.fromParts("package", activity.getPackageName(), null));
+		activity.startActivity(intent);
+	}
+}

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

@@ -1,5 +1,10 @@
 
 package com.introlab.rtabmap;
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+
+import android.app.Activity;
+import android.content.Context;
 import android.os.IBinder;
 import android.view.KeyEvent;
 import android.util.Log;
@@ -15,7 +20,7 @@ public class RTABMapLib
         // the correct library first.
         if (TangoInitializationHelper.loadTangoSharedLibrary() ==
                 TangoInitializationHelper.ARCH_ERROR) {
-            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+            Log.w(RTABMapActivity.class.getSimpleName(), "WArning! Unable to load libtango_client_api.so! This can be safely ignored if RTAB-Map NDK is not build with tango support.");
         }
         System.loadLibrary("NativeRTABMap");
     }
@@ -23,70 +28,118 @@ public class RTABMapLib
  // Initialize the Tango Service, this function starts the communication
     // between the application and Tango Service.
     // The activity object is used for checking if the API version is outdated.
-    public static native void onCreate(RTABMapActivity activity);
+    public static native long createNativeApplication(RTABMapActivity activity);
     
-    public static native void openEmptyDatabase();
-    public static native void openDatabase(String databasePath);
+    public static native void destroyNativeApplication(long nativeApplication);
     
-    /*
-     * Called when the Tango service is connected.
-     *
-     * @param binder The native binder object.
-     */
-    public static native boolean onTangoServiceConnected(IBinder binder);
-
-    // Release all non OpenGl resources that are allocated from the program.
-    public static native void onPause();
+    public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
+    
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase2(long nativeApplication, String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
+    
+    public static native boolean isBuiltWith(long nativeApplication, int cameraDriver);
+    public static native boolean startCamera(long nativeApplication, IBinder binder, Context context, Activity activity, int driver);
+    public static native void stopCamera(long nativeApplication);
 
     // Allocate OpenGL resources for rendering.
-    public static native void initGlContent();
+    public static native void initGlContent(long nativeApplication);
 
     // Setup the view port width and height.
-    public static native void setupGraphic(int width, int height);
+    public static native void setupGraphic(long nativeApplication, int width, int height);
 
     // Main render loop.
-    public static native int render();
+    public static native int render(long nativeApplication);
 
     // Set the render camera's viewing angle:
     //   first person, third person, or top down.
-    public static native void setCamera(int cameraIndex);
+    public static native void setCamera(long nativeApplication, int cameraIndex);
     
     // Pass touch events to the native layer.
-    public static native void onTouchEvent(int touchCount, int event0,
+    public static native void onTouchEvent(long nativeApplication, int touchCount, int event0,
                                            float x0, float y0, float x1, float y1);
     
+    public static native void setPausedMapping(long nativeApplication, boolean paused);
+    public static native void setOnlineBlending(long nativeApplication, boolean enabled);
+    public static native void setMapCloudShown(long nativeApplication, boolean shown);
+    public static native void setOdomCloudShown(long nativeApplication, boolean shown);
+    public static native void setMeshRendering(long nativeApplication, boolean enabled, boolean withTexture);
+    public static native void setLocalizationMode(long nativeApplication, boolean enabled);
+    public static native void setTrajectoryMode(long nativeApplication, boolean enabled);
+    public static native void setGraphOptimization(long nativeApplication, boolean enabled);
+    public static native void setNodesFiltering(long nativeApplication, boolean enabled);
+    public static native void setGraphVisible(long nativeApplication, boolean visible);
+    public static native void setGridVisible(long nativeApplication, boolean visible);
+    public static native void setRawScanSaved(long nativeApplication, boolean enabled);
+    public static native void setFullResolution(long nativeApplication, boolean enabled);
+    public static native void setSmoothing(long nativeApplication, boolean enabled);
+    public static native void setCameraColor(long nativeApplication, boolean enabled);
+    public static native void setAppendMode(long nativeApplication, boolean enabled);
+    public static native void setDataRecorderMode(long nativeApplication, boolean enabled);
+    public static native void setMaxCloudDepth(long nativeApplication, float value);
+    public static native void setMinCloudDepth(long nativeApplication, float value);
+    public static native void setPointSize(long nativeApplication, float value);
+    public static native void setFOV(long nativeApplication, float value);
+    public static native void setOrthoCropFactor(long nativeApplication, float value);
+    public static native void setGridRotation(long nativeApplication, float value);
+    public static native void setLighting(long nativeApplication, boolean enabled);
+    public static native void setBackfaceCulling(long nativeApplication, boolean enabled);
+    public static native void setWireframe(long nativeApplication, boolean enabled);
+    public static native void setCloudDensityLevel(long nativeApplication, int value);
+    public static native void setMeshAngleTolerance(long nativeApplication, float value);
+    public static native void setMeshTriangleSize(long nativeApplication, int value);
+    public static native void setClusterRatio(long nativeApplication, float value);
+    public static native void setMaxGainRadius(long nativeApplication, float value);
+    public static native void setRenderingTextureDecimation(long nativeApplication, int value);
+    public static native void setBackgroundColor(long nativeApplication, float gray);
+    public static native int setMappingParameter(long nativeApplication, String key, String value);
+    public static native void setGPS(
+    		long nativeApplication, 
+			double stamp,
+			double longitude, 
+			double latitude, 
+			double altitude,  
+			double accuracy,
+			double bearing);
+    public static native void addEnvSensor(long nativeApplication, int type, float value);
 
-    public static native void setPausedMapping(boolean paused);
-    public static native void setMapCloudShown(boolean shown);
-    public static native void setOdomCloudShown(boolean shown);
-    public static native void setMeshRendering(boolean enabled, boolean withTexture);
-    public static native void setLocalizationMode(boolean enabled);
-    public static native void setTrajectoryMode(boolean enabled);
-    public static native void setGraphOptimization(boolean enabled);
-    public static native void setNodesFiltering(boolean enabled);
-    public static native void setDriftCorrection(boolean enabled);
-    public static native void setGraphVisible(boolean visible);
-    public static native void setGridVisible(boolean visible);
-    public static native void setAutoExposure(boolean enabled);
-    public static native void setFullResolution(boolean enabled);
-    public static native void setAppendMode(boolean enabled);
-    public static native void setDataRecorderMode(boolean enabled);
-    public static native void setMaxCloudDepth(float value);
-    public static native void setMeshDecimation(int value);
-    public static native void setMeshAngleTolerance(float value);
-    public static native void setMeshTriangleSize(int value);
-    public static native int setMappingParameter(String key, String value);
-
-    public static native void resetMapping();
-    public static native void save(String outputDatabasePath);
-    public static native boolean exportMesh(String filePath);
-    public static native int postProcessing(int approach);
+    public static native void save(long nativeApplication, String outputDatabasePath);
+    public static native void cancelProcessing(long nativeApplication);
+    public static native boolean exportMesh(
+    		long nativeApplication, 
+    		float cloudVoxelSize,
+    		boolean regenerateCloud,
+    		boolean meshing,
+    		int textureSize,
+    		int textureCount,
+    		int normalK,
+    		boolean optimized,
+    		float optimizedVoxelSize,
+    		int optimizedDepth,
+    		int optimizedMaxPolygons,
+    		float optimizedColorRadius,
+    		boolean optimizedCleanWhitePolygons,
+    		int optimizedMinClusterSize,
+    		float optimizedMaxTextureDistance,
+    		int optimizedMinTextureClusterSize,
+    		boolean blockRendering);
+    public static native boolean writeExportedMesh(long nativeApplication, String directory, String name);
+    public static native boolean postExportation(long nativeApplication, boolean visualize);
+    public static native int postProcessing(long nativeApplication, int approach);
     
-    public static native String getStatus();
-    public static native int getTotalNodes();
-    public static native int getTotalWords();
-    public static native int getTotalPoints();
-    public static native float getUpdateTime();
-    public static native int getLoopClosureId();
+    public static native String getStatus(long nativeApplication);
+    public static native int getTotalNodes(long nativeApplication);
+    public static native int getTotalWords(long nativeApplication);
+    public static native int getTotalPoints(long nativeApplication);
+    public static native float getUpdateTime(long nativeApplication);
+    public static native int getLoopClosureId(long nativeApplication);
+    
+    public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw);
+    public static native void postOdometryEvent(long nativeApplication,
+    		float x, float y, float z, float qx, float qy, float qz, float qw,
+    		float fx, float fy, float cx, float cy,
+    		double stamp, 
+    		ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+    		ByteBuffer depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+    		FloatBuffer points, int pointsLen);
     
 }

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

@@ -16,8 +16,16 @@
 
 package com.introlab.rtabmap;
 
+import java.util.Vector;
+import java.util.concurrent.locks.ReentrantLock;
+
+import android.app.Activity;
 import android.app.ProgressDialog;
+import android.opengl.GLES20;
 import android.opengl.GLSurfaceView;
+import android.opengl.Matrix;
+import android.util.Log;
+import android.widget.Toast;
 
 import javax.microedition.khronos.egl.EGLConfig;
 import javax.microedition.khronos.opengles.GL10;
@@ -26,30 +34,217 @@ import javax.microedition.khronos.opengles.GL10;
 // ground grid, camera frustum, camera axis, and trajectory based on the Tango
 // device's pose.
 public class Renderer implements GLSurfaceView.Renderer {
+
+	private final float[] mtrxProjection = new float[16];
+	private final float[] mtrxView = new float[16];
+	private final float[] mtrxProjectionAndView = new float[16];
+
+	private TextManager mTextManager = null;
+	private float mSurfaceHeight = 0.0f;
+	private float mTextColor = 1.0f;
+	private int mOffset = 0;
+	private ARCoreSharedCamera mCamera = null;
 	
-	private ProgressDialog mProgressDialog;
+	private Vector<TextObject> mTexts;
+
+	private static RTABMapActivity mActivity;
+	public Renderer(RTABMapActivity c) {
+		mActivity = c;
+	}
+
+	private ProgressDialog mProgressDialog = null;
+	private Toast mToast = null;
 	
+	private boolean mTextChanged = false;
+	private ReentrantLock mTextLock = new ReentrantLock();
+
 	public void setProgressDialog(ProgressDialog progressDialog)
 	{
 		mProgressDialog = progressDialog;
 	}
+
+	public void setToast(Toast toast)
+	{
+		mToast = toast;
+	}
 	
-  // Render loop of the Gl context.
-  public void onDrawFrame(GL10 gl) {
-    int value = RTABMapLib.render();
-    if(value == 1 && mProgressDialog != null)
-    {
-    	mProgressDialog.dismiss();
-    }
-  }
+	public void setOffset(int offset)
+	{
+		mOffset = offset;
+	}
+	
+	public void setCamera(ARCoreSharedCamera camera)
+	{
+		mCamera = camera;
+	}
 
-  // Called when the surface size changes.
-  public void onSurfaceChanged(GL10 gl, int width, int height) {
-	  RTABMapLib.setupGraphic(width, height);
-  }
+	// Render loop of the Gl context.
+	public void onDrawFrame(GL10 useGLES20instead) {
 
-  // Called when the surface is created or recreated.
-  public void onSurfaceCreated(GL10 gl, EGLConfig config) {
-	  RTABMapLib.initGlContent();
-  }
+		synchronized (this) {
+			if(mActivity.nativeApplication != 0)
+			{
+				try
+				{
+					if(mCamera!=null)
+					{
+						mCamera.updateGL();
+					}
+					
+					final int value = RTABMapLib.render(mActivity.nativeApplication);
+		
+					if(mTextManager!=null)
+					{
+						if(mTextChanged)
+						{
+							mTextChanged = false;
+							Vector<TextObject> txtcollection = new Vector<TextObject>();
+							
+							mTextLock.lock();
+						    try {
+						    	if(mTexts.size() > 0)
+						    	{
+						    		txtcollection.addAll(mTexts);
+						    	}
+						    } finally {
+						    	mTextLock.unlock();
+						    }
+						    
+							// Prepare the text for rendering
+							mTextManager.PrepareDraw(txtcollection);
+						}
+						
+						float[] mvp = new float[16];
+						Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
+						mTextManager.Draw(mvp);
+					}
+		
+					if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
+								mProgressDialog.dismiss();
+								mActivity.resetNoTouchTimer();
+							}
+						});
+					}
+					if(value==-1)
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(mToast!=null)
+								{
+									mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+								}
+							}
+						});
+					}
+					else if(value==-2)
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(mToast!=null)
+								{
+									mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+								}
+							}
+						});
+					}
+				}
+				catch(final Exception e)
+				{
+					mActivity.runOnUiThread(new Runnable() {
+						public void run() {
+							if(mToast!=null)
+							{
+								mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+							}
+						}
+		
+					});
+				}
+			}
+		}
+	}
+
+	// Called when the surface size changes.
+	public void onSurfaceChanged(GL10 useGLES20instead, int width, int height) {
+		
+		if(mActivity.nativeApplication!=0)
+		{
+			RTABMapLib.setupGraphic(mActivity.nativeApplication, width, height);
+		}
+		
+		mSurfaceHeight = (float)height;
+
+		// Clear our matrices
+		for(int i=0;i<16;i++)
+		{
+			mtrxProjection[i] = 0.0f;
+			mtrxView[i] = 0.0f;
+			mtrxProjectionAndView[i] = 0.0f;
+		}
+
+		// Setup our screen width and height for normal sprite translation.
+		Matrix.orthoM(mtrxProjection, 0, 0f, width, 0.0f, height, 0, 50);
+
+		// Set the camera position (View matrix)
+		Matrix.setLookAtM(mtrxView, 0, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1.0f, 0.0f);
+
+		// Calculate the projection and view transformation
+		Matrix.multiplyMM(mtrxProjectionAndView, 0, mtrxProjection, 0, mtrxView, 0);
+	}
+
+	// Called when the surface is created or recreated.
+	public void onSurfaceCreated(GL10 useGLES20instead, EGLConfig config) {
+
+		if(mActivity.nativeApplication != 0)
+		{
+			RTABMapLib.initGlContent(mActivity.nativeApplication);
+		}
+		
+		// Create our text manager
+		mTextManager = new TextManager(mActivity);
+		mTextManager.setColor(mTextColor);
+	}
+	
+	public void updateTexts(String[] texts)
+	{
+		if(mTextManager != null && mSurfaceHeight > 0.0f)
+		{
+			Vector<TextObject> textObjects = new Vector<TextObject>();
+			float offset = mSurfaceHeight-mTextManager.getMaxTextHeight();
+			if(texts != null)
+			{
+				for(int i=0;i<texts.length; ++i)
+				{
+					if(texts[i]!=null && texts[i].length()>0)
+					{
+						TextObject txt = new TextObject(texts[i], 0, offset);
+						textObjects.add(txt);
+					}
+					offset-=mTextManager.getMaxTextHeight();
+				}
+			}
+	
+			mTextLock.lock();
+			try {
+				mTexts = textObjects;
+			} finally {
+				mTextLock.unlock();
+			}
+	
+			mTextChanged = true;
+		}
+	}
+	
+	public void setTextColor(float color) 
+	{
+		mTextColor = color;
+		if(mTextManager != null)
+		{
+			mTextManager.setColor(mTextColor);
+		}
+	}
 }

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

@@ -0,0 +1,387 @@
+package com.introlab.rtabmap;
+
+import java.io.File;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Iterator;
+import java.util.Map.Entry;
+
+import android.Manifest;
+import android.app.AlertDialog;
+import android.content.DialogInterface;
+import android.content.SharedPreferences;
+import android.content.SharedPreferences.OnSharedPreferenceChangeListener;
+import android.content.pm.PackageManager;
+import android.os.Bundle;
+import android.preference.CheckBoxPreference;
+import android.preference.ListPreference;
+import android.preference.Preference;
+import android.preference.PreferenceActivity;
+import android.text.InputType;
+import android.view.WindowManager;
+import android.view.inputmethod.EditorInfo;
+import android.widget.EditText;
+import android.widget.Toast;
+
+public class SettingsActivity extends PreferenceActivity implements OnSharedPreferenceChangeListener {
+	
+	private SettingsActivity getActivity() {return this;}
+	
+    @Override
+    public void onCreate(Bundle savedInstanceState) {
+        super.onCreate(savedInstanceState);
+        addPreferencesFromResource(R.layout.activity_settings);
+        
+        Preference buttonReset = findPreference(getString(R.string.pref_key_reset_button));
+        buttonReset.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		getPreferenceScreen().getSharedPreferences().edit().clear().commit();
+        	
+                recreate();
+        		
+        		return true;
+        	}
+        });
+        
+        Preference buttonOpen = findPreference(getString(R.string.pref_key_open_button));
+        buttonOpen.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Choose Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						//sp1 is the shared pref to copy to
+        						SharedPreferences.Editor ed = getPreferenceScreen().getSharedPreferences().edit(); 
+        						SharedPreferences sp = getActivity().getSharedPreferences(files[which], MODE_PRIVATE); //The shared preferences to copy from
+        						//Cycle through all the entries in the sp
+        						for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+        						 Object v = entry.getValue(); 
+        						 String key = entry.getKey();
+        						 //Now we just figure out what type it is, so we can copy it.
+        						 // Note that i am using Boolean and Integer instead of boolean and int.
+        						 // That's because the Entry class can only hold objects and int and boolean are primatives.
+        						 if(v instanceof Boolean) 
+        						 // Also note that i have to cast the object to a Boolean 
+        						 // and then use .booleanValue to get the boolean
+        						    ed.putBoolean(key, ((Boolean)v).booleanValue());
+        						 else if(v instanceof Float)
+        						    ed.putFloat(key, ((Float)v).floatValue());
+        						 else if(v instanceof Integer)
+        						    ed.putInt(key, ((Integer)v).intValue());
+        						 else if(v instanceof Long)
+        						    ed.putLong(key, ((Long)v).longValue());
+        						 else if(v instanceof String)
+        						    ed.putString(key, ((String)v));         
+        						}
+        						ed.commit(); //save it.
+        						recreate();
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonSave = findPreference(getString(R.string.pref_key_save_button));
+        buttonSave.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+    			builder.setTitle("Save Presets:");
+    			final EditText input = new EditText(getActivity());
+    			input.setInputType(InputType.TYPE_CLASS_TEXT); 
+    			input.setImeOptions(EditorInfo.IME_FLAG_NO_EXTRACT_UI);
+    			builder.setView(input);
+    			builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+    				@Override
+    				public void onClick(DialogInterface dialog, int which)
+    				{
+    					final String fileName = input.getText().toString();  
+    					dialog.dismiss();
+    					if(!fileName.isEmpty())
+    					{
+    						File newFile = new File(getApplicationInfo().dataDir + "/shared_prefs/" + fileName + ".xml");
+    						if(newFile.exists())
+    						{
+    							new AlertDialog.Builder(getActivity())
+    							.setTitle("Presets Already Exist")
+    							.setMessage("Do you want to overwrite the existing file?")
+    							.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									saveConfig(fileName);
+    								}
+    							})
+    							.setNegativeButton("No", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									dialog.dismiss();
+    								}
+    							})
+    							.show();
+    						}
+    						else
+    						{
+    							saveConfig(fileName);
+    						}
+    					}
+    				}
+    			});
+    			AlertDialog alertToShow = builder.create();
+    			alertToShow.getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_VISIBLE);
+    			alertToShow.show();
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonRemove = findPreference(getString(R.string.pref_key_remove_button));
+        buttonRemove.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Remove Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						File file = new File(getApplicationInfo().dataDir + "/shared_prefs/" + files[which] + ".xml");
+        						if(file.exists())
+        						{
+        							file.delete();
+        						}
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonGPS = findPreference(getString(R.string.pref_key_gps_saved));
+        buttonGPS.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {  
+        		if(((CustomSwitchPreference)preference).isChecked())
+        		{
+	        		if (!PermissionHelper.hasPermission(getActivity(), Manifest.permission.ACCESS_FINE_LOCATION)) {
+	        			((CustomSwitchPreference)preference).setChecked(false);
+	        			PermissionHelper.requestPermission(getActivity(), Manifest.permission.ACCESS_FINE_LOCATION);
+	        			return false;
+	        		}
+        		}
+        		return true;
+        	}
+        });
+        if(((CustomSwitchPreference)buttonGPS).isChecked())
+		{
+    		if (!PermissionHelper.hasPermission(this, Manifest.permission.ACCESS_FINE_LOCATION)) {
+    			((CustomSwitchPreference)buttonGPS).setChecked(false);
+    			PermissionHelper.requestPermission(this, Manifest.permission.ACCESS_FINE_LOCATION);
+    		}
+		}
+        
+        
+        ((Preference)findPreference(getString(R.string.pref_key_camera_driver))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_camera_driver))).getEntry() + ") "+getString(R.string.pref_summary_camera_driver));
+        ((Preference)findPreference(getString(R.string.pref_key_density))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_density))).getEntry() + ") "+getString(R.string.pref_summary_density));
+        ((Preference)findPreference(getString(R.string.pref_key_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth))).getEntry() + ") "+getString(R.string.pref_summary_depth));
+        ((Preference)findPreference(getString(R.string.pref_key_min_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getEntry() + ") "+getString(R.string.pref_summary_min_depth));
+        ((Preference)findPreference(getString(R.string.pref_key_point_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_point_size))).getEntry() + ") "+getString(R.string.pref_summary_point_size));
+        ((Preference)findPreference(getString(R.string.pref_key_angle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_angle))).getEntry() + ") "+getString(R.string.pref_summary_angle));
+        ((Preference)findPreference(getString(R.string.pref_key_triangle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_triangle))).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
+        ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
+          
+        ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
+        ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_sim_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_sim_thr))).getEntry() + ") "+getString(R.string.pref_summary_sim_thr));
+        ((Preference)findPreference(getString(R.string.pref_key_min_inliers))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_inliers))).getEntry() + ") "+getString(R.string.pref_summary_min_inliers));
+        ((Preference)findPreference(getString(R.string.pref_key_opt_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_error))).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
+        ((Preference)findPreference(getString(R.string.pref_key_features_voc))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_voc))).getEntry() + ") "+getString(R.string.pref_summary_features_voc));
+        ((Preference)findPreference(getString(R.string.pref_key_features))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features))).getEntry() + ") "+getString(R.string.pref_summary_features));
+        ((Preference)findPreference(getString(R.string.pref_key_features_type))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_type))).getEntry() + ") "+getString(R.string.pref_summary_features_type));
+        ((Preference)findPreference(getString(R.string.pref_key_optimizer))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_optimizer))).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        ((Preference)findPreference(getString(R.string.pref_key_marker_detection))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_marker_detection))).getEntry() + ") "+getString(R.string.pref_summary_marker_detection));
+        ((Preference)findPreference(getString(R.string.pref_key_marker_detection_depth_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_marker_detection_depth_error))).getEntry() + ") "+getString(R.string.pref_summary_marker_detection_depth_error));
+        
+        ((Preference)findPreference(getString(R.string.pref_key_cloud_voxel))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cloud_voxel))).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
+        ((Preference)findPreference(getString(R.string.pref_key_texture_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_size))).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
+        ((Preference)findPreference(getString(R.string.pref_key_texture_count))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_count))).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
+        ((Preference)findPreference(getString(R.string.pref_key_normal_k))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_normal_k))).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
+        ((Preference)findPreference(getString(R.string.pref_key_max_texture_distance))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_texture_distance))).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
+        ((Preference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));
+
+        ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
+        ((Preference)findPreference(getString(R.string.pref_key_opt_color_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_color_radius))).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        ((Preference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
+
+        ((Preference)findPreference(getString(R.string.pref_key_cluster_ratio))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cluster_ratio))).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
+        ((Preference)findPreference(getString(R.string.pref_key_gain_max_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_gain_max_radius))).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
+    }
+    
+    public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) {
+        Preference pref = findPreference(key);
+
+        if (pref instanceof ListPreference) {
+        	if(key.compareTo(getString(R.string.pref_key_camera_driver))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_camera_driver));
+        	if(key.compareTo(getString(R.string.pref_key_density))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_density));
+        	if(key.compareTo(getString(R.string.pref_key_depth))==0) 
+    		{
+    			pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth));
+    			float maxDepth = Float.parseFloat(((ListPreference)pref).getValue());
+    			float minDepth = Float.parseFloat(((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getValue());
+    			if(maxDepth > 0.0f && maxDepth <= minDepth)
+    			{
+    				((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).setValueIndex(0);
+    			}
+    		}
+        	if(key.compareTo(getString(R.string.pref_key_min_depth))==0) 
+        	{
+        		pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_depth));
+        		float maxDepth = Float.parseFloat(((ListPreference)findPreference(getString(R.string.pref_key_depth))).getValue());
+        		float minDepth = Float.parseFloat(((ListPreference)pref).getValue());
+        		if(minDepth >= maxDepth)
+        		{
+        			((ListPreference)findPreference(getString(R.string.pref_key_depth))).setValueIndex(0);
+        		}
+        	}
+        	if(key.compareTo(getString(R.string.pref_key_point_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_point_size));
+        	if(key.compareTo(getString(R.string.pref_key_angle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_angle));
+        	if(key.compareTo(getString(R.string.pref_key_triangle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
+        	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
+        	
+        	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
+        	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
+        	if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
+        	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
+        	if(key.compareTo(getString(R.string.pref_key_sim_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_sim_thr));
+        	if(key.compareTo(getString(R.string.pref_key_min_inliers))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_inliers));
+        	if(key.compareTo(getString(R.string.pref_key_opt_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
+        	if(key.compareTo(getString(R.string.pref_key_features_voc))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_voc));
+        	if(key.compareTo(getString(R.string.pref_key_features))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features));
+        	if(key.compareTo(getString(R.string.pref_key_features_type))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_type));
+        	if(key.compareTo(getString(R.string.pref_key_optimizer))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        	if(key.compareTo(getString(R.string.pref_key_marker_detection))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_marker_detection));
+        	if(key.compareTo(getString(R.string.pref_key_marker_detection_depth_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_marker_detection_depth_error));
+        	
+        	if(key.compareTo(getString(R.string.pref_key_cloud_voxel))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
+        	if(key.compareTo(getString(R.string.pref_key_texture_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
+        	if(key.compareTo(getString(R.string.pref_key_texture_count))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
+        	if(key.compareTo(getString(R.string.pref_key_normal_k))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
+        	if(key.compareTo(getString(R.string.pref_key_max_texture_distance))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
+        	if(key.compareTo(getString(R.string.pref_key_min_texture_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));
+
+        	if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
+        	if(key.compareTo(getString(R.string.pref_key_opt_color_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        	if(key.compareTo(getString(R.string.pref_key_opt_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
+        	
+        	if(key.compareTo(getString(R.string.pref_key_cluster_ratio))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
+        	if(key.compareTo(getString(R.string.pref_key_gain_max_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
+        
+        }
+    }
+    
+    @Override
+    protected void onResume() {
+        super.onResume();
+        // Set up a listener whenever a key changes
+        getPreferenceScreen().getSharedPreferences()
+                .registerOnSharedPreferenceChangeListener(this);
+    }
+
+    @Override
+    protected void onPause() {
+        super.onPause();
+        // Unregister the listener whenever a key changes
+        getPreferenceScreen().getSharedPreferences()
+                .unregisterOnSharedPreferenceChangeListener(this);
+    }
+    
+    private void saveConfig(String fileName)
+    {
+    	//sp1 is the shared pref to copy to
+		SharedPreferences.Editor ed = getActivity().getSharedPreferences(fileName, MODE_PRIVATE).edit(); 
+		SharedPreferences sp = getPreferenceScreen().getSharedPreferences(); //The shared preferences to copy from
+		ed.clear(); // This clears the one we are copying to, but you don't necessarily need to do that.
+		//Cycle through all the entries in the sp
+		for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+		 Object v = entry.getValue(); 
+		 String key = entry.getKey();
+		 //Now we just figure out what type it is, so we can copy it.
+		 // Note that i am using Boolean and Integer instead of boolean and int.
+		 // That's because the Entry class can only hold objects and int and boolean are primatives.
+		 if(v instanceof Boolean) 
+		 // Also note that i have to cast the object to a Boolean 
+		 // and then use .booleanValue to get the boolean
+		    ed.putBoolean(key, ((Boolean)v).booleanValue());
+		 else if(v instanceof Float)
+		    ed.putFloat(key, ((Float)v).floatValue());
+		 else if(v instanceof Integer)
+		    ed.putInt(key, ((Integer)v).intValue());
+		 else if(v instanceof Long)
+		    ed.putLong(key, ((Long)v).longValue());
+		 else if(v instanceof String)
+		    ed.putString(key, ((String)v));         
+		}
+		ed.commit(); //save it.
+    }
+    
+    @Override
+	public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
+
+		switch (requestCode) {
+			case PermissionHelper.ACCESS_FINE_LOCATION_CODE: {
+				// If request is cancelled, the result arrays are empty.
+				if (results.length > 0 && results[0] == PackageManager.PERMISSION_GRANTED) {
+					// permission was granted, yay! Do the
+					// contacts-related task you need to do.
+					Preference buttonGPS = findPreference(getString(R.string.pref_key_gps_saved));
+					((CustomSwitchPreference)buttonGPS).setChecked(true);
+				} else {
+					// permission denied, boo! Disable the
+					// functionality that depends on this permission.
+					Toast.makeText(this, "Location permission is needed to use GPS functionality", Toast.LENGTH_LONG).show();
+					if (!PermissionHelper.shouldShowRequestPermissionRationale(this, Manifest.permission.ACCESS_FINE_LOCATION)) {
+						PermissionHelper.launchPermissionSettings(this); // Permission denied with checking "Do not ask again".
+					}
+				}
+				return;
+			}
+		}
+	}
+}

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

@@ -0,0 +1,464 @@
+package com.introlab.rtabmap;
+
+import java.io.File;
+import java.io.IOException;
+import java.util.List;
+
+import android.app.Activity;
+import android.app.AlertDialog;
+import android.app.Dialog;
+import android.app.ProgressDialog;
+import android.content.Context;
+import android.content.DialogInterface;
+import android.content.Intent;
+import android.content.SharedPreferences;
+import android.net.ConnectivityManager;
+import android.net.NetworkInfo;
+import android.os.AsyncTask;
+import android.os.Bundle;
+import android.preference.PreferenceManager;
+import android.text.Editable;
+import android.text.SpannableString;
+import android.text.TextWatcher;
+import android.text.method.LinkMovementMethod;
+import android.text.util.Linkify;
+import android.util.Log;
+import android.view.View;
+import android.view.View.OnClickListener;
+import android.webkit.WebView;
+import android.webkit.WebViewClient;
+import android.widget.Button;
+import android.widget.CheckBox;
+import android.widget.EditText;
+import android.widget.TextView;
+import android.widget.Toast;
+
+public class SketchfabActivity extends Activity implements OnClickListener {
+	
+	private static final String AUTHORIZE_PATH	= "https://sketchfab.com/oauth2/authorize";
+	private static final String CLIENT_ID		= "RXrIJYAwlTELpySsyM8TrK9r3kOGQ5Qjj9VVDIfV";
+	private static final String REDIRECT_URI	= "https://introlab.github.io/rtabmap/oauth2_redirect";
+	
+	ProgressDialog mProgressDialog;
+	
+	private Dialog mAuthDialog;
+	
+	private String mAuthToken;
+	private String mWorkingDirectory;
+	
+	EditText mFilename;
+	EditText mDescription;
+	EditText mTags;
+	CheckBox mDraft;
+	Button mButtonOk;
+	
+	private SketchfabActivity getActivity() {return this;}
+
+	@Override
+	protected void onCreate(Bundle savedInstanceState) {
+		super.onCreate(savedInstanceState);
+		setContentView(R.layout.activity_sketchfab);
+		
+		mFilename = (EditText)findViewById(R.id.editText_filename);
+		mDescription = (EditText)findViewById(R.id.editText_description);
+		mTags = (EditText)findViewById(R.id.editText_tags);
+		mDraft = (CheckBox)findViewById(R.id.checkBox_draft);
+		mButtonOk = (Button)findViewById(R.id.button_ok);
+		
+		mProgressDialog = new ProgressDialog(this);
+		mProgressDialog.setCancelable(false);
+		mProgressDialog.setCanceledOnTouchOutside(false);
+		
+		mAuthToken = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY);
+		mFilename.setText(getIntent().getExtras().getString(RTABMapActivity.RTABMAP_FILENAME_KEY));
+		mWorkingDirectory = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_WORKING_DIR_KEY);
+		
+		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		String tags = sharedPref.getString(getString(R.string.pref_key_tags), getString(R.string.pref_default_tags));
+		if(tags.isEmpty())
+		{
+			tags = getString(R.string.pref_default_tags);
+		}
+		mTags.setText(tags);
+		
+		mButtonOk.setEnabled(mFilename.getText().toString().length()>0);
+		mButtonOk.setOnClickListener(this);
+		
+		mFilename.addTextChangedListener(new TextWatcher() {
+
+			   @Override
+			   public void afterTextChanged(Editable s) {}
+
+			   @Override    
+			   public void beforeTextChanged(CharSequence s, int start,
+			     int count, int after) {
+			   }
+
+			   @Override    
+			   public void onTextChanged(CharSequence s, int start,
+			     int before, int count) {
+			      mButtonOk.setEnabled(s.length() != 0);
+			   }
+			  });
+		
+		mFilename.setSelectAllOnFocus(true);
+		mFilename.requestFocus();
+	}
+	
+	@Override
+	public void onClick(View v) {
+		// Handle button clicks.
+		switch (v.getId()) {
+		case R.id.button_ok:
+			shareToSketchfab();
+			break;
+		default:
+			return;
+		}
+	}
+	
+	private void shareToSketchfab()
+	{
+		if(!mTags.getText().toString().isEmpty())
+		{
+			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+			SharedPreferences.Editor editor = sharedPref.edit();
+			editor.putString(getString(R.string.pref_key_tags), mTags.getText().toString());
+			// Commit the edits!
+			editor.commit();
+		}
+		
+		authorizeAndPublish(mFilename.getText().toString());	
+	}
+	
+	private boolean isNetworkAvailable() {
+	    ConnectivityManager connectivityManager 
+	          = (ConnectivityManager) getSystemService(Context.CONNECTIVITY_SERVICE);
+	    NetworkInfo activeNetworkInfo = connectivityManager.getActiveNetworkInfo();
+	    return activeNetworkInfo != null && activeNetworkInfo.isConnected();
+	}
+	
+	private void authorizeAndPublish(final String fileName)
+	{
+		if(!isNetworkAvailable())
+		{
+			// Visualize the result?
+			AlertDialog ad = new AlertDialog.Builder(this)
+			.setCancelable(false)
+			.setTitle("Sharing to Sketchfab...")
+			.setMessage("Network is not available. Make sure you have internet before continuing.")
+			.setPositiveButton("Try Again", new DialogInterface.OnClickListener() {
+				public void onClick(DialogInterface dialog, int which) {
+					authorizeAndPublish(fileName);
+				}
+			})
+			.setNeutralButton("Abort", new DialogInterface.OnClickListener() {
+				public void onClick(DialogInterface dialog, int which) {
+				}
+			})
+			.create();
+			ad.setCanceledOnTouchOutside(false);
+			ad.show();
+			return;
+		}	
+
+		// get token the first time
+		if(mAuthToken == null)
+		{
+			Log.i(RTABMapActivity.TAG,"We don't have the token, get it!");
+
+			WebView web;
+			mAuthDialog = new Dialog(this);
+			mAuthDialog.setCancelable(true);
+			mAuthDialog.setCanceledOnTouchOutside(true);
+			mAuthDialog.setContentView(R.layout.auth_dialog);
+			web = (WebView)mAuthDialog.findViewById(R.id.webv);
+			web.setWebContentsDebuggingEnabled(!RTABMapActivity.DISABLE_LOG);
+			web.getSettings().setJavaScriptEnabled(true);
+			String auth_url = AUTHORIZE_PATH+"?redirect_uri="+REDIRECT_URI+"&response_type=token&client_id="+CLIENT_ID;
+			Log.i(RTABMapActivity.TAG, "Auhorize url="+auth_url);
+			web.setWebViewClient(new WebViewClient() {
+
+				boolean authComplete = false;
+
+				@Override
+				public void onPageFinished(WebView view, String url) {
+					super.onPageFinished(view, url);
+
+					//Log.i(TAG,"onPageFinished url="+url);
+					if(url.contains("error=access_denied")){
+						Log.e(RTABMapActivity.TAG, "ACCESS_DENIED_HERE");
+						authComplete = true;
+						Toast.makeText(getApplicationContext(), "Error Occured", Toast.LENGTH_SHORT).show();
+						mAuthDialog.dismiss();
+					}
+					else if (url.startsWith(REDIRECT_URI) && url.contains("access_token") && authComplete != true) {
+						//Log.i(TAG,"onPageFinished received token="+url);
+						String[] sArray = url.split("access_token=");
+						mAuthToken = (sArray[1].split("&token_type=Bearer"))[0];
+						authComplete = true;
+						
+						mAuthDialog.dismiss();
+
+						zipAndPublish(fileName);
+					}
+				}
+			});
+			mAuthDialog.show();
+			mAuthDialog.setTitle("Authorize RTAB-Map");
+			web.loadUrl(auth_url);
+		}
+		else
+		{
+			zipAndPublish(fileName);
+		}
+	}
+
+	private void zipAndPublish(final String fileName)
+	{		
+		mProgressDialog.setTitle("Upload to Sketchfab");
+		mProgressDialog.setMessage(String.format("Compressing the files..."));
+		mProgressDialog.setCancelable(false);
+		mProgressDialog.setCanceledOnTouchOutside(false);
+		mProgressDialog.show();
+
+		Thread workingThread = new Thread(new Runnable() {
+			public void run() {
+				try{
+					
+					File tmpDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR);
+					tmpDir.mkdirs();
+					String[] fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+					if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR));
+					for(int i=0; i<fileNames.length; ++i)
+					{
+						File f = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i]);
+						if(f.delete())
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleted \"%s\"", f.getPath()));
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Failed deleting \"%s\"", f.getPath()));
+						}
+					}
+					File exportDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_EXPORT_DIR);
+					exportDir.mkdirs();
+					
+					if(RTABMapLib.writeExportedMesh(RTABMapActivity.nativeApplication, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
+					{
+						String[] files = new String[0];
+						// verify if we have all files
+				
+						fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+						if(fileNames.length > 0)
+						{
+							files = new String[fileNames.length];
+							for(int i=0; i<fileNames.length; ++i)
+							{
+								files[i] = mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i];
+							}
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, "Missing files!");
+						}
+				
+						if(files.length > 0)
+						{
+							final String[] filesToZip = files;
+							final String zipOutput = mWorkingDirectory+fileName+".zip";
+							Util.zip(filesToZip, zipOutput);
+							runOnUiThread(new Runnable() {
+								public void run() {
+									mProgressDialog.dismiss();
+
+									File f = new File(zipOutput);
+
+									// Continue?
+									AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+									builder.setTitle("File(s) compressed and ready to upload!");
+									
+									final int fileSizeMB = (int)f.length()/(1024 * 1024);
+									final int fileSizeKB = (int)f.length()/(1024);
+									if(fileSizeMB == 0)
+									{
+										Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d KB", fileSizeKB));
+										builder.setMessage(String.format("Total size to upload = %d KB. Do you want to continue?\n\n", fileSizeKB));
+									}
+									else
+									{
+										Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d MB", fileSizeMB));
+										builder.setMessage(String.format("Total size to upload = %d MB. %sDo you want to continue?\n\n"
+												+ "Tip: To reduce the model size, you can also look at the Settings->Exporting options.", fileSizeMB,
+												fileSizeMB>=50?"Note that for size over 50 MB, a Sketchfab PRO account is required, otherwise the upload may fail. ":""));
+									}
+
+									
+									builder.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+										public void onClick(DialogInterface dialog, int which) {
+											mProgressDialog.setTitle("Upload to Sketchfab");
+											if(fileSizeMB == 0)
+											{
+												mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d KB) to Sketchfab...", fileName, fileSizeKB));
+											}
+											else
+											{
+												mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d MB) to Sketchfab...", fileName, fileSizeMB));
+											}
+											mProgressDialog.show();
+											new uploadToSketchfabTask().execute(zipOutput, fileName);
+										}
+									});
+									builder.setNegativeButton("No", new DialogInterface.OnClickListener() {
+										public void onClick(DialogInterface dialog, int which) {
+											// do nothing...
+										}
+									});
+									AlertDialog ad = builder.create();
+									ad.setCancelable(false);
+									ad.setCanceledOnTouchOutside(false);
+									ad.show();
+								}
+							});
+						}
+					}
+					else
+					{
+						runOnUiThread(new Runnable() {
+							public void run() {
+								mProgressDialog.dismiss();
+								Toast.makeText(getActivity(), String.format("Failed writing files!"), Toast.LENGTH_LONG).show();
+							}
+						});
+					}
+				}
+				catch(IOException ex) {
+					Log.e(RTABMapActivity.TAG, "Failed to zip", ex);
+				}
+			}
+		});
+
+		workingThread.start();
+	}
+	
+	private class uploadToSketchfabTask extends AsyncTask<String, Void, Void>
+	{
+		String mModelUri;
+		String mModelFilePath;
+		String error = "";
+		String mFileName;
+
+		protected void onPreExecute() {
+			//display progress dialog.
+		}
+
+		@Override
+		protected void onPostExecute(Void result) {
+			
+			mProgressDialog.dismiss();
+			//Task you want to do on UIThread after completing Network operation
+			//onPostExecute is called after doInBackground finishes its task.
+			if(mModelFilePath!= null)
+			{
+				File f = new File(mModelFilePath);
+				f.delete(); // cleanup
+
+				// See on sketchfab?
+				  final SpannableString s = new SpannableString(
+				        "Model \"" + mFileName + "\" is now processing on Sketchfab! You can click "
+				        + "on the link below to see it on Sketchfab.\n\nhttps://sketchfab.com/models/"+mModelUri);
+				  Linkify.addLinks(s, Linkify.WEB_URLS);
+				final AlertDialog d = new AlertDialog.Builder(getActivity())
+				.setTitle("Upload finished!")
+				.setCancelable(false)
+				.setMessage(s)
+				.setPositiveButton("Close", new DialogInterface.OnClickListener() {
+					public void onClick(DialogInterface dialog, int which) {
+						Intent resultIntent = new Intent();
+						resultIntent.putExtra(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY, mAuthToken); 
+						setResult(Activity.RESULT_OK, resultIntent);
+						finish();
+					}
+				}).create();
+				d.setCanceledOnTouchOutside(false);
+				d.show();
+				((TextView)d.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
+			}
+			else
+			{
+				Toast.makeText(getApplicationContext(), String.format("Upload failed! Error=\"%s\"", error), Toast.LENGTH_SHORT).show();
+			}
+		}
+
+		@Override
+		protected Void doInBackground(String... files) {
+			String charset = "UTF-8";
+			File uploadFile = new File(files[0]);
+			mFileName = files[1];
+			String requestURL = "https://api.sketchfab.com/v3/models";
+
+			Log.i(RTABMapActivity.TAG, "Uploading " + files[0]);
+
+			try {
+				MultipartUtility multipart = new MultipartUtility(requestURL, mAuthToken, charset);
+
+				multipart.addFormField("name", mFileName);
+				multipart.addFormField("description", mDescription.getText().toString());
+				multipart.addFormField("tags", mTags.getText().toString());
+				multipart.addFormField("source", "RTAB-Map");
+				multipart.addFormField("isPublished", mDraft.isChecked()?"false":"true");	
+				multipart.addFilePart("modelFile", uploadFile);
+
+				Log.i(RTABMapActivity.TAG, "Starting multipart request");
+				List<String> response = multipart.finish();
+
+				Log.i(RTABMapActivity.TAG, "SERVER REPLIED:");
+
+				for (String line : response) {
+					Log.i(RTABMapActivity.TAG, line);
+					//{"uri":"https:\/\/api.sketchfab.com\/v3\/models\/XXXXXXXXX","uid":"XXXXXXXXXX"}
+					if(line.contains("\"uid\":\""))
+					{
+						String[] sArray = line.split("\"uid\":\"");
+						mModelUri = (sArray[1].split("\""))[0];
+						mModelFilePath = files[0];
+
+						//patch model for orientation
+						/*HttpClient httpClient = new DefaultHttpClient(); 
+						try {
+							String patchURL = "https://api.sketchfab.com/v3/models/"+ mModelUri +"/options";
+							HttpPatch request = new HttpPatch(patchURL);
+							String json = 
+											"{\n"+
+												"uid: "+ mModelUri + "\n" +
+												"shading: shadeless\n"+
+												//"orientation:\n"+
+												//"{\n"+
+												//	"axis : [1, 0, 0]\n"+
+												//	"angle : 0\n"+
+												//"}\n"+
+											"}";
+
+							request.setHeader("Authorization", "Bearer " + mAuthToken);
+							StringEntity params =new StringEntity(json, "UTF-8");
+							params.setContentType("application/json");
+							request.setEntity(params);
+							HttpResponse responsePatch = httpClient.execute(request);
+							int responseStatus = responsePatch.getStatusLine().getStatusCode();
+							Log.i(RTABMapActivity.TAG, "get data responseStatus: " + responseStatus);
+
+						}catch (Exception e) {
+							Log.e(RTABMapActivity.TAG, "Error while patching model", e);
+							error = e.getMessage();
+						}*/
+					}
+				}    		  
+			} catch (IOException ex) {
+				Log.e(RTABMapActivity.TAG, "Error while uploading", ex);
+				error = ex.getMessage();
+			}
+			return null;
+		}
+	}
+}

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

@@ -0,0 +1,87 @@
+package com.introlab.rtabmap;
+
+
+import android.graphics.ImageFormat;
+import android.media.Image;
+import android.media.ImageReader;
+import android.os.Handler;
+import android.os.HandlerThread;
+import android.util.Log;
+
+import java.nio.ByteBuffer;
+
+public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
+
+    public int WIDTH;
+    public int HEIGHT;
+    public ImageReader imageReader;
+    public int frameCount = 0;
+    public long timestamp;
+    
+    // Looper handler thread.
+    private HandlerThread backgroundThread;
+    // Looper handler.
+    private Handler backgroundHandler;
+
+    public ByteBuffer depth16_raw;
+
+    TOF_ImageReader(){
+    }
+
+    public void createImageReader(int width, int height){
+        this.WIDTH = width;
+        this.HEIGHT = height;
+        this.imageReader =
+                ImageReader.newInstance(
+                        width,
+                        height,
+                        ImageFormat.DEPTH16,
+                        2);
+        this.imageReader.setOnImageAvailableListener(this, this.backgroundHandler);
+    }
+
+    // CPU image reader callback.
+    @Override
+    public void onImageAvailable(ImageReader imageReader) {
+        Image image  = imageReader.acquireLatestImage();
+        if (image == null) {
+            Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
+            return;
+        }
+        else{
+            if(image.getFormat() == ImageFormat.DEPTH16){
+                this.timestamp = image.getTimestamp();
+                depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+                // copy raw undecoded DEPTH16 format depth data to NativeBuffer
+                frameCount++;
+            }
+            else{
+                Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
+            }
+        }
+        image.close();
+    }
+    
+ // Start background handler thread, used to run callbacks without blocking UI thread.
+    public void startBackgroundThread() {
+        this.backgroundThread = new HandlerThread("DepthDecoderThread");
+        this.backgroundThread.start();
+        this.backgroundHandler = new Handler(backgroundThread.getLooper());
+    }
+
+    // Stop background handler thread.
+    public void stopBackgroundThread() {
+        if (this.backgroundThread != null) {
+            this.backgroundThread.quitSafely();
+            try {
+                this.backgroundThread.join();
+                this.backgroundThread = null;
+                this.backgroundHandler = null;
+            } catch (InterruptedException e) {
+                Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
+            }
+        }
+    }
+
+}
+

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

@@ -0,0 +1,422 @@
+//package ri.blog.opengl008;
+package com.introlab.rtabmap;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.FloatBuffer;
+import java.nio.ShortBuffer;
+import java.util.Iterator;
+import java.util.Vector;
+
+import android.content.Context;
+import android.graphics.Bitmap;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Rect;
+import android.graphics.Typeface;
+import android.opengl.GLES20;
+import android.opengl.GLUtils;
+import android.text.TextPaint;
+import android.util.Log;
+
+public class TextManager {
+	
+	/* SHADER Text
+	 * 
+	 * This shader is for rendering 2D text textures straight from a texture
+	 * Color and alpha blended.
+	 * 
+	 */
+	public static final String vs_Text =
+		"uniform mat4 uMVPMatrix;" +
+		"attribute vec4 vPosition;" +
+		"attribute vec2 a_texCoord;" +
+		"varying vec2 v_texCoord;" +
+	    "void main() {" +
+	    "  gl_Position = uMVPMatrix * vPosition;" +
+	    "  v_texCoord = a_texCoord;" +
+	    "}";
+	public static final String fs_Text =
+	    "precision mediump float;" +
+	    "uniform float uColor;" +
+	    "varying vec2 v_texCoord;" +
+        "uniform sampler2D s_texture;" +
+	    "void main() {" +
+	    "  gl_FragColor = texture2D( s_texture, v_texCoord );" +
+	    "  gl_FragColor.rgb *= uColor;" +
+	    "}";
+	
+	public static int sp_Text;
+	
+	public static final int RI_TEXT_TEXTURE_SIZE = 512; // 512
+	public static final float RI_TEXT_HEIGHT_BASE = 32.0f;
+	public static final char RI_TEXT_START = ' ';
+	public static final char RI_TEXT_STOP = '\u00B0'+1;
+	
+	public float getMaxTextHeight() {return mTextHeight;}
+	
+	private float mUVWidth;
+	private float mUVHeight;
+	private float mTextHeight;
+	private float mColor;
+	
+	private FloatBuffer vertexBuffer;
+	private FloatBuffer textureBuffer;
+	private ShortBuffer drawListBuffer;
+	
+	private float[] vecs;
+	private float[] uvs;
+	private short[] indices;
+	
+	private int index_vecs;
+	private int index_indices;
+	private int index_uvs;
+
+	private int texturenr;
+	private int[] mTextures;
+	
+	private float uniformscale = 1.0f;
+	
+	float[] mCharacterWidth;
+	
+	public TextManager(Context context)
+	{
+		// Create the arrays
+		vecs = new float[3 * 10];
+		uvs = new float[2 * 10];
+		indices = new short[10];
+		
+		// init as 0 as default
+		texturenr = 0;
+		
+		// Text shader
+		int vshadert = TextManager.loadShader(GLES20.GL_VERTEX_SHADER, TextManager.vs_Text);
+		int fshadert = TextManager.loadShader(GLES20.GL_FRAGMENT_SHADER, TextManager.fs_Text);
+
+		TextManager.sp_Text = GLES20.glCreateProgram();
+		GLES20.glAttachShader(TextManager.sp_Text, vshadert);
+		GLES20.glAttachShader(TextManager.sp_Text, fshadert); 		// add the fragment shader to program
+		GLES20.glLinkProgram(TextManager.sp_Text);                  // creates OpenGL ES program executables
+		
+		// Generate Textures, if more needed, alter these numbers.
+		mTextures = new int[1];
+		GLES20.glGenTextures(1, mTextures, 0);
+		
+		// Create an empty, mutable bitmap
+		Bitmap bitmap = Bitmap.createBitmap(RI_TEXT_TEXTURE_SIZE, RI_TEXT_TEXTURE_SIZE, Bitmap.Config.ARGB_4444);
+		// get a canvas to paint over the bitmap
+		Canvas canvas = new Canvas(bitmap);
+		bitmap.eraseColor(0);
+
+		// Draw the text
+		TextPaint textPaint = new TextPaint();
+		textPaint.setTextSize(RI_TEXT_HEIGHT_BASE);
+		textPaint.setColor(Color.WHITE);
+		textPaint.setAntiAlias(true);
+		textPaint.setTypeface(Typeface.create("Arial", Typeface.BOLD));
+		
+		// compute real maximum height
+		mTextHeight=0.0f;
+		for(char c=RI_TEXT_START; c<RI_TEXT_STOP; ++c)
+		{
+			Rect textBounds = new Rect();
+			textPaint.getTextBounds(String.valueOf(c), 0, 1, textBounds);
+			if(textBounds.height() + textBounds.bottom > mTextHeight)
+			{
+				mTextHeight = textBounds.height() + textBounds.bottom;
+			}
+		}
+		mUVWidth = (float)RI_TEXT_HEIGHT_BASE/(float)RI_TEXT_TEXTURE_SIZE;
+		mUVHeight = mTextHeight/(float)RI_TEXT_TEXTURE_SIZE;
+		mColor = 1.0f;
+		
+		int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+		mCharacterWidth = new float[RI_TEXT_STOP-RI_TEXT_START];
+		int i=0;
+		for(char c=RI_TEXT_START; c<RI_TEXT_STOP; ++c)
+		{
+			Rect textBounds = new Rect();
+			textPaint.getTextBounds(String.valueOf(c), 0, 1, textBounds);
+			canvas.drawText(String.valueOf(c), (i%colCount)*RI_TEXT_HEIGHT_BASE, (i/colCount) * mTextHeight + RI_TEXT_HEIGHT_BASE, textPaint);
+			mCharacterWidth[i] = textPaint.measureText(String.valueOf(c));
+			++i;
+		}
+		
+		GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, mTextures[0]);
+		GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MIN_FILTER, GLES20.GL_LINEAR);
+        GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MAG_FILTER, GLES20.GL_LINEAR);
+        GLUtils.texImage2D(GLES20.GL_TEXTURE_2D, 0, bitmap, 0);
+		
+		//Clean up
+		bitmap.recycle();
+	}
+	
+	public void setTextureID(int val)
+	{
+		texturenr = val;
+	}
+	
+	public static int loadShader(int type, String shaderCode){
+
+	    // create a vertex shader type (GLES20.GL_VERTEX_SHADER)
+	    // or a fragment shader type (GLES20.GL_FRAGMENT_SHADER)
+	    int shader = GLES20.glCreateShader(type);
+
+	    // add the source code to the shader and compile it
+	    GLES20.glShaderSource(shader, shaderCode);
+	    GLES20.glCompileShader(shader);
+	    
+	    // return the shader
+	    return shader;
+	}
+	
+	
+	public void AddCharRenderInformation(float[] vec, float[] cs, float[] uv, short[] indi)
+	{
+		// We need a base value because the object has indices related to 
+		// that object and not to this collection so basicly we need to 
+		// translate the indices to align with the vertexlocation in ou
+		// vecs array of vectors.
+		short base = (short) (index_vecs / 3);
+			
+		// We should add the vec, translating the indices to our saved vector
+		for(int i=0;i<vec.length;i++)
+		{
+			vecs[index_vecs] = vec[i];
+			index_vecs++;
+		}
+		
+		// We should add the uvs
+		for(int i=0;i<uv.length;i++)
+		{
+			uvs[index_uvs] = uv[i];
+			index_uvs++;
+		}
+
+		// We handle the indices
+		for(int j=0;j<indi.length;j++)
+		{
+			indices[index_indices] = (short) (base + indi[j]);
+			index_indices++;
+		}
+	}
+
+	public void PrepareDrawInfo(Vector<TextObject> txtcollection)
+	{
+		// Reset the indices.
+		index_vecs = 0;
+		index_indices = 0;
+		index_uvs = 0;
+		
+		// Get the total amount of characters
+		int charcount = 0;
+		for (TextObject txt : txtcollection) {
+			if(txt!=null)
+			{
+				if(!(txt.text==null))
+				{
+					charcount += txt.text.length(); 
+				}
+			}
+		}
+		
+		// Create the arrays we need with the correct size.
+		vecs = null;
+		uvs = null;
+		indices = null;
+		
+		vecs = new float[charcount * 12];
+		uvs = new float[charcount * 8];
+		indices = new short[charcount * 6];
+
+	}
+	
+	public void PrepareDraw(Vector<TextObject> txtcollection)
+	{
+		// Setup all the arrays
+		PrepareDrawInfo(txtcollection);
+		
+		// Using the iterator protects for problems with concurrency
+		for( Iterator< TextObject > it = txtcollection.iterator(); it.hasNext() ; )
+	    {
+	    	TextObject txt = it.next();
+	    	if(txt!=null)
+			{
+		    	if(!(txt.text==null))
+				{
+					convertTextToTriangleInfo(txt);
+				}
+			}
+	    }
+	}
+	
+	public void Draw(float[] m)
+	{
+		if(vecs.length > 0)
+		{
+			GLES20.glDisable(GLES20.GL_DEPTH_TEST);
+			GLES20.glEnable(GLES20.GL_BLEND);
+			
+			// Set the correct shader for our grid object.
+			GLES20.glUseProgram(sp_Text);
+			
+			// The vertex buffer.
+			ByteBuffer bb = ByteBuffer.allocateDirect(vecs.length * 4);
+			bb.order(ByteOrder.nativeOrder());
+			vertexBuffer = bb.asFloatBuffer();
+			vertexBuffer.put(vecs);
+			vertexBuffer.position(0);
+			
+			// The texture buffer
+			ByteBuffer bb2 = ByteBuffer.allocateDirect(uvs.length * 4);
+			bb2.order(ByteOrder.nativeOrder());
+			textureBuffer = bb2.asFloatBuffer();
+			textureBuffer.put(uvs);
+			textureBuffer.position(0);
+			
+			// initialize byte buffer for the draw list
+			ByteBuffer dlb = ByteBuffer.allocateDirect(indices.length * 2);
+			dlb.order(ByteOrder.nativeOrder());
+			drawListBuffer = dlb.asShortBuffer();
+			drawListBuffer.put(indices);
+			drawListBuffer.position(0);
+			
+			// get handle to vertex shader's vPosition member
+		    int mPositionHandle = GLES20.glGetAttribLocation(sp_Text, "vPosition");
+		    
+		    // Enable a handle to the triangle vertices
+		    GLES20.glEnableVertexAttribArray(mPositionHandle);
+		 
+		    // Prepare the background coordinate data
+		    GLES20.glVertexAttribPointer(mPositionHandle, 3,
+		                                 GLES20.GL_FLOAT, false,
+		                                 0, vertexBuffer);
+		    
+		    int mTexCoordLoc = GLES20.glGetAttribLocation(sp_Text, "a_texCoord" );
+		    
+		    // Prepare the texturecoordinates
+		    GLES20.glVertexAttribPointer ( mTexCoordLoc, 2, GLES20.GL_FLOAT,
+	                false, 
+	                0, textureBuffer);
+	
+		    GLES20.glEnableVertexAttribArray ( mPositionHandle );
+	        GLES20.glEnableVertexAttribArray ( mTexCoordLoc );
+	
+		    // get handle to shape's transformation matrix
+	        int mtrxhandle = GLES20.glGetUniformLocation(sp_Text, "uMVPMatrix");
+	        
+	    	// Apply the projection and view transformation
+	        GLES20.glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
+	        
+	        // get handle to color value
+	        int colorhandle = GLES20.glGetUniformLocation(sp_Text, "uColor");
+	        GLES20.glUniform1f(colorhandle, mColor);
+	        
+	        int mSamplerLoc = GLES20.glGetUniformLocation (sp_Text, "s_texture" );
+	        
+	        // Texture activate unit 0
+	        GLES20.glActiveTexture(GLES20.GL_TEXTURE0);
+			// Bind the texture to this unit.
+	        GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, mTextures[0]);
+	        // Set the sampler texture unit to our selected id
+		    GLES20.glUniform1i ( mSamplerLoc, texturenr);
+		    
+	        // Draw the triangle
+	        GLES20.glDrawElements(GLES20.GL_TRIANGLES, indices.length, GLES20.GL_UNSIGNED_SHORT, drawListBuffer);
+	
+	        // Disable vertex array
+	        GLES20.glDisableVertexAttribArray(mPositionHandle);
+	        GLES20.glDisableVertexAttribArray(mTexCoordLoc);
+		}
+	}
+	
+	private void convertTextToTriangleInfo(TextObject val)
+	{		
+		// Get attributes from text object
+		float x = val.x;
+		float y = val.y;
+		String text = val.text;
+		
+		Log.i("RTABMapActivity", String.format("convertTextToTriangleInfo() set status=%s", text));
+		
+		// Create 
+		for(int j=0; j<text.length(); j++)
+		{
+			// get ascii value
+			char c = text.charAt(j);
+			int c_val = (int)c;
+			
+			int indx = c_val-RI_TEXT_START;
+			
+			if(indx<0 || indx>=mCharacterWidth.length) {
+				// unknown character, we will add a space for it to be save.
+				indx = 0;
+			}
+			
+			int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+			
+			// Calculate the uv parts
+			int row = indx / colCount;
+			int col = indx % colCount;
+			
+			float v = row * mUVHeight;
+			float v2 = v + mUVHeight;
+			float u = col * mUVWidth;
+			float u2 = u + mCharacterWidth[indx]/(float)RI_TEXT_TEXTURE_SIZE;
+			
+			// Creating the triangle information
+			float[] vec = new float[12];
+			float[] uv = new float[8];
+			float[] colors = new float[16];
+			
+			vec[0] = x;
+			vec[1] = y + (mTextHeight * uniformscale);
+			vec[2] = 0.99f;
+			vec[3] = x;
+			vec[4] = y;
+			vec[5] = 0.99f;
+			vec[6] = x + (mCharacterWidth[indx] * uniformscale);
+			vec[7] = y;
+			vec[8] = 0.99f;
+			vec[9] = x + (mCharacterWidth[indx] * uniformscale);
+			vec[10] = y + (mTextHeight * uniformscale);
+			vec[11] = 0.99f;
+		
+			colors = new float[]
+				    	{val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3]
+				    			  		           };
+			// 0.001f = texture bleeding hack/fix
+			uv[0] = u+0.001f;
+			uv[1] = v+0.001f;
+			uv[2] = u+0.001f;
+			uv[3] = v2-0.001f;
+			uv[4] = u2-0.001f;
+			uv[5] = v2-0.001f;
+			uv[6] = u2-0.001f;
+			uv[7] = v+0.001f;
+			
+			short[] inds = {0, 1, 2, 0, 2, 3};
+			
+			// Add our triangle information to our collection for 1 render call.
+			AddCharRenderInformation(vec, colors, uv, inds);
+			
+			// Calculate the new position
+			x += (mCharacterWidth[indx]  * uniformscale);
+		}
+	}
+
+	public float getUniformscale() {
+		return uniformscale;
+	}
+
+	public void setUniformscale(float uniformscale) {
+		this.uniformscale = uniformscale;
+	}
+	
+	public void setColor(float color) {
+		mColor = color;
+	}
+}

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

@@ -0,0 +1,33 @@
+//package ri.blog.opengl008;
+package com.introlab.rtabmap;
+
+public class TextObject {
+	
+	public String text;
+	public float x;
+	public float y;
+	public float[] color;
+	
+	public TextObject()
+	{
+		text = "default";
+		x = 0f;
+		y = 0f;
+		color = new float[] {1f, 1f, 1f, 1.0f};
+	}
+	
+	public TextObject(String txt, float xcoord, float ycoord)
+	{
+		text = txt;
+		x = xcoord;
+		y = ycoord;
+		color = new float[] {1f, 1f, 1f, 1.0f};
+	}
+	
+	/*public boolean validate()
+	{
+		if(text.compareTo("")==0) return false;
+		
+		return true;
+	}*/
+}

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

@@ -0,0 +1,126 @@
+package com.introlab.rtabmap;
+
+import java.io.BufferedInputStream;
+import java.io.BufferedOutputStream;
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+import java.io.FilenameFilter;
+import java.io.IOException;
+import java.util.Arrays;
+import java.util.zip.ZipEntry;
+import java.util.zip.ZipOutputStream;
+
+import android.content.Context;
+import android.net.ConnectivityManager;
+import android.net.NetworkInfo;
+import android.util.Log;
+
+public class Util {
+	
+	public static final int ZIP_BUFFER_SIZE = 1<<20; // 1MB
+
+	public static void zip(String file, String zipFile) throws IOException {
+		Log.i(RTABMapActivity.TAG, "Zipping " + file +" to " + zipFile);
+		String[] files = new String[1];
+		files[0] = file;
+		zip(files, zipFile);
+	}
+
+	public static void zip(String[] files, String zipFile) throws IOException {
+		Log.i(RTABMapActivity.TAG, "Zipping " + String.valueOf(files.length) +" files to " + zipFile);
+		BufferedInputStream origin = null;
+		ZipOutputStream out = new ZipOutputStream(new BufferedOutputStream(new FileOutputStream(zipFile)));
+		try { 
+			byte data[] = new byte[ZIP_BUFFER_SIZE];
+
+			for (int i = 0; i < files.length; i++) {
+				FileInputStream fi = new FileInputStream(files[i]);    
+				origin = new BufferedInputStream(fi, ZIP_BUFFER_SIZE);
+				try {
+					ZipEntry entry = new ZipEntry(files[i].substring(files[i].lastIndexOf("/") + 1));
+					out.putNextEntry(entry);
+					int count;
+					while ((count = origin.read(data, 0, ZIP_BUFFER_SIZE)) != -1) {
+						out.write(data, 0, count);
+					}
+				}
+				finally {
+					origin.close();
+				}
+			}
+		}
+		finally {
+			out.close();
+		}
+	}
+
+	public static String[] loadFileList(String directory, final boolean databasesOnly) {
+		File path = new File(directory); 
+		String fileList[];
+		try {
+			path.mkdirs();
+		}
+		catch(SecurityException e) {
+			Log.e(RTABMapActivity.TAG, "unable to write on the sd card " + e.toString());
+		}
+		if(path.exists()) {
+			FilenameFilter filter = new FilenameFilter() {
+
+				@Override
+				public boolean accept(File dir, String filename) {
+					File sel = new File(dir, filename);
+					if(databasesOnly)
+					{
+						return filename.compareTo(RTABMapActivity.RTABMAP_TMP_DB) != 0 && filename.endsWith(".db");
+					}
+					else
+					{
+						return sel.isFile();
+					}
+				}
+
+			};
+			fileList = path.list(filter);
+			Arrays.sort(fileList);
+		}
+		else {
+			fileList = new String[0];
+		}
+		return fileList;
+	}
+	
+	/**
+	 * https://stackoverflow.com/questions/6701948/efficient-way-to-compare-version-strings-in-java
+	 * Compares two version strings. 
+	 * 
+	 * Use this instead of String.compareTo() for a non-lexicographical 
+	 * comparison that works for version strings. e.g. "1.10".compareTo("1.6").
+	 * 
+	 * @note It does not work if "1.10" is supposed to be equal to "1.10.0".
+	 * 
+	 * @param str1 a string of ordinal numbers separated by decimal points. 
+	 * @param str2 a string of ordinal numbers separated by decimal points.
+	 * @return The result is a negative integer if str1 is _numerically_ less than str2. 
+	 *         The result is a positive integer if str1 is _numerically_ greater than str2. 
+	 *         The result is zero if the strings are _numerically_ equal.
+	 */
+	public static int versionCompare(String str1, String str2) {
+	    String[] vals1 = str1.split("\\.");
+	    String[] vals2 = str2.split("\\.");
+	    int i = 0;
+	    // set index to first non-equal ordinal or length of shortest version string
+	    while (i < vals1.length && i < vals2.length && vals1[i].equals(vals2[i])) {
+	      i++;
+	    }
+	    // compare first non-equal ordinal number
+	    if (i < vals1.length && i < vals2.length) {
+	        int diff = Integer.valueOf(vals1[i]).compareTo(Integer.valueOf(vals2[i]));
+	        return Integer.signum(diff);
+	    }
+	    // the strings are equal or one string is a substring of the other
+	    // e.g. "1.2.3" = "1.2.3" or "1.2.3" < "1.2.3.4"
+	    return Integer.signum(vals1.length - vals2.length);
+	}
+	
+}

app/src/CMakeLists.txt

@@ -74,7 +74,11 @@ ELSE()
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
 IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    IF(Qt5Svg_FOUND)
+        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    ELSE()
+        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
+    ENDIF()
 ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -100,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
         WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
        
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
   SET(plugin_dest_dir bin)
   SET(qtconf_dest_dir bin)
@@ -150,11 +154,28 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
 		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
 		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
 		list(GET loc_list 1 plugin_type)
+		IF(NOT plugin_root)
+			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
+		ENDIF(NOT plugin_root)
 	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)
 	endforeach()
+	IF(WIN32)
+		IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+			IF(EXISTS ${plugin_loc})
+				get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+				string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+				list(GET loc_list 1 plugin_type)
+				INSTALL(FILES ${plugin_loc}
+					DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+					COMPONENT runtime)
+				#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+			ENDIF(EXISTS ${plugin_loc})
+		ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+	ENDIF(WIN32)
   ENDIF()
   
   # install a qt.conf file
@@ -185,5 +206,5 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
   " COMPONENT runtime)
-ENDIF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

app/src/ObjDeletionHandler.h

@@ -46,17 +46,18 @@ public:
 	}
 	virtual ~ObjDeletionHandler() {}
 
-signals:
+Q_SIGNALS:
 	void objDeletionEventReceived(int);
 
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("UObjDeletedEvent") == 0 &&
 		   event->getCode() == _watchedId)
 		{
-			emit objDeletionEventReceived(_watchedId);
+			Q_EMIT objDeletionEventReceived(_watchedId);
 		}
+		return false;
 	}
 private:
 	int _watchedId;

app/src/main.cpp

@@ -43,12 +43,13 @@ int main(int argc, char* argv[])
 {
 	/* Set logger type */
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setLevel(ULogger::kWarning);
 
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
+	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box
 
-	ParametersMap parameters = Parameters::parseArguments(argc, argv, true);
+	ParametersMap parameters = Parameters::parseArguments(argc, argv, false);
 	MainWindow * mainWindow = new MainWindow();
     app->installEventFilter(mainWindow); // to catch FileOpen events.
     
@@ -60,11 +61,10 @@ int main(int argc, char* argv[])
            UFile::getExtension(value).compare("db") == 0)
         {
             database = value;
-            break;
         }
     }
 
-	UINFO("Program started...");
+	printf("Program started...\n");
 
 	UEventsManager::addHandler(mainWindow);
 
@@ -84,9 +84,9 @@ int main(int argc, char* argv[])
     
     if(!database.empty())
     {
-    	mainWindow->openDatabase(database.c_str());
+    	mainWindow->openDatabase(database.c_str(), parameters);
     }
-    if(parameters.size())
+    else if(parameters.size())
     {
     	mainWindow->updateParameters(parameters);
     }
@@ -100,14 +100,13 @@ int main(int argc, char* argv[])
 	UEventsManager::removeHandler(mainWindow);
 	UEventsManager::removeHandler(rtabmap);
 
-	UINFO("Killing threads...");
 	rtabmap->join(true);
 
-	UINFO("Closing RTAB-Map...");
+	printf("Closing RTAB-Map...\n");
 	delete rtabmap;
 	delete mainWindow;
 	delete app;
-	UINFO("All done!");
+	printf("All done!\n");
 
     return 0;
 }

cmake_modules/FindARCore.cmake

@@ -0,0 +1,31 @@
+# - Find ARCore
+# This module finds an installed ARCore client C-API package.
+#
+# It sets the following variables:
+#  ARCore_FOUND       - Set to false, or undefined, if ARCore isn't found.
+#  ARCore_INCLUDE_DIRS - The ARCore include directory.
+#  ARCore_LIBRARIES     - The ARCore library to link against.
+
+FIND_PATH(ARCore_INCLUDE_DIR arcore_c_api.h)
+
+FIND_LIBRARY(ARCore_c_LIBRARY NAMES arcore_sdk_c PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(ARCore_jni_LIBRARY NAMES arcore_sdk_jni PATH_SUFFIXES ${ANDROID_ABI})
+
+IF (ARCore_INCLUDE_DIR AND ARCore_c_LIBRARY AND ARCore_jni_LIBRARY)
+   SET(ARCore_FOUND TRUE)
+   SET(ARCore_INCLUDE_DIRS ${ARCore_INCLUDE_DIR})
+   SET(ARCore_LIBRARIES ${ARCore_c_LIBRARY} ${ARCore_jni_LIBRARY})
+ENDIF (ARCore_INCLUDE_DIR AND ARCore_c_LIBRARY AND ARCore_jni_LIBRARY)
+
+IF (ARCore_FOUND)
+   # show which ARCore was found only if not quiet
+   IF (NOT ARCore_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ARCore: ${ARCore_INCLUDE_DIRS}")
+   ENDIF (NOT ARCore_FIND_QUIETLY)
+ELSE (ARCore_FOUND)
+   # fatal error if ARCore is required but not found
+   IF (ARCore_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ARCore (client and/or support libraries)")
+   ENDIF (ARCore_FIND_REQUIRED)
+ENDIF (ARCore_FOUND)
+

cmake_modules/FindAREngine.cmake

@@ -0,0 +1,32 @@
+# - Find AREngine
+# This module finds an installed AREngine client C-API package.
+#
+# It sets the following variables:
+#  AREngine_FOUND       - Set to false, or undefined, if AREngine isn't found.
+#  AREngine_INCLUDE_DIRS - The AREngine include directory.
+#  AREngine_LIBRARIES     - The AREngine library to link against.
+
+FIND_PATH(AREngine_INCLUDE_DIR huawei_arengine_interface.h)
+
+FIND_LIBRARY(AREngine_impl_LIBRARY NAMES huawei_arengine_impl PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(AREngine_jni_LIBRARY NAMES huawei_arengine_jni PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(AREngine_ndk_LIBRARY NAMES huawei_arengine_ndk PATH_SUFFIXES ${ANDROID_ABI})
+
+IF (AREngine_INCLUDE_DIR AND AREngine_impl_LIBRARY AND AREngine_jni_LIBRARY AND AREngine_ndk_LIBRARY)
+   SET(AREngine_FOUND TRUE)
+   SET(AREngine_INCLUDE_DIRS ${AREngine_INCLUDE_DIR})
+   SET(AREngine_LIBRARIES ${AREngine_impl_LIBRARY} ${AREngine_jni_LIBRARY} ${AREngine_ndk_LIBRARY})
+ENDIF (AREngine_INCLUDE_DIR AND AREngine_impl_LIBRARY AND AREngine_jni_LIBRARY AND AREngine_ndk_LIBRARY)
+
+IF (AREngine_FOUND)
+   # show which AREngine was found only if not quiet
+   IF (NOT AREngine_FIND_QUIETLY)
+      MESSAGE(STATUS "Found AREngine: ${AREngine_INCLUDE_DIRS}")
+   ENDIF (NOT AREngine_FIND_QUIETLY)
+ELSE (AREngine_FOUND)
+   # fatal error if AREngine is required but not found
+   IF (AREngine_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find AREngine's impl, jni and ndk libraries)")
+   ENDIF (AREngine_FIND_REQUIRED)
+ENDIF (AREngine_FOUND)
+

cmake_modules/FindFastCV.cmake

@@ -0,0 +1,37 @@
+# - Find FastCV (https://developer.qualcomm.com/software/fastcv-sdk)
+#
+# It sets the following variables:
+#  FastCV_FOUND         - Set to false, or undefined, if FastCV isn't found.
+#  FastCV_INCLUDE_DIRS  - The FastCV include directory.
+#  FastCV_LIBRARIES     - The FastCV library to link against.
+
+IF(NOT WIN32 AND NOT APPLE)
+
+   EXECUTE_PROCESS( COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE )
+   MESSAGE( STATUS "Architecture: ${ARCHITECTURE}" )
+
+   # Currently only tested on aarch64!
+   IF(${ARCHITECTURE} STREQUAL "aarch64")
+      find_path(FastCV_INCLUDE_DIRS NAMES fastcv.h PATH_SUFFIXES fastcv)
+      find_library(FastCV_LIBRARY NAMES fastcvopt fastcv)
+   ENDIF(${ARCHITECTURE} STREQUAL "aarch64")
+ENDIF(NOT WIN32 AND NOT APPLE)
+
+IF (FastCV_INCLUDE_DIRS AND FastCV_LIBRARY)
+   SET(FastCV_FOUND TRUE)
+ENDIF (FastCV_INCLUDE_DIRS AND FastCV_LIBRARY)
+
+IF (FastCV_FOUND)
+   SET(FastCV_LIBRARIES ${FastCV_LIBRARY})
+     
+   # show which RealSense was found only if not quiet
+   IF (NOT FastCV_FIND_QUIETLY)
+      MESSAGE(STATUS "Found FastCV: ${FastCV_LIBRARIES}")
+   ENDIF (NOT FastCV_FIND_QUIETLY)
+ELSE (FastCV_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (FastCV_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find FastCV")
+   ENDIF (FastCV_FIND_REQUIRED)
+ENDIF (FastCV_FOUND)
+

cmake_modules/FindFlyCapture2.cmake

@@ -7,32 +7,39 @@
 #  FlyCapture2_LIBRARIES     - The FlyCapture2 library to link against.
 
 if(CMAKE_CL_64)
-set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib64)
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib64 $ENV{FC2LIB}/lib64)
 else()
-set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib)
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib $ENV{FC2LIB}/lib)
 endif()
 
 if(CMAKE_CL_64)
-set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib64)
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib64 $ENV{TRICLOPSLIB}/lib64)
 else()
-set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib)
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib $ENV{TRICLOPSLIB}/lib)
 endif()
 
 #FlyCapture2 SDK
-find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include)
-find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR})
+find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include $ENV{FlyCapture2_ROOT_DIR}/include/flycapture $ENV{FC2LIB}/include $ENV{FC2LIB}/include/flycapture)
+find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v140 FlyCapture2_v100 FlyCapture2 flycapture2 flycapture NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR}/vs2015 ${FlyCapture2_LIBDIR})
+
+MESSAGE(STATUS "FlyCapture2_INCLUDE_DIR=${FlyCapture2_INCLUDE_DIR}")
+MESSAGE(STATUS "FlyCapture2_LIBRARY=${FlyCapture2_LIBRARY}")
 
 # Triclops SDK
-find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include)
-find_library(Triclops_LIBRARY NAMES triclops triclops_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include $ENV{Triclops_ROOT_DIR}/include/triclops $ENV{TRICLOPSLIB}/include $ENV{TRICLOPSLIB}/include/triclops)
+find_library(Triclops_LIBRARY NAMES triclops triclops_v140 triclops_v100 libtriclops.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v140 flycapture2bridge_v100 libflycapture2bridge.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v140 pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
 
-IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+MESSAGE(STATUS "Triclops_INCLUDE_DIR=${Triclops_INCLUDE_DIR}")
+MESSAGE(STATUS "Triclops_LIBRARY=${Triclops_LIBRARY}")
+MESSAGE(STATUS "FlyCaptureBridge_LIBRARY=${FlyCaptureBridge_LIBRARY}")
+
+IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY)
    SET(FlyCapture2_FOUND TRUE)
    SET(FlyCapture2_INCLUDE_DIRS ${FlyCapture2_INCLUDE_DIR} ${Triclops_INCLUDE_DIR})
-   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
-ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY})
+ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY)
 
 IF (FlyCapture2_FOUND)
    # show which FlyCapture2 was found only if not quiet

cmake_modules/FindG2O.cmake

@@ -17,6 +17,14 @@ FIND_LIBRARY(CHOLMOD_LIB cholmod)
 FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 
   PATHS "C:\\Program Files\\g2o\\include")
 
+FIND_FILE(G2O_CONFIG_FILE g2o/config.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
+#ifdef G2O_NUMBER_FORMAT_STR
+#define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
+#endif
+
 # Macro to unify finding both the debug and release versions of the
 # libraries; this is adapted from the rtabmap config
 
@@ -75,7 +83,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER
 
 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
   SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
   SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -105,5 +113,15 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	  ${CHOLMOD_LIB})
   ENDIF(G2O_SOLVER_CHOLMOD)
 
+  FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
+  STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
+  IF(${matchres} EQUAL -1)
+    MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
+    SET(G2O_CPP11 0)
+  ELSE()
+    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
+    SET(G2O_CPP11 1)
+  ENDIF()
+
   SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)

cmake_modules/FindK4A.cmake

@@ -0,0 +1,34 @@
+# - Find K4A 
+# This module finds an kinect 4 azure SDK
+#
+# It sets the following variables:
+#  K4A_FOUND       - Set to false, or undefined, if K4A isn't found.
+#  K4A_INCLUDE_DIRS - The K4A include directory.
+#  K4A_LIBRARIES     - The K4A library to link against.
+
+find_library(K4A_LIBRARY NAMES k4a NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_library(K4ARECORD_LIBRARY NAMES k4arecord NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_path(K4A_INCLUDE_DIR NAMES k4a/k4a.h PATHS $ENV{K4A_ROOT_DIR}/sdk/include)
+
+IF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+   SET(K4A_FOUND TRUE)
+   SET(K4A_INCLUDE_DIRS ${K4A_INCLUDE_DIR})
+   SET(K4A_LIBRARIES ${K4A_LIBRARY} ${K4ARECORD_LIBRARY})
+
+   # Compatibility with linux names
+   SET(k4a_LIBRARIES ${K4A_LIBRARIES})
+   SET(k4a_INCLUDE_DIRS ${K4A_INCLUDE_DIRS})
+   SET(k4a_FOUND ${K4A_FOUND})
+ENDIF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+
+IF (K4A_FOUND)
+   # show which K4A was found only if not quiet
+   IF (NOT K4A_FIND_QUIETLY)
+      MESSAGE(STATUS "Found K4A: ${K4A_LIBRARIES}")
+   ENDIF (NOT K4A_FIND_QUIETLY)
+ELSE (K4A_FOUND)
+   # fatal error if K4A is required but not found
+   IF (K4A_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find K4A (Kinect for Azure SDK)")
+   ENDIF (K4A_FIND_REQUIRED)
+ENDIF (K4A_FOUND)

cmake_modules/FindKinectSDK2.cmake

@@ -0,0 +1,183 @@
+#.rst:
+# FindKinectSDK2
+# --------------
+#
+# Find Kinect for Windows SDK v2 (Kinect SDK v2) include dirs, library dirs, libraries
+#
+# Use this module by invoking find_package with the form::
+#
+#    find_package( KinectSDK2 [REQUIRED] )
+#
+# Results for users are reported in following variables::
+#
+#    KinectSDK2_FOUND                - Return "TRUE" when Kinect SDK v2 found. Otherwise, Return "FALSE".
+#    KinectSDK2_INCLUDE_DIRS         - Kinect SDK v2 include directories. (${KinectSDK2_DIR}/inc)
+#    KinectSDK2_LIBRARY_DIRS         - Kinect SDK v2 library directories. (${KinectSDK2_DIR}/Lib/x86 or ${KinectSDK2_DIR}/Lib/x64)
+#    KinectSDK2_LIBRARIES            - Kinect SDK v2 library files. (${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib (If check the box of any application festures, corresponding library will be added.))
+#    KinectSDK2_COMMANDS             - Copy commands of redist files for application functions of Kinect SDK v2. (If uncheck the box of all application features, this variable has defined empty command.)
+#
+# This module reads hints about search locations from following environment variables::
+#
+#    KINECTSDK20_DIR                 - Kinect SDK v2 root directory. (This environment variable has been set by installer of Kinect SDK v2.)
+#
+# CMake entries::
+#
+#    KinectSDK2_DIR                  - Kinect SDK v2 root directory. (Default $ENV{KINECTSDK20_DIR})
+#    KinectSDK2_FACE                 - Check the box when using Face or HDFace features. (Default uncheck)
+#    KinectSDK2_FUSION               - Check the box when using Fusion features. (Default uncheck)
+#    KinectSDK2_VGB                  - Check the box when using Visual Gesture Builder features. (Default uncheck)
+#
+# Example to find Kinect SDK v2::
+#
+#    cmake_minimum_required( VERSION 2.8 )
+#
+#    project( project )
+#    add_executable( project main.cpp )
+#    set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "project" )
+#
+#    # Find package using this module.
+#    find_package( KinectSDK2 REQUIRED )
+#
+#    if(KinectSDK2_FOUND)
+#      # [C/C++]>[General]>[Additional Include Directories]
+#      include_directories( ${KinectSDK2_INCLUDE_DIRS} )
+#
+#      # [Linker]>[General]>[Additional Library Directories]
+#      link_directories( ${KinectSDK2_LIBRARY_DIRS} )
+#
+#      # [Linker]>[Input]>[Additional Dependencies]
+#      target_link_libraries( project ${KinectSDK2_LIBRARIES} )
+#
+#      # [Build Events]>[Post-Build Event]>[Command Line]
+#      add_custom_command( TARGET project POST_BUILD ${KinectSDK2_COMMANDS} )
+#    endif()
+#
+# =============================================================================
+#
+# Copyright (c) 2016 Tsukasa SUGIURA
+# Distributed under the MIT License.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+#
+# =============================================================================
+
+##### Utility #####
+
+# Check Directory Macro
+macro(CHECK_DIR _DIR)
+  if(NOT EXISTS "${${_DIR}}")
+    message(WARNING "Directory \"${${_DIR}}\" not found.")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_DIR)
+  endif()
+endmacro()
+
+# Check Files Macro
+macro(CHECK_FILES _FILES _DIR)
+  set(_MISSING_FILES)
+  foreach(_FILE ${${_FILES}})
+    if(NOT EXISTS "${_FILE}")
+      get_filename_component(_FILE ${_FILE} NAME)
+      set(_MISSING_FILES "${_MISSING_FILES}${_FILE}, ")
+    endif()
+  endforeach()
+  if(_MISSING_FILES)
+    message(WARNING "In directory \"${${_DIR}}\" not found files: ${_MISSING_FILES}")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_FILES)
+  endif()
+endmacro()
+
+# Target Platform
+set(TARGET_PLATFORM)
+if(NOT CMAKE_CL_64)
+  set(TARGET_PLATFORM x86)
+else()
+  set(TARGET_PLATFORM x64)
+endif()
+
+##### Find Kinect SDK v2 #####
+
+# Found
+set(KinectSDK2_FOUND TRUE)
+if(MSVC_VERSION LESS 1700)
+  message(WARNING "Kinect for Windows SDK v2 supported Visual Studio 2012 or later.")
+  set(KinectSDK2_FOUND FALSE)
+endif()
+
+# Options
+option(KinectSDK2_FACE "Face and HDFace features" FALSE)
+option(KinectSDK2_FUSION "Fusion features" FALSE)
+option(KinectSDK2_VGB "Visual Gesture Builder features" FALSE)
+
+# Root Directoty
+set(KinectSDK2_DIR)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_DIR $ENV{KINECTSDK20_DIR} CACHE PATH "Kinect for Windows SDK v2 Install Path." FORCE)
+  check_dir(KinectSDK2_DIR)
+endif()
+
+# Include Directories
+set(KinectSDK2_INCLUDE_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_INCLUDE_DIRS ${KinectSDK2_DIR}/inc)
+  check_dir(KinectSDK2_INCLUDE_DIRS)
+endif()
+
+# Library Directories
+set(KinectSDK2_LIBRARY_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARY_DIRS ${KinectSDK2_DIR}/Lib/${TARGET_PLATFORM})
+  check_dir(KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Dependencies
+set(KinectSDK2_LIBRARIES)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib)
+
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Face.lib)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Fusion.lib)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.VisualGestureBuilder.lib)
+  endif()
+
+  check_files(KinectSDK2_LIBRARIES KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Custom Commands
+set(KinectSDK2_COMMANDS)
+if(KinectSDK2_FOUND)
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Face/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Fusion/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/VGB/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  # Empty Commands
+  if(NOT KinectSDK2_COMMANDS)
+    set(KinectSDK2_COMMANDS COMMAND)
+  endif()
+endif()
+
+message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")

cmake_modules/FindORB_SLAM2.cmake

@@ -0,0 +1,33 @@
+# - Find ORB_SLAM2 
+#
+# It sets the following variables:
+#  ORB_SLAM2_FOUND         - Set to false, or undefined, if ORB_SLAM2 isn't found.
+#  ORB_SLAM2_INCLUDE_DIRS  - The ORB_SLAM2 include directory.
+#  ORB_SLAM2_LIBRARIES     - The ORB_SLAM2 library to link against.
+#
+#  Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder.
+
+find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
+find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
+find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
+find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
+
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+   SET(ORB_SLAM2_FOUND TRUE)
+   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
+   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY})
+ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+
+IF (ORB_SLAM2_FOUND)
+   # show which ORB_SLAM2 was found only if not quiet
+   IF (NOT ORB_SLAM2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}")
+   ENDIF (NOT ORB_SLAM2_FIND_QUIETLY)
+ELSE (ORB_SLAM2_FOUND)
+   # fatal error if ORB_SLAM2 is required but not found
+   IF (ORB_SLAM2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2")
+   ENDIF (ORB_SLAM2_FIND_REQUIRED)
+ENDIF (ORB_SLAM2_FOUND)
+

cmake_modules/FindRealSense.cmake

@@ -4,9 +4,18 @@
 #
 # It sets the following variables:
 #  RealSense_FOUND       - Set to false, or undefined, if RealSense isn't found.
+#  RealSenseSlam_FOUND    - Set to false, or undefined, if RealSense slam module isn't found.
 #  RealSense_INCLUDE_DIRS - The RealSense include directory.
 #  RealSense_LIBRARIES     - The RealSense library to link against.
 
+# Use find_package( RealSense COMPONENTS slam ) to search for realsense slam library
+if( RealSense_FIND_COMPONENTS )
+   foreach( component ${RealSense_FIND_COMPONENTS} )
+     string( TOUPPER ${component} _COMPONENT )
+     set( REALSENSE_USE_${_COMPONENT} 1 )
+   endforeach()
+endif()
+
 #RealSense library
 find_path(RealSense_INCLUDE_DIRS NAMES librealsense/rs.hpp PATHS $ENV{RealSense_ROOT_DIR}/include)
 if(CMAKE_CL_64)
@@ -19,9 +28,44 @@ IF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
    SET(RealSense_FOUND TRUE)
 ENDIF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
 
+#SLAM
+if(REALSENSE_USE_SLAM)
+   find_path(RealSenseSlam_INCLUDE_DIRS NAMES librealsense/slam/slam.h PATHS $ENV{RealSense_ROOT_DIR}/include)
+   if(CMAKE_CL_64)
+      find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+      find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+      find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+      find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+   else()
+      find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+      find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+      find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+      find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+   endif()
+else()
+   set(RealSenseSlam_INCLUDE_DIRS "")
+endif()
+
 IF (RealSense_FOUND)
-   # show which RealSense was found only if not quiet
+
+   IF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+      SET(RealSenseSlam_FOUND TRUE)
+   ENDIF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+
    SET(RealSense_LIBRARIES ${RealSense_LIBRARY})
+   IF (RealSenseSlam_FOUND)
+      IF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+         SET(RealSenseSlam_FOUND TRUE)
+      ENDIF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+      SET(RealSense_LIBRARIES 
+            ${RealSense_LIBRARIES}
+            ${RealSenseSlam_LIBRARY}
+            ${RealSenseImage_LIBRARY}
+            ${RealSenseSP_Core_LIBRARY}
+            ${RealSenseTracker_LIBRARY})
+   ENDIF(RealSenseSlam_FOUND)
+
+   # show which RealSense was found only if not quiet
    IF (NOT RealSense_FIND_QUIETLY)
       MESSAGE(STATUS "Found RealSense: ${RealSense_LIBRARIES}")
    ENDIF (NOT RealSense_FIND_QUIETLY)

cmake_modules/FindRealSense2.cmake

@@ -0,0 +1,41 @@
+# - Find librealsense (https://github.com/IntelRealSense/librealsense)
+#
+#  RealSense2_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  RealSense2_FOUND         - Set to false, or undefined, if RealSense2 isn't found.
+#  RealSense2_INCLUDE_DIRS  - The RealSense2 include directory.
+#  RealSense2_LIBRARIES     - The RealSense2 library to link against.
+
+#RealSense library
+
+find_path(RealSense2_INCLUDE_DIRS NAMES librealsense2/rs.hpp PATHS $ENV{RealSense2_ROOT_DIR}/include)
+if(CMAKE_CL_64)
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x64  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x64)
+else()
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x86  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x86)
+endif()
+
+IF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+   SET(RealSense2_FOUND TRUE)
+ENDIF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+
+IF (RealSense2_FOUND)
+   SET(RealSense2_LIBRARIES ${RealSense2_LIBRARY})
+   
+   # Compatibility with linux names
+   SET(realsense2_LIBRARIES ${RealSense2_LIBRARIES})
+   SET(realsense2_INCLUDE_DIRS ${RealSense2_INCLUDE_DIRS})
+   SET(realsense2_FOUND ${RealSense2_FOUND})
+  
+   # show which RealSense was found only if not quiet
+   IF (NOT RealSense2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RealSense: ${RealSense2_LIBRARIES}")
+   ENDIF (NOT RealSense2_FIND_QUIETLY)
+ELSE (RealSense2_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (RealSense2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RealSense2 (librealsense2)")
+   ENDIF (RealSense2_FIND_REQUIRED)
+ENDIF (RealSense2_FOUND)
+

cmake_modules/FindSqlite3.cmake

@@ -2,46 +2,29 @@
 # This module finds an installed Sqlite3 package.
 #
 # It sets the following variables:
-#  SQLITE3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  SQLITE3_INCLUDE_DIR - The Sqlite3 include directory.
-#  SQLITE3_LIBRARY     - The Sqlite3 library to link against.
+#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
+#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
+#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.
 
-SET(SQLITE3_VERSION_REQUIRED "3.6.0")
+FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})
 
-IF(UNIX)
-	FIND_PROGRAM(SQLITE3_EXEC NAME sqlite3 PATHS)  
-	IF(SQLITE3_EXEC)  
-        MESSAGE(STATUS "Found Sqlite3 executable : ${SQLITE3_EXEC}")
-		EXECUTE_PROCESS(COMMAND ${SQLITE3_EXEC} --version 
-				   	    OUTPUT_VARIABLE SQLITE3_VERSION
-						OUTPUT_STRIP_TRAILING_WHITESPACE
-						WORKING_DIRECTORY "./"
-   		)
-		IF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-		    MESSAGE(FATAL_ERROR "Sqlite ${SQLITE3_VERSION} found, but version ${SQLITE3_VERSION_REQUIRED} minimum is required")
-		ENDIF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-	ELSE(SQLITE3_EXEC)
-		MESSAGE(FATAL_ERROR "Could not find Sqlite3 executable")
-	ENDIF(SQLITE3_EXEC)
-ENDIF(UNIX)
+FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
 
-FIND_PATH(SQLITE3_INCLUDE_DIR sqlite3.h)
+IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
+   SET(Sqlite3_FOUND TRUE)
+   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
+   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
+ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
 
-FIND_LIBRARY(SQLITE3_LIBRARY NAMES sqlite3.dll sqlite3)
-
-IF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-   SET(SQLITE3_FOUND TRUE)
-ENDIF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-
-IF (SQLITE3_FOUND)
+IF (Sqlite3_FOUND)
    # show which Sqlite3 was found only if not quiet
    IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3")
+      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
    ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (SQLITE3_FOUND)
+ELSE (Sqlite3_FOUND)
    # fatal error if Sqlite3 is required but not found
    IF (Sqlite3_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Sqlite3")
    ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (SQLITE3_FOUND)
+ENDIF (Sqlite3_FOUND)
 

cmake_modules/FindTango.cmake

@@ -8,8 +8,8 @@
 
 FIND_PATH(Tango_INCLUDE_DIR tango_client_api.h)
 
-FIND_LIBRARY(Tango_LIBRARY NAMES tango_client_api)
-FIND_LIBRARY(Tango_support_LIBRARY NAMES tango_support_api)
+FIND_LIBRARY(Tango_LIBRARY NAMES tango_client_api PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(Tango_support_LIBRARY NAMES tango_support_api PATH_SUFFIXES ${ANDROID_ABI})
 
 IF (Tango_INCLUDE_DIR AND Tango_LIBRARY AND Tango_support_LIBRARY)
    SET(Tango_FOUND TRUE)

corelib/include/rtabmap/core/BayesFilter.h

@@ -59,18 +59,14 @@ public:
 	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
 	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
 
-	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
+	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,
 			const std::vector<int> & oldIds,
-			const std::vector<int> & newIds) const;
+			const std::vector<int> & newIds);
 	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
-	float addNeighborProb(cv::Mat & prediction,
-			unsigned int col,
-			const std::map<int, int> & neighbors,
-			const std::map<int, int> & idToIndexMap) const;
 	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
 
 private:
@@ -80,6 +76,8 @@ private:
 	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
 	bool _fullPredictionUpdate;
 	float _totalPredictionLCValues;
+	float _predictionEpsilon;
+	std::map<int, std::map<int, int> > _neighborsIndex;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Camera.h

@@ -66,6 +66,7 @@ public:
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 
+	void resetTimer();
 protected:
 	/**
 	 * Constructor

corelib/include/rtabmap/core/CameraInfo.h

@@ -43,6 +43,7 @@ public:
 		timeCapture(0.0f),
 		timeDisparity(0.0f),
 		timeMirroring(0.0f),
+		timeStereoExposureCompensation(0.0f),
 		timeImageDecimation(0.0f),
 		timeScanFromDepth(0.0f),
 		timeUndistortDepth(0.0f),
@@ -59,6 +60,7 @@ public:
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
+	float timeStereoExposureCompensation;
 	float timeImageDecimation;
 	float timeScanFromDepth;
 	float timeUndistortDepth;
@@ -66,6 +68,7 @@ public:
 	float timeTotal;
 	Transform odomPose;
 	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraModel.h

@@ -50,7 +50,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
 
 	// minimal
 	CameraModel(
@@ -58,7 +58,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,13 +68,14 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
 	virtual ~CameraModel() {}
 
 	void initRectificationMap();
+	bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
 
 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
@@ -112,8 +113,14 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
+	double fovX() const {return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;}
+	double fovY() const {return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;}
+
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
+	std::vector<unsigned char> serialize() const;
+	unsigned int deserialize(const std::vector<unsigned char>& data);
+	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;

corelib/include/rtabmap/core/CameraRGB.h

@@ -27,201 +27,5 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/utilite/UTimer.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
-
-class UDirectory;
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraImages
-/////////////////////////
-class RTABMAP_EXP CameraImages :
-	public Camera
-{
-public:
-	CameraImages();
-	CameraImages(
-			const std::string & path,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const { return odometry_.size() > 0; }
-	std::string getPath() const {return _path;}
-	unsigned int imagesCount() const;
-	std::vector<std::string> filenames() const;
-	bool isImagesRectified() const {return _rectifyImages;}
-	int getBayerMode() const {return _bayerMode;}
-	const CameraModel & cameraModel() const {return _model;}
-
-	void setPath(const std::string & dir) {_path=dir;}
-	void setStartIndex(int index) {_startAt = index;} // negative means last
-	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
-	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
-	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
-
-	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
-	{
-		_filenamesAreTimestamps = fileNamesAreStamps;
-		_timestampsPath=filePath;
-		_syncImageRateWithStamps = syncImageRateWithStamps;
-	}
-
-	void setScanPath(
-			const std::string & dir,
-			int maxScanPts = 0,
-			int downsampleStep = 1,
-			float voxelSize = 0.0f,
-			int normalsK = 0, // compute normals if > 0
-			const Transform & localTransform=Transform::getIdentity())
-	{
-		_scanPath = dir;
-		_scanLocalTransform = localTransform;
-		_scanMaxPts = maxScanPts;
-		_scanDownsampleStep = downsampleStep;
-		_scanNormalsK = normalsK;
-		_scanVoxelSize = voxelSize;
-		if(_scanDownsampleStep>1)
-		{
-			_scanMaxPts /= _scanDownsampleStep;
-		}
-	}
-
-	void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
-	{
-		_depthFromScan = enabled;
-		_depthFromScanFillHoles = fillHoles;
-		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
-	}
-
-	void setOdometryPath(const std::string & filePath, int format = 0)
-	{
-		_odometryPath = filePath;
-		_odometryFormat = format;
-	}
-
-	void setGroundTruthPath(const std::string & filePath, int format = 0)
-	{
-		_groundTruthPath = filePath;
-		_groundTruthFormat = format;
-	}
-
-	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
-	{
-		_isDepth = isDepth;
-		_depthScaleFactor=depthScaleFactor;
-	}
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	bool readPoses(std::list<Transform> & outputPoses, std::list<double> & stamps, const std::string & filePath, int format) const;
-
-private:
-	std::string _path;
-	int _startAt;
-	// If the list of files in the directory is refreshed
-	// on each call of takeImage()
-	bool _refreshDir;
-	bool _rectifyImages;
-	int _bayerMode;
-	bool _isDepth;
-	float _depthScaleFactor;
-	int _count;
-	UDirectory * _dir;
-	std::string _lastFileName;
-
-	int _countScan;
-	UDirectory * _scanDir;
-	std::string _lastScanFileName;
-	std::string _scanPath;
-	Transform _scanLocalTransform;
-	int _scanMaxPts;
-	int _scanDownsampleStep;
-	float _scanVoxelSize;
-	int _scanNormalsK;
-
-	bool _depthFromScan;
-	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
-	bool _depthFromScanFillHolesFromBorder;
-
-	bool _filenamesAreTimestamps;
-	std::string _timestampsPath;
-	bool _syncImageRateWithStamps;
-
-	std::string _odometryPath;
-	int _odometryFormat;
-
-	std::string _groundTruthPath;
-	int _groundTruthFormat;
-
-	std::list<double> _stamps;
-	std::list<Transform> odometry_;
-	std::list<Transform> groundTruth_;
-	CameraModel _model;
-
-	UTimer _captureTimer;
-	double _captureDelay;
-};
-
-
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-class RTABMAP_EXP CameraVideo :
-	public Camera
-{
-public:
-	enum Source{kVideoFile, kUsbDevice};
-
-public:
-	CameraVideo(int usbDevice = 0,
-			bool rectifyImages = false,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraVideo(const std::string & filePath,
-			bool rectifyImages = false,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraVideo();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	int getUsbDevice() const {return _usbDevice;}
-	const std::string & getFilePath() const {return _filePath;}
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	// File type
-	std::string _filePath;
-	bool _rectifyImages;
-
-	cv::VideoCapture _capture;
-	Source _src;
-
-	// Usb camera
-	int _usbDevice;
-	std::string _guid;
-
-	CameraModel _model;
-};
-
-
-} // namespace rtabmap
+#include <rtabmap/core/camera/CameraImages.h>
+#include <rtabmap/core/camera/CameraVideo.h>

corelib/include/rtabmap/core/CameraRGBD.h

@@ -27,331 +27,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include <rtabmap/core/camera/CameraFreenect.h>
+#include <rtabmap/core/camera/CameraFreenect2.h>
+#include <rtabmap/core/camera/CameraK4W2.h>
+#include <rtabmap/core/camera/CameraOpenni.h>
+#include <rtabmap/core/camera/CameraOpenNI2.h>
+#include <rtabmap/core/camera/CameraOpenNICV.h>
+#include <rtabmap/core/camera/CameraRealSense.h>
+#include <rtabmap/core/camera/CameraRealSense2.h>
+#include <rtabmap/core/camera/CameraRGBDImages.h>
+#include <rtabmap/core/camera/CameraK4A.h>
 
-#include "rtabmap/utilite/UMutex.h"
-#include "rtabmap/utilite/USemaphore.h"
-#include "rtabmap/core/CameraModel.h"
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraRGB.h"
-#include "rtabmap/core/Version.h"
-
-#include <pcl/pcl_config.h>
-
-#ifdef HAVE_OPENNI
-#include <pcl/io/openni_camera/openni_depth_image.h>
-#include <pcl/io/openni_camera/openni_image.h>
-#endif
-
-#include <boost/signals2/connection.hpp>
-
-namespace openni
-{
-class Device;
-class VideoStream;
-}
-
-namespace pcl
-{
-class Grabber;
-}
-
-namespace libfreenect2
-{
-class Freenect2;
-class Freenect2Device;
-class SyncMultiFrameListener;
-class Registration;
-class PacketPipeline;
-}
-
-namespace rs
-{
-	class context;
-	class device;
-}
-
-typedef struct _freenect_context freenect_context;
-typedef struct _freenect_device freenect_device;
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraOpenNIPCL
-/////////////////////////
-class RTABMAP_EXP CameraOpenni :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraOpenni(const std::string & deviceId="",
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenni();
-#ifdef HAVE_OPENNI
-    void image_cb (
-    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
-			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
-			float constant);
-#endif
-
-    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-    virtual bool isCalibrated() const;
-    virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-    pcl::Grabber* interface_;
-    std::string deviceId_;
-    boost::signals2::connection connection_;
-    cv::Mat depth_;
-    cv::Mat rgb_;
-    float depthConstant_;
-    UMutex dataMutex_;
-    USemaphore dataReady_;
-};
-
-/////////////////////////
-// CameraOpenNICV
-/////////////////////////
-class RTABMAP_EXP CameraOpenNICV :
-	public Camera
-{
-
-public:
-	static bool available();
-
-public:
-	CameraOpenNICV(bool asus = false,
-					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenNICV();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const {return "";} // unknown with OpenCV
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	bool _asus;
-	cv::VideoCapture _capture;
-	float _depthFocal;
-};
-
-/////////////////////////
-// CameraOpenNI2
-/////////////////////////
-class RTABMAP_EXP CameraOpenNI2 :
-	public Camera
-{
-
-public:
-	static bool available();
-	static bool exposureGainAvailable();
-	enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
-
-public:
-	CameraOpenNI2(const std::string & deviceId = "",
-					Type type = kTypeColorDepth,
-					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenNI2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-	bool setAutoWhiteBalance(bool enabled);
-	bool setAutoExposure(bool enabled);
-	bool setExposure(int value);
-	bool setGain(int value);
-	bool setMirroring(bool enabled);
-	void setOpenNI2StampsAndIDsUsed(bool used);
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_OPENNI2
-	Type _type;
-	openni::Device * _device;
-	openni::VideoStream * _color;
-	openni::VideoStream * _depth;
-	float _depthFx;
-	float _depthFy;
-	std::string _deviceId;
-	bool _openNI2StampsAndIDsUsed;
-	StereoCameraModel _stereoModel;
-#endif
-};
-
-
-/////////////////////////
-// CameraFreenect
-/////////////////////////
-class FreenectDevice;
-
-class RTABMAP_EXP CameraFreenect :
-	public Camera
-{
-public:
-	static bool available();
-	enum Type {kTypeColorDepth, kTypeIRDepth};
-
-public:
-	// default local transform z in, x right, y down));
-	CameraFreenect(int deviceId= 0,
-					Type type = kTypeColorDepth,
-					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraFreenect();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FREENECT
-	int deviceId_;
-	Type type_;
-	freenect_context * ctx_;
-	FreenectDevice * freenectDevice_;
-	StereoCameraModel stereoModel_;
-#endif
-};
-
-/////////////////////////
-// CameraFreenect2
-/////////////////////////
-
-class RTABMAP_EXP CameraFreenect2 :
-	public Camera
-{
-public:
-	static bool available();
-
-	enum Type{
-		kTypeColor2DepthSD,
-		kTypeDepth2ColorSD,
-		kTypeDepth2ColorHD,
-		kTypeDepth2ColorHD2,
-		kTypeIRDepth,
-		kTypeColorIR
-	};
-
-public:
-	// default local transform z in, x right, y down));
-	CameraFreenect2(int deviceId= 0,
-					Type type = kTypeColor2DepthSD,
-					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
-					float minDepth = 0.3f,
-					float maxDepth = 12.0f,
-					bool bilateralFiltering = true,
-					bool edgeAwareFiltering = true,
-					bool noiseFiltering = true);
-	virtual ~CameraFreenect2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FREENECT2
-	int deviceId_;
-	Type type_;
-	StereoCameraModel stereoModel_;
-	libfreenect2::Freenect2 * freenect2_;
-	libfreenect2::Freenect2Device *dev_;
-	libfreenect2::SyncMultiFrameListener * listener_;
-	libfreenect2::Registration * reg_;
-	float minKinect2Depth_;
-	float maxKinect2Depth_;
-	bool bilateralFiltering_;
-	bool edgeAwareFiltering_;
-	bool noiseFiltering_;
-#endif
-};
-
-/////////////////////////
-// CameraRealSense
-/////////////////////////
-class RTABMAP_EXP CameraRealSense :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraRealSense(
-		int deviceId = 0,
-		int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRealSense();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_REALSENSE
-	rs::context * ctx_;
-	rs::device * dev_;
-	int deviceId_;
-	int presetRGB_;
-	int presetDepth_;
-#endif
-};
-
-
-/////////////////////////
-// CameraRGBDImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraRGBDImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraRGBDImages(
-			const std::string & pathRGBImages,
-			const std::string & pathDepthImages,
-			float depthScaleFactor = 1.0f,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRGBDImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages cameraDepth_;
-};
-
-} // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -27,225 +27,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include "rtabmap/core/CameraModel.h"
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraRGB.h"
-#include "rtabmap/core/Version.h"
-#include <list>
-
-namespace FlyCapture2
-{
-class Camera;
-}
-
-namespace sl
-{
-namespace zed
-{
-class Camera;
-}
-}
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraStereoDC1394
-/////////////////////////
-class DC1394Device;
-
-class RTABMAP_EXP CameraStereoDC1394 :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoDC1394();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_DC1394
-	DC1394Device *device_;
-	StereoCameraModel stereoModel_;
-#endif
-};
-
-/////////////////////////
-// CameraStereoFlyCapture2
-/////////////////////////
-class RTABMAP_EXP CameraStereoFlyCapture2 :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoFlyCapture2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FLYCAPTURE2
-	FlyCapture2::Camera * camera_;
-	void * triclopsCtx_; // TriclopsContext
-#endif
-};
-
-/////////////////////////
-// CameraStereoZED
-/////////////////////////
-class RTABMAP_EXP CameraStereoZed :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoZed(
-			int deviceId,
-			int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 1,// 0=FULL, 1=RAW
-			int confidenceThr = 100,
-			bool computeOdometry = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
-	CameraStereoZed(
-			const std::string & svoFilePath,
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 1,// 0=FULL, 1=RAW
-			int confidenceThr = 100,
-			bool computeOdometry = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
-	virtual ~CameraStereoZed();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_ZED
-	sl::zed::Camera * zed_;
-	StereoCameraModel stereoModel_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-	std::string svoFilePath_;
-	int resolution_;
-	int quality_;
-	bool selfCalibration_;
-	int sensingMode_;
-	int confidenceThr_;
-	bool computeOdometry_;
-	bool lost_;
-#endif
-};
-
-/////////////////////////
-// CameraStereoImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoImages(
-			const std::string & pathLeftImages,
-			const std::string & pathRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoImages(
-			const std::string & pathLeftRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages * camera2_;
-	StereoCameraModel stereoModel_;
-};
-
-
-/////////////////////////
-// CameraStereoVideo
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoVideo :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoVideo(
-			const std::string & pathSideBySide,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			const std::string & pathLeft,
-			const std::string & pathRight,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			int device,
-			bool rectifyImages = false,
-			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoVideo();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	cv::VideoCapture capture_;
-	cv::VideoCapture capture2_;
-	std::string path_;
-	std::string path2_;
-	bool rectifyImages_;
-	StereoCameraModel stereoModel_;
-	std::string cameraName_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-};
-
-} // namespace rtabmap
+#include <rtabmap/core/camera/CameraStereoDC1394.h>
+#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
+#include <rtabmap/core/camera/CameraStereoImages.h>
+#include <rtabmap/core/camera/CameraStereoVideo.h>
+#include <rtabmap/core/camera/CameraStereoZed.h>
+#include <rtabmap/core/camera/CameraStereoTara.h>
+#include <rtabmap/core/camera/CameraMyntEye.h>

corelib/include/rtabmap/core/CameraThread.h

@@ -42,7 +42,10 @@ namespace rtabmap
 {
 
 class Camera;
+class CameraInfo;
+class SensorData;
 class StereoDense;
+class IMUFilter;
 
 /**
  * Class CameraThread
@@ -58,6 +61,7 @@ public:
 	virtual ~CameraThread();
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
@@ -65,21 +69,31 @@ public:
 	void setDistortionModel(const std::string & path);
 	void enableBilateralFiltering(float sigmaS, float sigmaR);
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
+	void disableIMUFiltering();
 
-	void setScanFromDepth(
-			bool enabled,
-			int decimation=4,
-			float maxDepth=4.0f,
+	void setScanParameters(
+			bool fromDepth,
+			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
+			float rangeMin=0.0f,
+			float rangeMax=0.0f,
 			float voxelSize = 0.0f,
-			int normalsK = 0)
+			int normalsK = 0,
+			int normalsRadius = 0.0f,
+			bool forceGroundNormalsUp = false)
 	{
-		_scanFromDepth = enabled;
-		_scanDecimation=decimation;
-		_scanMaxDepth = maxDepth;
+		_scanFromDepth = fromDepth;
+		_scanDownsampleStep=downsampleStep;
+		_scanRangeMin = rangeMin;
+		_scanRangeMax = rangeMax;
 		_scanVoxelSize = voxelSize;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
+		_scanForceGroundNormalsUp = forceGroundNormalsUp;
 	}
 
+	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
+
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
@@ -94,20 +108,24 @@ private:
 private:
 	Camera * _camera;
 	bool _mirroring;
+	bool _stereoExposureCompensation;
 	bool _colorOnly;
 	int _imageDecimation;
 	bool _stereoToDepth;
 	bool _scanFromDepth;
-	int _scanDecimation;
-	float _scanMaxDepth;
-	float _scanMinDepth;
+	int _scanDownsampleStep;
+	float _scanRangeMin;
+	float _scanRangeMax;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
+	bool _scanForceGroundNormalsUp;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;
 	float _bilateralSigmaS;
 	float _bilateralSigmaR;
+	IMUFilter * _imuFilter;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Compression.h

@@ -83,5 +83,8 @@ cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
 
+cv::Mat RTABMAP_EXP compressString(const std::string & str);
+std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */