bump 0.20.18

CCCorelib: updated wrong output check. Updated RGBD/MaxOdomCacheSize approach to work properly with Rtabmap/CreateIntermediateNodes=true.
fixed build
2026-09-11 14:00:20 +08:00 · 2022-01-28 17:12:24 -05:00 · 2022-01-28 14:48:32 -05:00 · 2022-01-27 22:55:59 -05:00 · 2022-01-27 17:57:56 -05:00 · 2022-01-26 20:12:04 -05:00
801 changed files with 155185 additions and 35087 deletions
@@ -0,0 +1,149 @@
+
+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
+  #- appveyor-retry appveyor DownloadFile http://downloads.sourceforge.net/project/opencvlibrary/4.5.2/opencv-4.5.2-vc14_vc15.exe
+  #- cmd: opencv-4.5.2-vc14_vc15.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
+  - ps: wget 'https://dl.dropboxusercontent.com/s/o6ofn491bc0jso1/opencv450_vc14.exe?dl=0' -outfile opencv.exe
+  - cmd: opencv.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenCV:"
+  - ps: "ls \"C:/Program Files/opencv\""
+  - set PATH=%PATH%;C:\Program Files\opencv\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
+  # yaml-cpp
+  - 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\""
+  # RealSense2
+  - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe
+  - cmd: realsense2.exe /VERYSILENT
+  - ECHO "Installed RealSense2:"
+  - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\""
+  - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64
+  - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0
+  # Kinect 4 Azure
+  - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe
+  - cmd: azure.exe /quiet
+  - ECHO "Installed Kinect For Azure:"
+  - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\""
+  - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools
+  - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1
+  
+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" -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
@@ -0,0 +1,65 @@
+name: CMake
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-18.04]
+        include:
+        - os: ubuntu-20.04
+          ros_distro: 'noetic'
+        - os: ubuntu-18.04
+          ros_distro: 'melodic'
+
+    steps:
+    - uses: ros-tooling/setup-ros@v0.2
+      with:
+        required-ros-distributions: ${{ matrix.ros_distro }}
+    
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
+        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
+
+    - uses: actions/checkout@v2
+
+    - name: Configure CMake
+      # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
+      # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      # Build your program with the given configuration
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        ./rtabmap-console --version
+
+#    - name: Test
+#      working-directory: ${{github.workspace}}/build
+#      # Execute tests defined by the CMake configuration.  
+#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+#      run: ctest -C ${{env.BUILD_TYPE}}
+      
@@ -0,0 +1,96 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      matrix:
+        docker_tag: [xenial, bionic, focal, android23, android24, android26, android30]
+        include:
+        - docker_tag: xenial
+          docker_tags: |
+            introlab3it/rtabmap:xenial
+            introlab3it/rtabmap:16.04
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'xenial'
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'bionic'
+        - docker_tag: focal
+          docker_tags: |
+            introlab3it/rtabmap:focal
+            introlab3it/rtabmap:20.04
+            introlab3it/rtabmap:latest
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api23'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api24'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api26'
+        - docker_tag: android30
+          docker_tags: |
+            introlab3it/rtabmap:android30
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api30'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v1
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v2
+        with:
+          context: ./docker/${{ matrix.docker_path }}
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          build-args: |
+            CACHE_DATE=${{ github.head_ref }}.${{ github.sha }}
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+
@@ -1,7 +1,12 @@
 /lib
 .DS_Store
 .settings/language.settings.xml
+.idea/
+.vscode
+cmake-build-debug/
 app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml
 app/android/res/raw/
+compile_flags.txt
+tags
@@ -1,31 +0,0 @@
-sudo: true
-dist: trusty
-language: cpp
-
-compiler:
-  - gcc
-  - clang
-
-addons:
-  apt:
-    packages:
-      - cmake
-      - libopencv-dev
-      - libqt4-dev
-      - libsqlite3-dev
-
-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
-
-script: 
-    - source /opt/ros/indigo/setup.bash
-    - mkdir -p build && cd build
-    - cmake .. 
-    - make
-
-notifications:
-  email:
-    - matlabbe@gmail.com
@@ -1,5 +1,6 @@
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
+RTAB-Map - https://github.com/introlab/rtabmap
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) XXX, contributors, all rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
@@ -11,7 +12,7 @@ modification, are permitted provided that the following conditions are met:
  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
+* Neither the name of the copyright holders nor the names of the 
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

@@ -24,4 +25,4 @@ 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.
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@@ -1,10 +1,27 @@
-rtabmap [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap)
+rtabmap
 =======

+[![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://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) 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.20.16-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>
@@ -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,18 @@ 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})
+if(@CONF_DEPTH_AI@)
+   FIND_PACKAGE(depthai 2 QUIET REQUIRED)
+endif(@CONF_DEPTH_AI@)
+SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")

 #backward compatibilities
 set(RTABMAP_CORE ${RTABMap_CORE})
@@ -74,3 +93,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)
@@ -40,18 +40,60 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@NONFREE@#define RTABMAP_NONFREE
@TORO@#define RTABMAP_TORO
@G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @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
+@CCCORELIB@#define RTABMAP_CCCORELIB
+@OPEN3D@#define RTABMAP_OPEN3D
+@FASTCV@#define RTABMAP_FASTCV
+@PDAL@#define RTABMAP_PDAL
+@LOAM@#define RTABMAP_LOAM
+@FLOAM@#define RTABMAP_FLOAM
@DC1394@#define RTABMAP_DC1394
@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
@ZED@#define RTABMAP_ZED
+@ZEDOC@#define RTABMAP_ZEDOC
@REALSENSE@#define RTABMAP_REALSENSE
+@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
+@MYNTEYE@#define RTABMAP_MYNTEYE
+@DEPTHAI@#define RTABMAP_DEPTHAI
@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
+@OPENVINS@#define RTABMAP_OPENVINS
+@ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
+@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
+@TORCH@#define RTABMAP_TORCH
+@PYTHON@#define RTABMAP_PYTHON
+@MADGWICK@#define RTABMAP_MADGWICK
+
+#include <pcl/pcl_config.h>
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+#include <pcl/types.h>
+#define RTABMAP_PCL_INDEX pcl::index_t
+#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
+#define RTABMAP_PCL_INDEX std::uint32_t
+#else
+#include <pcl/pcl_macros.h>
+#define RTABMAP_PCL_INDEX pcl::uint32_t
+#endif

 #endif /* VERSION_H_ */

@@ -1 +1,2 @@
 /bin/
+/gen/
@@ -2,44 +2,87 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
        package="com.introlab.rtabmap"
-        android:versionCode="34"
+        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.ACCESS_MEDIA_LOCATION" />
    <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="19" />
+    <!-- This is the platform API where depth16 support in android was introduced. -->
+    <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
+    
+    <queries>
+        <package android:name="com.google.ar.core" />
+        <package android:name="com.huawei.ar.engine" /> 
+    </queries>

    <!-- 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" />
            <intent-filter>
                <action android:name="android.intent.action.MAIN" />
                <category android:name="android.intent.category.LAUNCHER" />
-            </intent-filter>
+		    </intent-filter>
+		    <intent-filter>
+		        <action android:name="android.intent.action.SEND" />
+		        <action android:name="android.intent.action.SEND_MULTIPLE" />
+		        <action android:name="android.intent.action.OPEN_DOCUMENT" />
+		        <category android:name="android.intent.category.DEFAULT" />
+		        <data android:mimeType="application/octet-stream" />
+		        <data android:pathPattern=".*\.db" />
+		    </intent-filter>
+		    
        </activity>

-        <activity android:name="SettingsActivity" android:label="@string/settings"/>
+        <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>
@@ -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"
@@ -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_ */
+
@@ -4,7 +4,10 @@
 			<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"/>
+			<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>
+</project>
@@ -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>

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

-find_package(Tango REQUIRED)
-
 SET(INCLUDE_DIRS
    ${CMAKE_CURRENT_SOURCE_DIR}
    ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
    ${CMAKE_CURRENT_SOURCE_DIR}/third-party/include
+	${PROJECT_BINARY_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${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
  graph_drawable.cpp
+  background_renderer.cc
  tango-gl/axis.cpp 
  tango-gl/camera.cpp 
  tango-gl/conversions.cpp 
@@ -44,6 +38,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 +128,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)
@@ -0,0 +1,704 @@
+/*
+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 {
+
+//////////////////////////////
+// CameraARCore
+//////////////////////////////
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing):
+	CameraMobile(smoothing),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false),
+	depthFromMotion_(depthFromMotion)
+{
+}
+
+CameraARCore::~CameraARCore() {
+	// Disconnect ARCore service
+	close();
+}
+
+
+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";
+}
+
+bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	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_);
+
+	int32_t is_depth_supported = 0;
+	ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+	ArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	if (is_depth_supported!=0) {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
+	} else {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
+	}
+
+	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;
+
+	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;
+
+	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();
+}
+
+void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		ArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
+SensorData CameraARCore::captureImage(CameraInfo * info)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	if(textureId_ == 0)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	if(textureId_!=0)
+		ArSession_setCameraTextureName(arSession_, textureId_);
+
+	// 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;
+	}
+
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
+	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;
+
+		Transform poseArCore = pose;
+		if(pose.isNull())
+		{
+			LOGE("CameraARCore: Pose is null");
+		}
+		else
+		{
+			this->poseReceived(pose);
+			// adjust origin
+			if(!getOriginOffset().isNull())
+			{
+				pose = getOriginOffset() * pose;
+			}
+			info->odomPose = pose;
+		}
+
+		// 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);
+
+			int32_t is_depth_supported = 0;
+			ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+			ArImage * image = nullptr;
+			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			if(status == AR_SUCCESS)
+			{
+				if(is_depth_supported)
+				{
+					LOGD("Acquire depth image!");
+					ArImage * depthImage = nullptr;
+					ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+					ArImageFormat format;
+					ArImage_getFormat(arSession_, depthImage, &format);
+					if(format == AR_IMAGE_FORMAT_DEPTH16)
+					{
+						LOGD("Depth format detected!");
+						int planeCount;
+						ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+						LOGD("planeCount=%d", planeCount);
+						UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+						const uint8_t *data = nullptr;
+						int len = 0;
+						int stride;
+						int depth_width;
+						int depth_height;
+						ArImage_getWidth(arSession_, depthImage, &depth_width);
+						ArImage_getHeight(arSession_, depthImage, &depth_height);
+						ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+						ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
+
+						cv::Mat occlusionImage = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+
+						float scaleX = (float)depth_width / (float)width;
+						float scaleY = (float)depth_height / (float)height;
+						CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+						this->setOcclusionImage(occlusionImage, occlusionModel);
+					}
+					ArImage_release(depthImage);
+				}
+
+				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::COLOR_YUV2BGR_NV21);
+						}
+						else
+						{
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, cv::COLOR_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, poseArCore, model, rgb, &kpts, &kpts3);
+							}
+						}
+						else
+						{
+							LOGI("pointCloud empty");
+						}
+
+						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():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);
+
+	return data;
+
+}
+
+void CameraARCore::capturePoseOnly()
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	if(!arSession_)
+	{
+		return;
+	}
+
+	if(textureId_ != 0)
+	{
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		ArSession_setCameraTextureName(arSession_, textureId_);
+	}
+
+	// Update session to get current frame and render camera background.
+	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
+		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
+		return;
+	}
+
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
+	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);
+
+			if(!getOriginOffset().isNull())
+			{
+				pose = getOriginOffset() * pose;
+			}
+		}
+
+		int32_t is_depth_supported = 0;
+		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+		if(is_depth_supported)
+		{
+			LOGD("Acquire depth image!");
+			ArImage * depthImage = nullptr;
+			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+			ArImageFormat format;
+			ArImage_getFormat(arSession_, depthImage, &format);
+			if(format == AR_IMAGE_FORMAT_DEPTH16)
+			{
+				LOGD("Depth format detected!");
+				int planeCount;
+				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+				LOGD("planeCount=%d", planeCount);
+				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+				const uint8_t *data = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				ArImage_getWidth(arSession_, depthImage, &width);
+				ArImage_getHeight(arSession_, depthImage, &height);
+				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
+
+				cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+
+				float fx,fy, cx, cy;
+				int32_t rgb_width, rgb_height;
+				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
+
+				float scaleX = (float)width / (float)rgb_width;
+				float scaleY = (float)height / (float)rgb_height;
+				CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+				this->setOcclusionImage(occlusionImage, occlusionModel);
+			}
+			ArImage_release(depthImage);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+}
+
+} /* namespace rtabmap */
@@ -0,0 +1,103 @@
+/*
+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 <background_renderer.h>
+
+#include <arcore_c_api.h>
+#include <camera/NdkCameraDevice.h>
+#include <camera/NdkCameraManager.h>
+#include <media/NdkImageReader.h>
+#include <android/native_window.h>
+
+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 depthFromMotion = false, bool smoothing = false);
+	virtual ~CameraARCore();
+
+	bool uvsInitialized() const {return uvs_initialized_;}
+	const float* uvsTransformed() const {return transformed_uvs_;}
+	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	void setupGL();
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+	GLuint getTextureId() const {return textureId_;}
+
+	void imageCallback(AImageReader *reader);
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
+	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_;
+	UMutex arSessionMutex_;
+
+	bool depthFromMotion_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARCORE_H_ */
@@ -0,0 +1,356 @@
+/*
+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::COLOR_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);
+        // adjust origin
+        if(!getOriginOffset().isNull())
+        {
+            pose = getOriginOffset() * 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 */
@@ -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_ */
@@ -0,0 +1,461 @@
+/*
+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"
+#include <glm/gtx/transform.hpp>
+
+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),
+		textureId_(0),
+		uvs_initialized_(false),
+		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();
+
+    if(textureId_ != 0)
+    {
+        glDeleteTextures(1, &textureId_);
+        textureId_ = 0;
+    }
+}
+
+void CameraMobile::resetOrigin()
+{
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	pose_ = Transform();
+	data_ = SensorData();
+	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, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
+{
+	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	data_ = data;
+    pose_ = pose;
+
+    viewMatrix_ = viewMatrix;
+    projectionMatrix_ = projectionMatrix;
+    
+    // adjust origin
+    if(!originOffset_.isNull())
+    {
+        pose_ = originOffset_ * pose_;
+        viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+    }
+    
+    if(textureId_ == 0)
+    {
+    	glGenTextures(1, &textureId_);
+    }
+
+    if(texCoord)
+    {
+        memcpy(transformed_uvs_, texCoord, 8*sizeof(float));
+        uvs_initialized_ = true;
+    }
+    
+    LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
+
+    if(textureId_ != 0 && texCoord != 0)
+    {
+        cv::Mat rgbImage;
+        cv::cvtColor(data.imageRaw(), rgbImage, cv::COLOR_BGR2RGBA);
+        
+        glBindTexture(GL_TEXTURE_2D, textureId_);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+        
+        glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
+        //glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+        //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
+        //glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
+        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
+
+        GLint error = glGetError();
+        if(error != GL_NO_ERROR)
+        {
+            LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+            textureId_ = 0;
+            return;
+        }
+    }
+}
+
+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)
+		{
+			UDEBUG("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);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_180)
+		{
+			UDEBUG("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);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_270)
+		{
+			UDEBUG("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);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+		}
+
+		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() && info.odomPose.isNull())
+	{
+		LOGW("Odometry lost");
+		this->post(new OdometryEvent());
+	}
+}
+
+SensorData CameraMobile::captureImage(CameraInfo * info)
+{
+	if(info)
+	{
+		info->odomPose = pose_;
+	}
+	return data_;
+}
+
+LaserScan CameraMobile::scanFromPointCloudData(
+        const cv::Mat & pointCloudData,
+        int points,
+        const Transform & pose,
+        const CameraModel & model,
+        const cv::Mat & rgb,
+        std::vector<cv::KeyPoint> * kpts,
+        std::vector<cv::Point3f> * kpts3D,
+        int kptsSize)
+{
+    if(!pointCloudData.empty())
+    {
+        cv::Mat scanData(1, pointCloudData.cols, CV_32FC4);
+        float * ptr = scanData.ptr<float>();
+        const float * inPtr = pointCloudData.ptr<float>();
+        int ic = pointCloudData.channels();
+        UASSERT(pointCloudData.depth() == CV_32F && ic >= 3);
+        
+        int oi = 0;
+        for(unsigned int i=0;i<points; ++i)
+        {
+            cv::Point3f pt(inPtr[i*ic], inPtr[i*ic + 1], inPtr[i*ic + 2]);
+            pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
+            ptr[oi*4] = pt.x;
+            ptr[oi*4 + 1] = pt.y;
+            ptr[oi*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,kptsSize));
+                if(kpts3D)
+                    kpts3D->push_back(org);
+                
+                *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+                ++oi;
+            }
+
+            //confidence
+            //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
+
+        }
+        return LaserScan::backwardCompatibility(scanData.colRange(0, oi), 0, 10, rtabmap::Transform::getIdentity());
+    }
+    return LaserScan();
+}
+
+} /* namespace rtabmap */
@@ -0,0 +1,163 @@
+/*
+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:
+    static LaserScan scanFromPointCloudData(
+            const cv::Mat & 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,
+            int kptsSize = 3);
+
+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;}
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setGPS(const GPS & gps);
+	void addEnvSensor(int type, float value);
+	void setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
+
+	void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
+    
+    GLuint getTextureId() {return textureId_;}
+    bool uvsInitialized() const {return uvs_initialized_;}
+    const float* uvsTransformed() const {return transformed_uvs_;}
+    void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+    ScreenRotation getScreenRotation() const {return colorCameraToDisplayRotation_;}
+    
+    void setOcclusionImage(const cv::Mat & image, const CameraModel & model) {occlusionModel_ = model; occlusionImage_ = image;}
+    const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
+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_;
+    
+    GLuint textureId_;
+    glm::mat4 viewMatrix_;
+    glm::mat4 projectionMatrix_;
+    float transformed_uvs_[8];
+    bool uvs_initialized_ = false;
+
+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_;
+    
+    cv::Mat occlusionImage_;
+    CameraModel occlusionModel_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERATANGO_H_ */
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"
 #include <tango_client_api.h>
 #include <tango_support_api.h>
+#include "tango-gl/camera.h"

 namespace rtabmap {

@@ -40,18 +41,33 @@ namespace rtabmap {
 const int kVersionStringLength = 128;
 const int holeSize = 5;
 const float maxDepthError = 0.10;
-const int scanDownsampling = 10;
+const int scanDownsampling = 1;
+
+//android phone
+//11 10 01 00 // portrait
+//01 11 00 10 // left
+//10 00 11 01 // right
+//00 01 10 11 // down
+
+const float kTextureCoords0[] = {1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
+const float kTextureCoords90[] = {0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0};
+const float kTextureCoords180[] = {0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0};
+const float kTextureCoords270[] = {1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0};

 // 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(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)
 	{
@@ -65,6 +81,10 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
 	{
 		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);
@@ -81,7 +101,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,14 +114,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),
-		stampEpochOffset_(0.0),
+		colorCamera_(colorCamera),
 		decimation_(decimation),
-		autoExposure_(autoExposure),
-		cloudStamp_(0),
+		rawScanPublished_(publishRawScan),
 		tangoColorType_(0),
 		tangoColorStamp_(0)
 {
@@ -113,11 +131,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");
@@ -137,38 +214,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.
@@ -233,7 +287,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);
@@ -282,7 +336,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)
 	{
@@ -297,31 +351,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;
 }

@@ -331,14 +427,19 @@ 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() && !cloud.empty())
+	if(!cloud.empty())
 	{
 		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);

@@ -355,20 +456,248 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 		//  So, synchronize with the last RGB frame before the Depth is acquired
 		if(!tangoColor_.empty())
 		{
+			UTimer timer;
 			double dt = fabs(timestamp - tangoColorStamp_);

 			//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);

 			if(dt >= 0.0 && dt < 0.5)
 			{
-				bool notify = cloud_.empty();
-				cloud_ = cloud.clone();
-				cloudStamp_ = timestamp;
+				bool notify = !data_.isValid();
+
+				cv::Mat tangoImage = tangoColor_;
+				cv::Mat rgb;
+				double cloudStamp = timestamp;
+				double rgbStamp = tangoColorStamp_;
+				int tangoColorType = tangoColorType_;
+
+				tangoColor_ = cv::Mat();
+				tangoColorStamp_ = 0.0;
+				tangoColorType_ = 0;
+
+				LOGD("tangoColorType=%d", tangoColorType);
+				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_RGBA2BGR);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_YV12);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_NV21);
+				}
+				else if(tangoColorType == 35)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
+				}
+				else
+				{
+					LOGE("Not supported color format : %d.", tangoColorType);
+					data_ = SensorData();
+					return;
+				}
+
+				//for(int i=0; i<rgb.cols; ++i)
+				//{
+				//	UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
+				//}
+
+				CameraModel model = model_;
+
+				if(colorCamera_)
+				{
+					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
+				// timestamp.
+				cv::Mat depth;
+
+				// Calculate the relative pose from color camera frame at timestamp
+				// color_timestamp t1 and depth
+				// camera frame at depth_timestamp t0.
+				Transform colorToDepth;
+				TangoPoseData pose_color_image_t1_T_depth_image_t0;
+				if (TangoSupport_calculateRelativePose(
+						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)
+				{
+					colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
+				}
+				else
+				{
+					LOGE(
+						"SynchronizationApplication: Could not find a valid relative pose at "
+						"time for color and "
+						" depth cameras.");
+				}
+
+				if(colorToDepth.getNormSquared() > 100000)
+				{
+					LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
+					colorToDepth.setNull();
+				}
+				cv::Mat scan;
+				if(!colorToDepth.isNull())
+				{
+					// The Color Camera frame at timestamp t0 with respect to Depth
+					// Camera frame at timestamp t1.
+					//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+					LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
+
+					int pixelsSet = 0;
+					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)
+					{
+						const 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 && rawScanPublished_)
+						{
+							scanData.at(oi++) = pt;
+						}
+
+						int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+						// get the coordinate on image plane.
+						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_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_l, pixel_x_l);
+							if(depthPixel == 0 || depthPixel > depth_value)
+							{
+								depthPixel = depth_value;
+								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();
+					}
+					//LOGD("pixels depth set= %d", pixelsSet);
+				}
+				else
+				{
+					LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
+				}
+
+				if(!rgb.empty() && !depth.empty())
+				{
+					depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
+
+					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());
+
+					// adjust origin
+					if(!getOriginOffset().isNull())
+					{
+						odom = getOriginOffset() * odom;
+					}
+
+					// occlusion depth
+					if(!depth.empty())
+					{
+						rtabmap::CameraModel depthModel = model.scaled(float(depth.cols) / float(model.imageWidth()));
+						depthModel.setLocalTransform(odom*model.localTransform());
+						this->setOcclusionImage(depth, depthModel);
+					}
+
+					//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
+					//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
+
+					Transform scanLocalTransform = model.localTransform();
+
+					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);
+					}
+					data_.setGroundTruth(odom);
+				}
+				else
+				{
+					LOGE("Could not get depth and rgb images!?!");
+					data_ = SensorData();
+					return;
+				}
+
 				if(notify)
 				{
-					//LOGD("Cloud: Release semaphore");
 					dataReady_.release();
 				}
+				LOGD("process cloud received %fs", timer.ticks());
 			}
 		}
 	}
@@ -376,7 +705,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)

 void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
 {
-	if(this->isRunning() && !tangoImage.empty())
+	if(!tangoImage.empty())
 	{
 		//LOGD("RGB received! %fs", timestamp);

@@ -388,27 +717,9 @@ 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)
-{
-	if(!pose.isNull() && pose.getNormSquared() < 100000)
-	{
-		// send pose of the camera (without optical rotation), not the device
-		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotationTango));
-	}
-}
-
 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
@@ -458,8 +769,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;
@@ -469,228 +779,116 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 {
 	//LOGI("Capturing image...");

-	SensorData data;
-	if(!dataReady_.acquire(1, 2000))
+	if(textureId_ == 0)
 	{
-		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."));
-
-			boost::mutex::scoped_lock  lock(dataMutex_);
-			if(!cloud_.empty() && !tangoColor_.empty())
-			{
-				UERROR("cloud and image were set!?");
-			}
-		}
-		cloud_ = cv::Mat();
-		cloudStamp_ = 0.0;
-		tangoColor_ = cv::Mat();
-		tangoColorStamp_ = 0.0;
-		tangoColorType_ = 0;
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 	}
-	else
+
+	// Update Texture (optional, just for first-view rendering)
+	if(colorCamera_ && textureId_)
 	{
-		cv::Mat cloud;
-		cv::Mat tangoImage;
-		cv::Mat rgb;
-		double cloudStamp = 0.0;
-		double rgbStamp = 0.0;
-		int tangoColorType = 0;
+		double video_overlay_timestamp;
+		TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);

+		if (status == TANGO_SUCCESS)
 		{
-			boost::mutex::scoped_lock  lock(dataMutex_);
-			cloud = cloud_;
-			cloudStamp = cloudStamp_;
-			cloud_ = cv::Mat();
-			cloudStamp_ = 0.0;
-			tangoImage = tangoColor_;
-			rgbStamp = tangoColorStamp_;
-			tangoColorType = tangoColorType_;
-			tangoColor_ = cv::Mat();
-			tangoColorStamp_ = 0.0;
-			tangoColorType_ = 0;
-		}
-
-		if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
-		}
-		else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
-		}
-		else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
-		}
-		else
-		{
-			LOGE("Not supported color format : %d.", tangoColorType);
-			return data;
-		}
-
-		//for(int i=0; i<rgb.cols; ++i)
-		//{
-		//	UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
-		//}
-
-		CameraModel model = model_;
-		if(decimation_ > 1)
-		{
-			rgb = util2d::decimate(rgb, decimation_);
-			model = model.scaled(1.0/double(decimation_));
-		}
-
-		// Querying the depth image's frame transformation based on the depth image's
-		// timestamp.
-		cv::Mat depth;
-
-		// Calculate the relative pose from color camera frame at timestamp
-		// color_timestamp t1 and depth
-		// camera frame at depth_timestamp t0.
-		Transform colorToDepth;
-		TangoPoseData pose_color_image_t1_T_depth_image_t0;
-		if (TangoSupport_calculateRelativePose(
-				rgbStamp, TANGO_COORDINATE_FRAME_CAMERA_COLOR, cloudStamp,
-			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
-			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
-		{
-			colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
-		}
-		else
-		{
-			LOGE(
-				"SynchronizationApplication: Could not find a valid relative pose at "
-				"time for color and "
-				" depth cameras.");
-		}
-
-		if(colorToDepth.getNormSquared() > 100000)
-		{
-			LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
-			colorToDepth.setNull();
-		}
-		cv::Mat scan;
-		if(!colorToDepth.isNull())
-		{
-			// The Color Camera frame at timestamp t0 with respect to Depth
-			// Camera frame at timestamp t1.
-			//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
-
-			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 oi=0;
-			for(unsigned int i=0; i<cloud.total(); ++i)
+			if(info)
 			{
-				float * p = cloud.ptr<float>(0,i);
-				cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
+				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
+			}

-				if(pt.z > 0.0f && i%scanDownsampling == 0)
-				{
-					scanData.at(oi++) = pt;
-				}
+			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
+			  if (rotation > 3) {
+				  rotation -= 4;
+			  }

-				int pixel_x, pixel_y;
-				// 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());
-				unsigned short depth_value(pt.z * 1000.0f);
+			TangoDoubleMatrixTransformData matrix_transform;
+			status = TangoSupport_getDoubleMatrixTransformAtTime(
+					video_overlay_timestamp,
+					TANGO_COORDINATE_FRAME_CAMERA_COLOR,
+					TANGO_COORDINATE_FRAME_START_OF_SERVICE,
+					TANGO_SUPPORT_ENGINE_OPENGL,
+					TANGO_SUPPORT_ENGINE_OPENGL,
+					static_cast<TangoSupportRotation>(rotation),
+					&matrix_transform);
+			if (matrix_transform.status_code == TANGO_POSE_VALID)
+			{
+				// Get projection matrix
+				TangoCameraIntrinsics color_camera_intrinsics;
+				int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
+						TANGO_CAMERA_COLOR,
+						static_cast<TangoSupportRotation>(rotation),
+						&color_camera_intrinsics);

-				if(pixel_x>=0 && pixel_x<depth.cols &&
-				   pixel_y>0 && pixel_y<depth.rows &&
-				   depth_value)
-				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y, pixel_x);
-					if(depthPixel == 0 || depthPixel > depth_value)
+				if (ret == TANGO_SUCCESS) {
+					float image_width = static_cast<float>(color_camera_intrinsics.width);
+					float image_height = static_cast<float>(color_camera_intrinsics.height);
+					float fx = static_cast<float>(color_camera_intrinsics.fx);
+					float fy = static_cast<float>(color_camera_intrinsics.fy);
+					float cx = static_cast<float>(color_camera_intrinsics.cx);
+					float cy = static_cast<float>(color_camera_intrinsics.cy);
+
+					viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
+					if(!getOriginOffset().isNull())
 					{
-						depthPixel = depth_value;
-						pixelsSet += 1;
+						viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
 					}
+
+					projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
+							image_width, image_height, fx, fy, cx, cy, 0.3, 50);
+
+					switch(rotation)
+					{
+					case ROTATION_90:
+						memcpy(transformed_uvs_, kTextureCoords90, 8*sizeof(float));
+						break;
+					case ROTATION_180:
+						memcpy(transformed_uvs_, kTextureCoords180, 8*sizeof(float));
+						break;
+					case ROTATION_270:
+						memcpy(transformed_uvs_, kTextureCoords270, 8*sizeof(float));
+						break;
+					case ROTATION_0:
+					default:
+						memcpy(transformed_uvs_, kTextureCoords0, 8*sizeof(float));
+					}
+					uvs_initialized_ = true;
+				}
+				else
+				{
+					UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
 				}
 			}
-
-			if(oi)
+			else
 			{
-				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
+				UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
 			}
-			//LOGD("pixels depth set= %d", pixelsSet);
 		}
 		else
 		{
-			LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
+			UERROR("TangoService_updateTextureExternalOes failed!");
 		}
+	}

-		if(!rgb.empty() && !depth.empty())
+	SensorData data;
+	if(dataReady_.acquireTry(1))
+	{
+		boost::mutex::scoped_lock  lock(dataMutex_);
+		data = data_;
+		data_ = SensorData();
+		if(info)
 		{
-			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
-
-			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
-
-			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
-			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
-			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
-
-			//Rotate in RTAB-Map's coordinate
-			Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
-
-			//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);
-		}
-		else
-		{
-			LOGE("Could not get depth and rgb images!?!");
+			info->odomPose = data.groundTruth();
+			data.setGroundTruth(Transform());
 		}
 	}
 	return data;

 }

-void CameraTango::mainLoopBegin()
-{
-	double t = cameraStartedTime_.elapsed();
-	if(t < 5.0)
-	{
-		uSleep((5.0-t)*1000); // just to make sure that the camera is started
-	}
-}
-
-void CameraTango::mainLoop()
-{
-	if(tango_config_)
-	{
-		SensorData data = this->captureImage();
-
-		if(!data.groundTruth().isNull())
-		{
-			rtabmap::Transform pose = data.groundTruth();
-			data.setGroundTruth(Transform());
-			// convert stamp to epoch
-			if(firstFrame_)
-			{
-				stampEpochOffset_ = UTimer::now()-data.stamp();
-			}
-			data.setStamp(stampEpochOffset_ + data.stamp());
-			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
-			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
-			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 */
@@ -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,25 +66,19 @@ protected:
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);

-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-
 private:
 	void * tango_config_;
-	bool firstFrame_;
-	UTimer cameraStartedTime_;
-	double stampEpochOffset_;
+	bool colorCamera_;
 	int decimation_;
-	bool autoExposure_;
-	cv::Mat cloud_;
-	double cloudStamp_;
+	bool rawScanPublished_;
+	SensorData data_;
 	cv::Mat tangoColor_;
 	int tangoColorType_;
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	CameraModel model_;
-	Transform deviceTColorCamera_;
+	cv::Mat fisheyeRectifyMapX_;
+	cv::Mat fisheyeRectifyMapY_;
 };

 } /* namespace rtabmap */
@@ -0,0 +1,155 @@
+/*
+ * 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>
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+
+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)
+#ifdef __ANDROID__
+    , jvm_(0), rtabmap_(0)
+#else
+    , swiftClassPtr_(0)
+#endif
+	{
+		registerToEventsManager();
+	}
+
+#ifdef __ANDROID__
+	void setJavaObjects(JavaVM * jvm, jobject rtabmap)
+	{
+		jvm_ = jvm;
+		rtabmap_ = rtabmap;
+	}
+#else
+    void setSwiftCallback(void * classPtr, void(*callback)(void *, int, int))
+    {
+        swiftClassPtr_ = classPtr;
+        swiftCallback = callback;
+    }
+#endif
+
+	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;
+#ifdef __ANDROID__
+			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();
+			}
+#else // APPLE
+            if(swiftClassPtr_)
+            {
+                std::function<void()> actualCallback = [&](){
+                    swiftCallback(swiftClassPtr_, count_, max_);
+                };
+                actualCallback();
+                success = true;
+            }
+#endif
+			if(!success)
+			{
+				UERROR("Failed to call rtabmap::updateProgressionCallback");
+			}
+		}
+		return false;
+	}
+
+private:
+	int count_;
+	int max_;
+#ifdef __ANDROID__
+	JavaVM *jvm_;
+	jobject rtabmap_;
+#else
+    void * swiftClassPtr_;
+    void(*swiftCallback)(void *, int, int);
+#endif
+};
+
+}
+
+
+#endif /* APP_ANDROID_JNI_PROGRESSIONSTATUS_H_ */
@@ -28,15 +28,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTABMAP_APP_H_
 #define RTABMAP_APP_H_

+#ifdef __ANDROID__
 #include <jni.h>
+#endif
 #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>
@@ -44,42 +46,43 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_base.h>
 #include <pcl/TextureMesh.h>

+
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
 public:
  // Constructor and deconstructor.
+#ifdef __ANDROID__
+  RTABMapApp(JNIEnv* env, jobject caller_activity);
+#else // __APPLE__
  RTABMapApp();
+  void setupSwiftCallbacks(void * classPtr,
+                           void(*progressCallback)(void *, int, int),
+                           void(*initCallback)(void *, int, const char*),
+                           void(*statsUpdatedCallback)(void *,
+                                                    int, int, int, int,
+                                                    float,
+                                                    int, int, int, int, int ,int,
+                                                    float,
+                                                    int,
+                                                    float,
+                                                    int,
+                                                    float, float, float, float,
+                                                    int, int,
+                                                    float, float, float, float, float, float));
+    
+#endif
  ~RTABMapApp();

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

-  void openDatabase(const std::string & databasePath = "");
-
-  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);
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);

+  bool isBuiltWith(int cameraDriver) const;
+#ifdef __ANDROID__
+  bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
+#else // __APPLE__
+    bool startCamera();
+#endif
  // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
  void InitializeGLContent();

@@ -89,8 +92,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.
  //
@@ -111,116 +113,201 @@ 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 setDepthFromMotion(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 setMeshDecimationFactor(float value);
  void setMeshTriangleSize(int value);
-  void setMinClusterSize(int value);
+  void setClusterRatio(float value);
  void setMaxGainRadius(float value);
+  void setRenderingTextureDecimation(int value);
+  void setBackgroundColor(float gray);
+  void setDepthConfidence(int value);
  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);
-  cv::Mat mergeTextures(pcl::TextureMesh & mesh, int textureSize) const;
+  bool recover(const std::string & from, const std::string & to);
+  void cancelProcessing();
  bool exportMesh(
-		  const std::string & filePath,
 		  float cloudVoxelSize,
+		  bool regenerateCloud,
 		  bool meshing,
 		  int textureSize,
+		  int textureCount,
 		  int normalK,
 		  bool optimized,
 		  float optimizedVoxelSize,
 		  int optimizedDepth,
-		  float optimizedDecimationFactor,
+		  int optimizedMaxPolygons,
 		  float optimizedColorRadius,
 		  bool optimizedCleanWhitePolygons,
-		  bool optimizedColorWhitePolygons,
+		  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, double stamp);
+
+  void postOdometryEvent(
+		rtabmap::Transform pose,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+		const rtabmap::Transform & rgbFrame,
+		const rtabmap::Transform & depthFrame,
+		double stamp,
+		double depthStamp,
+        const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+        const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+        const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
+        const float * points, int pointsLen, int pointsChannels,
+		const rtabmap::Transform & viewMatrix, //view matrix
+        float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+        float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
+
 protected:
-  virtual void handleEvent(UEvent * event);
+  virtual bool handleEvent(UEvent * event);

 private:
+  int updateMeshDecimation(int width, int height);
  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 depthFromMotion_;
+  bool cameraColor_;
  bool fullResolution_;
  bool appendMode_;
  float maxCloudDepth_;
-  int meshDecimation_;
+  float minCloudDepth_;
+  int cloudDensityLevel_;
  int meshTrianglePix_;
  float meshAngleToleranceDeg_;
-  int minClusterSize_;
+  float meshDecimationFactor_;
+  float clusterRatio_;
  float maxGainRadius_;
+  int renderingTextureDecimation_;
+  float backgroundColor_;
+  int depthConfidence_;

  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 totalPoints_;
  int totalPolygons_;
  int lastDrawnCloudsCount_;
  float renderingTime_;
+  double lastPostRenderEventTime_;
+  double lastPoseEventTime_;
+  std::map<std::string, float> bufferedStatsData_;

  bool visualizingMesh_;
  bool exportedMeshUpdated_;
-  pcl::TextureMesh::Ptr exportedMesh_;
-  cv::Mat exportedTexture_;
+  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_;
+    
+    UTimer fpsTime_;

-	std::list<rtabmap::Statistics> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
+	std::map<double, rtabmap::Transform> poseBuffer_;

+	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_;
+    
+#ifndef __ANDROID__
+    void * swiftClassPtr_;
+    void(*swiftInitCallback)(void *, int, const char *);
+    void(*swiftStatsUpdatedCallback)(void *,
+                             int, int, int, int,
+                             float,
+                             int, int, int, int, int ,int,
+                             float,
+                             int,
+                             float,
+                             int,
+                             float, float, float, float,
+                             int, int,
+                             float, float, float, float, float, float);
+    
+#endif
 };

 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_
@@ -0,0 +1,210 @@
+/*
+ * 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.
+ */
+
+// This modules handles drawing the passthrough camera image into the OpenGL
+// scene.
+
+#include "background_renderer.h"
+
+#include <type_traits>
+
+namespace {
+
+const std::string kVertexShader =
+		"attribute vec4 a_Position;\n"
+		"attribute vec2 a_TexCoord;\n"
+
+		"varying vec2 v_TexCoord;\n"
+
+		"void main() {\n"
+		"   gl_Position = a_Position;\n"
+		"   v_TexCoord = a_TexCoord;\n"
+		"}\n";
+
+const std::string kFragmentShaderOES =
+	"#extension GL_OES_EGL_image_external : require\n"
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+	"uniform samplerExternalOES sTexture;\n"
+    "uniform bool uRedUnknown;\n"
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "    gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+	"}\n";
+
+const std::string kFragmentShaderBlendingOES =
+    "#extension GL_OES_EGL_image_external : require\n"
+    "precision mediump float;\n"
+    "varying vec2 v_TexCoord;\n"
+    "uniform samplerExternalOES sTexture;\n"
+	"uniform sampler2D uDepthTexture;\n"
+	"uniform vec2 uScreenScale;\n"
+    "uniform bool uRedUnknown;\n"
+    "void main() {\n"
+    "    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+    "    vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+    "    float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
+    "    if(depth > 0.0)\n"
+    "        gl_FragColor = vec4(sample.r, sample.g, sample.b, 0.5);\n"
+    "    else {\n"
+    "        float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "        gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+    "    }\n"
+    "}\n";
+
+const std::string kFragmentShader =
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+    "uniform sampler2D sTexture;\n"
+    "uniform bool uRedUnknown;\n"
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+	"    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+	"    gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+	"}\n";
+
+const std::string kFragmentShaderBlending =
+    "precision mediump float;\n"
+    "varying vec2 v_TexCoord;\n"
+    "uniform sampler2D sTexture;\n"
+    "uniform sampler2D uDepthTexture;\n"
+    "uniform vec2 uScreenScale;\n"
+    "uniform bool uRedUnknown;\n"
+    "void main() {\n"
+    "    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+    "    vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+    "    float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
+    "    if(depth > 0.0)\n"
+    "        gl_FragColor = vec4(sample.r, sample.g, sample.b, 0.5);\n"
+    "    else {\n"
+    "        float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "        gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+    "    }\n"
+    "}\n";
+
+/* To debug depth texture
+ const std::string kFragmentShader =
+     "precision mediump float;\n"
+     "varying vec2 v_TexCoord;\n"
+     "uniform sampler2D sTexture;\n"
+
+     "void main() {\n"
+     "  float uNearZ = 0.2;\n"
+     "  float uFarZ = 1000.0;\n"
+     "  float depth = texture2D(sTexture, v_TexCoord).r;\n"
+     "  float num =  (2.0 * uNearZ * uFarZ);\n"
+     "  float diff = (uFarZ - uNearZ);\n"
+     "  float add = (uFarZ + uNearZ);\n"
+     "  float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
+     "  float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
+     "  float grey = linearDepth/3.0;\n"
+     "  gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+     "}\n";
+ */
+
+}  // namespace
+
+std::vector<GLuint> BackgroundRenderer::shaderPrograms_;
+
+BackgroundRenderer::~BackgroundRenderer()
+{
+	for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
+	{
+		glDeleteShader(shaderPrograms_[i]);
+	}
+	shaderPrograms_.clear();
+}
+
+void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
+{
+	LOGI("textureId=%d", textureId);
+
+  texture_id_ = textureId;
+#ifdef __ANDROID__
+  oes_ = oes;
+#endif
+
+    if(shaderPrograms_.empty())
+    {
+        shaderPrograms_.resize(2,0);
+        shaderPrograms_[0] = tango_gl::util::CreateProgram(
+              kVertexShader.c_str(),
+              oes_?kFragmentShaderOES.c_str():kFragmentShader.c_str());
+        UASSERT(shaderPrograms_[0]!=0);
+        shaderPrograms_[1] = tango_gl::util::CreateProgram(
+              kVertexShader.c_str(),
+              oes_?kFragmentShaderBlendingOES.c_str():kFragmentShaderBlending.c_str());
+        UASSERT(shaderPrograms_[1]!=0);
+    }
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) {
+  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+
+  GLuint program = shaderPrograms_[depthTexture>0?1:0];
+    
+  glUseProgram(program);
+  glDepthMask(GL_FALSE);
+  glEnable (GL_BLEND);
+
+  glActiveTexture(GL_TEXTURE0);
+#ifdef __ANDROID__
+  if(oes_)
+    glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+  else
+#endif
+    glBindTexture(GL_TEXTURE_2D, texture_id_);
+    
+  if(depthTexture>0)
+  {
+    // Texture activate unit 1
+    glActiveTexture(GL_TEXTURE1);
+    // Bind the texture to this unit.
+    glBindTexture(GL_TEXTURE_2D, depthTexture);
+    // Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
+    GLuint depth_texture_handle = glGetUniformLocation(program, "uDepthTexture");
+    glUniform1i(depth_texture_handle, 1);
+      
+    GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
+    glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
+  }
+    
+  GLuint screenScale_handle = glGetUniformLocation(program, "uRedUnknown");
+  glUniform1i(screenScale_handle, redUnknown);
+
+  GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
+  GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
+
+  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
+
+  glEnableVertexAttribArray(attributeVertices);
+  glEnableVertexAttribArray(attributeUvs);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+
+  glDisableVertexAttribArray(attributeVertices);
+  glDisableVertexAttribArray(attributeUvs);
+
+  glUseProgram(0);
+  glDepthMask(GL_TRUE);
+  glDisable (GL_BLEND);
+  tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
+}
+
@@ -0,0 +1,70 @@
+/*
+ * 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.
+ */
+
+#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+
+#ifdef __ANDROID__
+#include <GLES2/gl2.h>
+#include <GLES2/gl2ext.h>
+#else // __APPLE__
+#include <OpenGLES/ES2/gl.h>
+#include <OpenGLES/ES2/glext.h>
+#endif
+#include <cstdlib>
+
+#include "util.h"
+
+static const GLfloat BackgroundRenderer_kVertices[] = {
+	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+};
+static const GLfloat BackgroundRenderer_kTexCoord[] = {
+   1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+};
+
+//android phone
+//11 10 01 00 // portrait
+//01 11 00 10 // left
+//10 00 11 01 // right
+//00 01 10 11 // down
+
+
+// This class renders the passthrough camera image into the OpenGL frame.
+class BackgroundRenderer {
+public:
+	// Positions of the quad vertices in clip space (X, Y).
+
+	static constexpr int kNumVertices = 4;
+
+ public:
+  BackgroundRenderer() = default;
+  ~BackgroundRenderer();
+
+  // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
+  // other methods below.
+  void InitializeGlContent(GLuint textureId, bool oes);
+
+  // Draws the background image.  This methods must be called for every ArFrame
+  // returned by ArSession_update() to catch display geometry change events.
+  void Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown);
+
+private:
+  static std::vector<GLuint> shaderPrograms_;
+  GLuint texture_id_;
+  bool oes_ = false;
+};
+
+#endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_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_
@@ -31,7 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "util.h"

+#ifdef __ANDROID__
 #include <GLES2/gl2.h>
+#else //__APPLE__
+#include <OpenGLES/ES2/gl.h>
+#endif

 GraphDrawable::GraphDrawable(
 		GLuint shaderProgram,
@@ -28,8 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef GRAPH_DRAWABLE_H_
 #define GRAPH_DRAWABLE_H_

+#ifdef __ANDROID__
 #include <jni.h>
-
+#endif
 #include <tango-gl/util.h>
 #include <vector>
 #include <pcl/point_cloud.h>
@@ -28,7 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
 #define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

+#ifdef __ANDROID__
 #include <jni.h>
+#endif

 #include <tango-gl/util.h>
 #include <vector>
@@ -40,29 +42,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;}
+  bool hasMesh() const {return index_buffers_[0] != 0;}
+  bool hasTexture() const {return texture_ != 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.
  //
@@ -70,28 +84,57 @@ 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,
+  void Render(
+		  const glm::mat4 & projectionMatrix,
 		  const glm::mat4 & viewMatrix,
 		  bool meshRendering = true,
 		  float pointSize = 3.0f,
 		  bool textureRendering = false,
-		  bool lighting = true);
+		  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<GLuint> polygons_;
+  GLuint vertex_buffer_;
+  GLuint texture_;
+  std::vector<GLuint> index_buffers_;
+  std::vector<int> index_buffers_count_;
  int nPoints_;
-  glm::mat4 pose_;
+  rtabmap::Transform pose_;
+  glm::mat4 poseGl_;
  bool visible_;
  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_
@@ -17,11 +17,13 @@
 #ifndef TANGO_POINT_CLOUD_SCENE_H_
 #define TANGO_POINT_CLOUD_SCENE_H_

+#ifdef __ANDROID__
 #include <jni.h>
+#endif
 #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>
@@ -35,8 +37,10 @@
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>

-#include <point_cloud_drawable.h>
-#include <graph_drawable.h>
+#include "point_cloud_drawable.h"
+#include "graph_drawable.h"
+#include "bounding_box_drawable.h"
+#include "background_renderer.h"

 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -56,6 +60,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

@@ -65,17 +74,18 @@ class Scene {
  //         frame's timestamp.
  // @param: point_cloud_vertices, point cloud's vertices of the current point
  //         frame.
-  int Render();
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh(), bool mapping=false);

  // Set render camera's viewing angle, first person, third person or top down.
  //
  // @param: camera_type, camera type includes first person, third person and
  //         top down
  void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+  tango_gl::GestureCamera::CameraType GetCameraType() const {return gesture_camera_->GetCameraType();}

-  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 +106,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,9 +121,9 @@ 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);
@@ -115,19 +132,32 @@ class Scene {
  bool hasTexture(int id) const;
  std::set<int> getAddedClouds() const;
  void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
-  void updateMesh(int id, const Mesh & mesh, 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 setFrustumCulling(bool enabled) {frustumCulling_ = enabled;}
+  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 isFrustumCulling() const {return frustumCulling_;}
  bool isLighting() const {return lighting_;}
+  bool isBackfaceCulling() const {return backfaceCulling_;}
+  bool isWireframe() const {return wireFrame_;}
+
+  BackgroundRenderer * background_renderer_;

 private:
  // Camera object that allows user to use touch input to interact with.
@@ -142,28 +172,47 @@ 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 frustumCulling_;
+  bool boundingBoxRendering_;
  bool lighting_;
+  bool backfaceCulling_;
+  bool wireFrame_;
+  float r_;
+  float g_;
+  float b_;
+  GLuint fboId_;
+  GLuint rboId_;
+  GLuint depthTexture_; // 0=objects+occlusion
+  GLsizei screenWidth_;
+  GLsizei screenHeight_;
+  bool doubleTapOn_;
+  cv::Point2f doubleTapPos_;
 };

 #endif  // TANGO_POINT_CLOUD_SCENE_H_
@@ -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.5f;
+  far_clip_plane_ = 50.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() {
 }

@@ -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();
@@ -43,9 +43,11 @@ const float kCamViewMaxDist = 100.f;

 // FOV set up values.
 // Third and top down camera's FOV is 65 degrees.
-// First person camera's FOV is 45 degrees.
-const float kHighFov = 65.0f;
+// First person camera's FOV is 85 degrees.
+const float kHighestFov = 120.0f;
+const float kHighFov = 85.0f;
 const float kLowFov = 65.0f;
+const float kLowestFov = 40.0f;
 }

 namespace tango_gl {
@@ -62,68 +64,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, kLowestFov, kHighestFov));
+			}
+			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,7 +181,9 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
  camera_type_ = camera_index;
  switch (camera_index) {
    case kFirstPerson:
-      SetFieldOfView(kLowFov);
+      SetOrthoMode(false);
+      SetFieldOfView(kLowestFov);
+      SetNearFarClipPlanes(0.25, 25);
      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
      cam_cur_dist_ = 0.0f;
@@ -177,20 +196,24 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
      break;
    case kThirdPerson:
    case kThirdPersonFollow:
-      SetFieldOfView(kHighFov);
+      SetOrthoMode(false);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
      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;
+      cam_cur_dist_ = camera_index==kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 4.0f;
-      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
+      cam_cur_angle_.x = -M_PI / 12.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.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(kLowFov);
+      SetNearFarClipPlanes(1, 50);
      cam_cur_dist_ = kTopDownCameraDist;
      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
      cam_cur_angle_.x = -M_PI / 2.0f;
@@ -198,6 +221,21 @@ 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);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
+	  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;
  }
@@ -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
@@ -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_
@@ -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_;

@@ -18,7 +18,10 @@
 #define TANGO_GL_TEXTURE_H_

 #include <errno.h>
+
+#ifdef __ANDROID__
 #include <png.h>
+#endif

 #include "tango-gl/util.h"

@@ -30,11 +33,15 @@ class Texture {
  Texture& operator=(const Texture&) = delete;
  ~Texture();

+#ifdef __ANDROID__
  bool LoadFromPNG(const char* file_path);
+#endif
  GLuint GetTextureID() const;

 private:
+#ifdef __ANDROID__
  png_uint_32 width_, height_;
+#endif
  int bit_depth_, color_type_;
  char* byte_data_;
  GLuint texture_id_;
@@ -21,10 +21,16 @@
 #define GL_VERTEX_PROGRAM_POINT_SIZE 0x8642

 #include <stdlib.h>
+#ifdef __ANDROID__
 #include <jni.h>
 #include <android/log.h>
 #include <GLES2/gl2.h>
 #include <GLES2/gl2ext.h>
+#else // __APPLE__
+#include <OpenGLES/ES2/gl.h>
+#include <OpenGLES/ES2/glext.h>
+#include <syslog.h>
+#endif

 #include "glm/glm.hpp"
 #include "glm/gtc/matrix_transform.hpp"
@@ -33,10 +39,28 @@
 #include "glm/gtx/matrix_decompose.hpp"

 #define LOG_TAG "rtabmap"
+#if defined(DISABLE_LOG)
+#define LOGD(...) ;
+#define LOGI(...) ;
+#define LOGW(...) ;
+#else
+#ifdef __APPLE__
+#define LOGD(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#define LOGI(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#define LOGW(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#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
+#endif
+#ifdef __APPLE__
+#define LOGE(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#define LOGF(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#else
 #define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
+#define LOGF(...) __android_log_print(ANDROID_LOG_FATAL,LOG_TAG,__VA_ARGS__)
+#endif

 #ifndef M_PI
 #define M_PI 3.1415926f
@@ -76,6 +100,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
@@ -57,7 +57,7 @@ Quad::Quad() {

 Quad::~Quad() { glDeleteShader(shader_program_); }

-void Quat::SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }
+void Quad::SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }

 void Quad::Render(const glm::mat4& projection_mat,
                  const glm::mat4& view_mat) const {
@@ -34,11 +34,13 @@ static int RoundUpPowerOfTwo(int w) {
 }

 Texture::Texture(const char* file_path) {
+#ifdef __ANDROID__
  if (!LoadFromPNG(file_path)) {
    LOGE("Texture initialing error");
  }
+#endif
 }
-
+#ifdef __ANDROID__
 bool Texture::LoadFromPNG(const char* file_path) {
  FILE* file = fopen(file_path, "rb");

@@ -94,7 +96,7 @@ bool Texture::LoadFromPNG(const char* file_path) {

  return true;
 }
-
+#endif
 GLuint Texture::GetTextureID() const { return texture_id_; }

 Texture::~Texture() {
@@ -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);
@@ -0,0 +1 @@
+ 
@@ -28,8 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef UTIL_H_
 #define UTIL_H_

-
-#include <android/log.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/core/CameraModel.h>
@@ -39,44 +37,60 @@ 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()
 	{
+#ifdef DISABLE_LOG
 		ULogger::setLevel(ULogger::kWarning);
 		ULogger::setEventLevel(ULogger::kWarning);
+#else
+        ULogger::setLevel(ULogger::kDebug);
+        ULogger::setEventLevel(ULogger::kDebug);
+#endif
 		ULogger::setPrintThreadId(true);

 		registerToEventsManager();
 	}
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
-		if(event->getClassName().compare("ULogEvent") == 0)
+        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 +101,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)
 {
@@ -150,19 +169,103 @@ public:
 		cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
 		normals(new pcl::PointCloud<pcl::Normal>),
 		indices(new std::vector<int>),
-		visible(true),
-		gain(1.0f)
-	{}
+		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;
-	std::vector<Eigen::Vector2f> texCoords;
+	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 orientation, 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_ */
@@ -0,0 +1 @@
+*.jar
@@ -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>
@@ -15,297 +15,156 @@
 -->
 <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" />
+        android:layout_gravity="top"/>
    
-   <LinearLayout
-        android:id="@+id/debug_layout"
+    <RelativeLayout 
        android:layout_width="wrap_content"
+        android:layout_height="match_parent"
+        android:fitsSystemWindows="true">
+        
+    <ImageButton
+        android:id="@+id/start_button"
+        android:layout_width="100dp"
        android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
-        android:layout_alignParentLeft="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>
-        
-        <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>
+        android:layout_alignParentBottom="true"
+        android:layout_centerHorizontal="true"
+        android:src="@drawable/start96"
+        android:background="@null"
+        android:layout_marginBottom="10dp" />

+    <ImageButton
+        android:id="@+id/stop_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        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: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/first_person_button"
-        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_above="@+id/backface_button"
+        android:layout_alignLeft="@+id/backface_button"
        android:layout_alignParentRight="true"
-        android:layout_marginBottom="15dp"
+        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/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:textOff="@string/first_person"
-        android:textOn="@string/first_person" />
    
    <ToggleButton
-        android:id="@+id/third_person_button"
+        android:id="@+id/backface_button"
        android:layout_width="100dp"
        android:layout_height="wrap_content"
-        android:layout_above="@+id/top_down_button"
+        android:layout_above="@+id/camera_button"
+        android:layout_alignRight="@+id/camera_button"
        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
+        android:layout_marginBottom="15dp"
        android:layout_marginRight="5dp"
        android:paddingRight="5dp"
-        android:textOff="@string/third_person"
-        android:textOn="@string/third_person" />
-
-    <ToggleButton
-        android:id="@+id/top_down_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
+        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:textOff="@string/top_down"
-        android:textOn="@string/top_down" />
+        android:paddingBottom="10dp"
+        android:text="@string/camera_button"
+        android:spinnerMode="dropdown"/>

-    <ToggleButton
-        android:id="@+id/pause_button"
-        android:layout_width="100dp"
+
+    <Button
+        android:id="@+id/button_shareToSketchfab"
+        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
-        android:layout_alignLeft="@+id/first_person_button"
-        android:layout_alignParentTop="true"
-        android:layout_marginTop="61dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/pause"
-        android:textOn="@string/resume" />
-    
+        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_alignParentBottom="true"
-        android:layout_marginLeft="5dp"
-        android:paddingLeft="5dp"
-        android:text="@string/close_visualization"/>
-        
+        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:layout_marginTop="20dp"
+        android:text="@string/new_scan" />
+
+</RelativeLayout>        
 </RelativeLayout>
@@ -1,14 +1,22 @@
 <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_decimation"
-    		android:title="@string/pref_title_decimation"
-    		android:summary="@string/pref_summary_decimation"
-   			android:entries="@array/pref_decimation_keys"
-   			android:entryValues="@array/pref_decimation_values"
-   			android:defaultValue="@string/pref_default_decimation"/>
+            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"
@@ -16,6 +24,13 @@
   			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"
@@ -37,167 +52,308 @@
   			android:entries="@array/pref_triangle_keys"
   			android:entryValues="@array/pref_triangle_values"
   			android:defaultValue="@string/pref_default_triangle"/>
-        <SwitchPreference
+        <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_mapping">
-    <PreferenceScreen
-            android:key="pref_button_mapping"
-            android:title="@string/pref_title_mapping"
-            android:summary="@string/pref_summary_mapping"
-            android:persistent="false">
-        
-	    <SwitchPreference
-            android:key="@string/pref_key_append"
-            android:title="@string/pref_title_append"
-            android:summary="@string/pref_summary_append"
-            android:defaultValue="@string/pref_default_append"/>
-	    <SwitchPreference
-            android:key="@string/pref_key_drift_correction"
-            android:title="@string/pref_title_drift_correction"
-            android:summary="@string/pref_summary_drift_correction"
-            android:defaultValue="@string/pref_default_drift_correction"/>
-	    <SwitchPreference
-            android:key="@string/pref_key_auto_exposure"
-            android:title="@string/pref_title_auto_exposure"
-            android:summary="@string/pref_summary_auto_exposure"
-            android:defaultValue="@string/pref_default_auto_exposure"/>
-	    <SwitchPreference
-            android:key="@string/pref_key_resolution"
-            android:title="@string/pref_title_resolution"
-            android:summary="@string/pref_summary_resolution"
-            android:defaultValue="@string/pref_default_resolution"/>
-  
-        <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_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_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_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"
-    		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"/>
-    </PreferenceScreen>
-    </PreferenceCategory>
-    
-    <PreferenceCategory
-        android:title="@string/pref_title_export">
-    <PreferenceScreen
-            android:key="pref_button_export"
-            android:title="@string/pref_title_export"
-            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_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"/>
-        
-	    <SwitchPreference
-            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_values"
-	   			android:entryValues="@array/pref_opt_depth_values"
-	   			android:defaultValue="@string/pref_default_opt_depth"/>
-	        
-	        <ListPreference 
-	            android:key="@string/pref_key_opt_decimation_factor"
-	    		android:title="@string/pref_title_opt_decimation_factor"
-	    		android:summary="@string/pref_summary_opt_decimation_factor"
-	   			android:entries="@array/pref_opt_decimation_factor_keys"
-	   			android:entryValues="@array/pref_opt_decimation_factor_values"
-	   			android:defaultValue="@string/pref_default_opt_decimation_factor"/>
-	        
-	        <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"/>
-	        
-	        <SwitchPreference
-	            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"/>
-	        
-	       </PreferenceCategory>
- 	</PreferenceScreen>
- 	</PreferenceCategory>
 	 	
    <PreferenceCategory
        android:title="@string/pref_title_general">
        
-        <ListPreference 
+        <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_depth_from_motion"
+                android:title="@string/pref_title_depth_from_motion"
+                android:summary="@string/pref_summary_depth_from_motion"
+                android:defaultValue="@string/pref_default_depth_from_motion"/>
+             <ListPreference
+		        android:key="@string/pref_key_arcore_localization_filtering_speed"
+		        android:title="@string/pref_title_arcore_localization_filtering_speed"
+		        android:summary="@string/pref_summary_arcore_localization_filtering_speed"
+		        android:entries="@array/pref_arcore_localization_filtering_speed_keys"
+		   	    android:entryValues="@array/pref_arcore_localization_filtering_speed_values"
+		   		android:defaultValue="@string/pref_default_arcore_localization_filtering_speed"/>
+		    <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"
@@ -205,13 +361,26 @@
   			android:entryValues="@array/pref_gain_max_radius_values"
   			android:defaultValue="@string/pref_default_gain_max_radius"/>
        <ListPreference 
-            android:key="@string/pref_key_min_cluster_size"
-    		android:title="@string/pref_title_min_cluster_size"
-    		android:summary="@string/pref_summary_min_cluster_size"
-   			android:entries="@array/pref_min_cluster_size_values"
-   			android:entryValues="@array/pref_min_cluster_size_values"
-   			android:defaultValue="@string/pref_default_min_cluster_size"/>
-        
+            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>
@@ -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>
@@ -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>
@@ -7,15 +7,18 @@
 		  <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/save" android:title="Save" android:showAsAction="ifRoom"/>
-       <item android:id="@+id/post_processing" android:title="Optimize" android:showAsAction="ifRoom">
+	   
+       <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/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" />
@@ -25,29 +28,28 @@
 	         </item>
          </menu>
       </item>
-       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
+       
+       <item android:id="@+id/export" android:title="Assemble">
          <menu>
-              <item android:id="@+id/export_point_cloud" android:title="Point Cloud (*.ply)" />
-              <item android:id="@+id/export_mesh_menu" android:title="Raw Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_mesh" android:title="Colored Mesh (*.ply)" />
-			          <item android:id="@+id/export_mesh_texture" android:title="Textured Mesh (*.obj)" />
-		          </menu>
-	          </item>
+              <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 (*.ply)" />
-			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh (*.obj)" />
+		              <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/open" android:title="Open" android:showAsAction="ifRoom"/>
-       
       <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/map_shown" android:checked="true" android:title="Map Visible" />
            <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
@@ -67,7 +69,7 @@
 	       </menu>
 	   </item>
 	   <item android:id="@+id/settings" android:title="Settings"/>
-       <item android:id="@+id/reset" android:title="Reset"/>
+       <item android:id="@+id/resume" android:title="Resume"/>
       <item android:id="@+id/about" android:title="About"/>
    </group>
 </menu>
@@ -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>
@@ -0,0 +1,3 @@
+<resources>
+    <dimen name="image_width">150dp</dimen>
+</resources>
@@ -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>
@@ -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>
@@ -0,0 +1,882 @@
+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.Anchor;
+import com.google.ar.core.Camera;
+import com.google.ar.core.CameraConfig;
+import com.google.ar.core.CameraConfigFilter;
+import com.google.ar.core.CameraIntrinsics;
+import com.google.ar.core.Config;
+import com.google.ar.core.Coordinates2d;
+import com.google.ar.core.Frame;
+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.CameraMetadata;
+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;
+import android.widget.Toast;
+
+public class ARCoreSharedCamera {
+
+	public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
+
+	private static final float[] QUAD_COORDS =
+      new float[] {
+        -1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+      };
+	
+	private static RTABMapActivity mActivity;
+	public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSpeed) {
+		mActivity = c;
+		mARCoreLocalizationFilteringSpeed = arCoreLocalizationFilteringSpeed;
+	}
+
+	// Depth TOF Image.
+	// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
+	private int depthWidth = 640;
+	private int depthHeight = 480;
+
+	// GL Surface used to draw camera preview image.
+	public GLSurfaceView surfaceView;
+
+	// ARCore session that supports camera sharing.
+	private Session sharedSession;
+	
+	private Pose previousAnchorPose = null;
+	private long previousAnchorTimeStamp;
+	private Pose arCoreCorrection = Pose.IDENTITY;
+	private Pose odomPose = Pose.IDENTITY;
+	private float mARCoreLocalizationFilteringSpeed = 1.0f;
+
+	// 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;
+	
+	private Toast mToast = null;
+
+	// Camera ID for the camera used by ARCore.
+	private String cameraId;
+	private String depthCameraId;
+	
+	private Pose rgbExtrinsics;
+	private Pose depthExtrinsics;
+	private float[] depthIntrinsics = null;
+	
+	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;}
+
+	public void setToast(Toast toast)
+	{
+		mToast = toast;
+	}
+	
+	// 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 && cameraId.compareTo(depthCameraId) == 0) 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();
+	}
+
+    /**
+     * Return true if the given array contains the given integer.
+     *
+     * @param modes array to check.
+     * @param mode  integer to get for.
+     * @return true if the array contains the given integer, otherwise false.
+     */
+    private static boolean contains(int[] modes, int mode) {
+        if (modes == null) {
+            return false;
+        }
+        for (int i : modes) {
+            if (i == mode) {
+                return true;
+            }
+        }
+        return false;
+    }
+    
+	// Perform various checks, then open camera device and create CPU image reader.
+	public boolean openCamera() {
+
+		close();
+		
+		startBackgroundThread();
+
+		if(cameraTextureId == -1)
+		{
+			int[] textures = new int[1];
+			GLES20.glGenTextures(1, textures, 0);
+			cameraTextureId = textures[0];
+		}
+
+		Log.v(TAG, "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;
+			}
+			
+			// First obtain the session handle before getting the list of various camera configs.
+			// Create filter here with desired fps filters.
+			CameraConfigFilter cameraConfigFilter = new CameraConfigFilter(sharedSession);
+			CameraConfig[] cameraConfigs = sharedSession.getSupportedCameraConfigs(cameraConfigFilter).toArray(new CameraConfig[0]);
+			Log.i(TAG, "Size of supported CameraConfigs list is " + cameraConfigs.length);
+
+			// Determine the highest and lowest CPU resolutions.
+			int highestResolutionIndex=-1;
+			int highestResolution = 0;
+			for(int i=0; i<cameraConfigs.length; ++i)
+			{
+				Log.i(TAG, "Camera ID: " + cameraConfigs[i].getCameraId());
+				Log.i(TAG, "Resolution: " + cameraConfigs[i].getImageSize().getWidth() + "x" + cameraConfigs[i].getImageSize().getHeight());
+				if(highestResolution == 0 || highestResolution < cameraConfigs[i].getImageSize().getWidth())
+				{
+					highestResolutionIndex = i;
+					highestResolution = cameraConfigs[i].getImageSize().getWidth();
+				}
+			}
+			if(highestResolutionIndex>=0)
+			{
+				//Log.i(TAG, "Setting camera resolution to " + cameraConfigs[highestResolutionIndex].getImageSize().getWidth() + "x" + cameraConfigs[highestResolutionIndex].getImageSize().getHeight());
+				//FIXME: Can we make it work to use HD rgb images? To avoid this error "CaptureRequest contains unconfigured Input/Output Surface!"
+				//sharedSession.setCameraConfig(cameraConfigs[highestResolutionIndex]);
+			}
+
+			// Enable auto focus mode while ARCore is running.
+			Config config = sharedSession.getConfig();
+			config.setFocusMode(Config.FocusMode.FIXED);
+			config.setUpdateMode(Config.UpdateMode.BLOCKING);
+			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
+			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
+			config.setCloudAnchorMode(Config.CloudAnchorMode.DISABLED);
+			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();
+
+		Log.d(TAG, "Shared camera ID: " + cameraId);
+
+		mTOFAvailable = false;
+
+		// Store a reference to the camera system service.
+		cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
+		
+		depthCameraId = null;
+		// show all cameras
+		try {
+            // Find a CameraDevice that supports DEPTH16 captures, and configure state.
+            for (String tmpCameraId : cameraManager.getCameraIdList()) {
+                CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(tmpCameraId);
+
+                Log.i(TAG, "Camera " + tmpCameraId + " extrinsics:");
+                
+            	float[] translation = characteristics.get(CameraCharacteristics.LENS_POSE_TRANSLATION);
+                if(translation != null)
+                {
+                	 Log.i(TAG, String.format("Translation (x,y,z): %f,%f,%f", translation[0], translation[1], translation[2]));
+                }
+                float[] rotation = characteristics.get(CameraCharacteristics.LENS_POSE_ROTATION);
+                if(rotation != null)
+                {
+                	 Log.i(TAG, String.format("Rotation (qx,qy,qz,qw): %f,%f,%f,%f",  rotation[0], rotation[1], rotation[2], rotation[3]));
+                }
+                
+                if(tmpCameraId.compareTo(cameraId)==0 && translation!=null && rotation!=null)
+                {
+                	rgbExtrinsics = new Pose(translation, rotation);
+                	Log.i(TAG,"Set rgb extrinsics!");
+                }
+                
+                if (!contains(characteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES_DEPTH_OUTPUT) ||
+                		characteristics.get(CameraCharacteristics.LENS_FACING) == CameraMetadata.LENS_FACING_FRONT) {
+                    continue;
+                }
+                Log.i(TAG, "Camera " + tmpCameraId + " has depth output available");
+                
+                depthCameraId = tmpCameraId;
+                if(translation!=null && rotation != null)
+                {
+                	depthExtrinsics = new Pose(translation, rotation);
+                	Log.i(TAG,"Set depth extrinsics!");
+                }
+                depthIntrinsics = characteristics.get(CameraCharacteristics.LENS_INTRINSIC_CALIBRATION);
+                Log.i(TAG, String.format("Intrinsics (fx,fy,cx,cy,s): %f,%f,%f,%f,%f", 
+                		depthIntrinsics[0],
+                		depthIntrinsics[1],
+                		depthIntrinsics[2],
+                		depthIntrinsics[3],
+                		depthIntrinsics[4]));
+            }
+        } catch (CameraAccessException e) {
+            e.printStackTrace();
+        }
+		if(rgbExtrinsics == null)
+		{
+			float[] translation = {0,0,0};
+			float[] rotation = {0,0,0,1};
+			rgbExtrinsics = new Pose(translation, rotation);
+		}
+		if(depthExtrinsics == null)
+		{
+			depthExtrinsics = rgbExtrinsics;
+		}
+
+		if(depthCameraId != null)
+		{
+			ArrayList<String> resolutions = getResolutions(mActivity, depthCameraId, ImageFormat.DEPTH16);
+			if (resolutions != null) {
+				float[] newDepthIntrinsics = null;
+				int largestWidth = 0;
+				for( String temp : resolutions) {
+					Log.i(TAG, "DEPTH16 resolution: " + temp);
+					depthWidth = Integer.parseInt(temp.split("x")[0]);
+					depthHeight = Integer.parseInt(temp.split("x")[1]);
+					if(depthIntrinsics != null)
+					{
+						if(depthIntrinsics[0] != 0)
+						{
+							if(largestWidth == 0 && depthWidth>largestWidth)
+							{
+								largestWidth = depthWidth; // intrinsics should match this resolution
+							}
+							// Samsung Galaxy Note10+: take smallest resolution and match the intrinsics
+							if(depthWidth < largestWidth)
+							{
+								float scale = (float)depthWidth/(float)largestWidth;
+								newDepthIntrinsics = depthIntrinsics.clone();
+								newDepthIntrinsics[0] *= scale;
+								newDepthIntrinsics[1] *= scale;
+								newDepthIntrinsics[2] *= scale;
+								newDepthIntrinsics[3] *= scale;
+							}
+						}
+						else if(depthWidth ==240 && depthHeight==180)
+						{
+							// Huawei P30 Pro: only 240x180 is working
+							break;
+						}
+					}
+				}
+				if (resolutions.size()>0) {
+					mTOFAvailable = true;
+					if(newDepthIntrinsics!=null) {
+						depthIntrinsics = newDepthIntrinsics;
+					}
+				}
+			}
+		}
+		Log.i(TAG, "TOF_available: " + mTOFAvailable);
+
+		// Color CPU Image.
+		// Use the currently configured CPU image size.
+		//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
+
+		if (mTOFAvailable) mTOFImageReader.createImageReader(depthWidth, depthHeight);
+
+		// When ARCore is running, make sure it also updates our CPU image surface.
+		if (mTOFAvailable && cameraId.compareTo(depthCameraId) == 0) {
+			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);
+
+			// Open the camera device using the ARCore wrapped callback.
+			cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
+			
+			if(mTOFAvailable && cameraId.compareTo(depthCameraId) != 0)
+			{
+				cameraManager.openCamera(depthCameraId, mTOFImageReader.cameraDeviceCallback, mTOFImageReader.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;
+		} 
+		
+		return true;
+	}
+	
+	public static void rotationMatrixToQuaternion(float[] R, float[] q) {
+        final float m00 = R[0];
+        final float m10 = R[1];
+        final float m20 = R[2];
+        final float m01 = R[4];
+        final float m11 = R[5];
+        final float m21 = R[6];
+        final float m02 = R[8];
+        final float m12 = R[9];
+        final float m22 = R[10];
+
+        float tr = m00 + m11 + m22;
+
+        if (tr > 0) {
+            float S = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw 
+            q[0] = 0.25f * S;/* w  w  w.j ava  2s.co m*/
+            q[1] = (m21 - m12) / S;
+            q[2] = (m02 - m20) / S;
+            q[3] = (m10 - m01) / S;
+        } else if ((m00 > m11) & (m00 > m22)) {
+            float S = (float) Math.sqrt(1.0 + m00 - m11 - m22) * 2; //
+            // S=4*q[1]
+            q[0] = (m21 - m12) / S;
+            q[1] = 0.25f * S;
+            q[2] = (m01 + m10) / S;
+            q[3] = (m02 + m20) / S;
+        } else if (m11 > m22) {
+            float S = (float) Math.sqrt(1.0 + m11 - m00 - m22) * 2; //
+            // S=4*q[2]
+            q[0] = (m02 - m20) / S;
+            q[1] = (m01 + m10) / S;
+            q[2] = 0.25f * S;
+            q[3] = (m12 + m21) / S;
+        } else {
+            float S = (float) Math.sqrt(1.0 + m22 - m00 - m11) * 2; //
+            // S=4*q[3]
+            q[0] = (m10 - m01) / S;
+            q[1] = (m02 + m20) / S;
+            q[2] = (m12 + m21) / S;
+            q[3] = 0.25f * S;
+        }
+
+    }
+
+
+	// Close the camera device.
+	public void close() {
+		Log.w(TAG, "close()");
+		
+		if (sharedSession != null)  {
+			sharedSession.close();
+			sharedSession = null;
+		}
+
+		if (captureSession != null) {
+			captureSession.close();
+			captureSession = null;
+		}
+		if (cameraDevice != null) {
+			cameraDevice.close();
+			cameraDevice = null;
+		}
+		
+		mTOFImageReader.close();
+
+		if(cameraTextureId>=0)
+		{
+			GLES20.glDeleteTextures(1, new int[] {cameraTextureId}, 0);
+		}
+		
+		stopBackgroundThread();
+	}
+
+	public void setDisplayGeometry(int rotation, int width, int height)
+	{
+		if(sharedSession!=null)
+			sharedSession.setDisplayGeometry(rotation, width, height);
+	}
+	
+	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */	
+	// Draw frame when in AR mode. Called on the GL thread.
+	public void updateGL() throws CameraNotAvailableException {
+
+		if(!mReady.get() || sharedSession == null)
+		{
+			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
+		{
+			Log.e(TAG, String.format("frame.getCamera() null!"));
+			return;
+		}
+
+		if (camera == null) { 
+			Log.e(TAG, String.format("camera is null!"));
+			return;
+		}
+		if (camera.getTrackingState() != TrackingState.TRACKING) {
+			final String trackingState = camera.getTrackingState().toString();
+			Log.e(TAG, String.format("Tracking lost! state=%s", trackingState));
+			// This will force a new session on the next frame received
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, 0,0,0,0,0,0,0,0);
+			mActivity.runOnUiThread(new Runnable() {
+				public void run() {
+					if(mToast!=null && previousAnchorPose != null)
+					{
+						String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
+						if(mToast.getView() == null || !mToast.getView().isShown())
+						{
+							mToast.makeText(mActivity.getApplicationContext(), 
+									msg, Toast.LENGTH_LONG).show();
+						}
+						else
+						{
+							mToast.setText(msg);
+						}
+						previousAnchorPose = null;
+					}
+				}
+			});
+			return;
+		}
+
+		if (frame.getTimestamp() != 0) {
+			Pose pose = camera.getPose();
+			
+			// Remove ARCore SLAM corrections by integrating pose from previous frame anchor
+			if(previousAnchorPose == null || mARCoreLocalizationFilteringSpeed==0)
+			{
+				odomPose = pose;
+			}
+			else
+			{
+				float[] t = previousAnchorPose.inverse().compose(pose).getTranslation();
+				final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8);
+				if(speed>=mARCoreLocalizationFilteringSpeed)
+				{
+					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose).compose(pose.inverse());
+					t = arCoreCorrection.getTranslation();
+					Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2]));
+					
+					t = odomPose.getTranslation();
+					float[] t2 = (arCoreCorrection.compose(pose)).getTranslation();
+					float[] t3 = previousAnchorPose.getTranslation();
+					float[] t4 = pose.getTranslation();
+					Log.e(TAG, String.format("Odom = %f %f %f -> %f %f %f ArCore= %f %f %f -> %f %f %f", t[0], t[1], t[2], t2[0], t2[1], t2[2], t3[0], t3[1], t3[2], t4[0], t4[1], t4[2]));
+				
+					mActivity.runOnUiThread(new Runnable() {
+						public void run() {
+							if(mToast!=null)
+							{
+								String msg = String.format("ARCore localization has been suppressed "
+										+ "because of high speed detected (%f m/s) causing a jump! You can change "
+										+ "ARCore localization filtering speed in Settings->Mapping if you are "
+										+ "indeed moving as fast.", speed);
+								if(mToast.getView() == null || !mToast.getView().isShown())
+								{
+									mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
+								}
+								else
+								{
+									mToast.setText(msg);
+								}
+								previousAnchorPose = null;
+							}
+						}
+					});
+				}
+				odomPose = arCoreCorrection.compose(pose);
+			}
+			previousAnchorPose = pose;
+			previousAnchorTimeStamp = frame.getTimestamp();
+			
+			double stamp = (double)frame.getTimestamp()/10e8;
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp));
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
+			
+			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
+			try{
+				Image image = frame.acquireCameraImage();
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("frame=%d vs image=%d", frame.getTimestamp(), image.getTimestamp()));
+				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();
+
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d stamp=%f", 
+						image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
+				
+				float[] texCoord = new float[8];
+				frame.transformCoordinates2d(
+				        Coordinates2d.OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				        QUAD_COORDS,
+				        Coordinates2d.IMAGE_NORMALIZED,
+				        texCoord);
+				
+				float[] p = new float[16];
+		        camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
+   
+		        float[] viewMatrix = new float[16];
+		        arCoreCorrection.compose(camera.getDisplayOrientedPose()).inverse().toMatrix(viewMatrix, 0);
+		        float[] quat = new float[4];
+		        rotationMatrixToQuaternion(viewMatrix, quat);
+
+		        
+				if(mTOFAvailable)
+				{
+					ByteBuffer depth;
+					double depthStamp;
+					synchronized (mTOFImageReader) {
+						depth = mTOFImageReader.depth16_raw;
+						depthStamp = (double)mTOFImageReader.timestamp/10e8;
+					}
+					
+					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp));
+
+					RTABMapLib.postOdometryEventDepth(
+							RTABMapActivity.nativeApplication,
+							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], 
+							depthIntrinsics[0], depthIntrinsics[1], depthIntrinsics[2], depthIntrinsics[3],
+							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
+							depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(),
+							stamp,
+							depthStamp,
+							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							points, points.limit()/4,
+							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
+							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
+                            texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
+				}
+				else
+				{
+					RTABMapLib.postOdometryEvent(
+							RTABMapActivity.nativeApplication,
+							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], 
+							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
+							stamp, 
+							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							points, points.limit()/4,
+							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
+							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
+                            texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
+				}
+				
+				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;
+	}
+
+}
@@ -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) {
@@ -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); 
+            } 
+        } 
+    } 
+
+}
@@ -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;
+	}
+
+}
@@ -0,0 +1,166 @@
+package com.introlab.rtabmap;
+
+/*
+ * Copyright 2017 Google LLC
+ *
+ * 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.
+ */
+
+import android.app.Activity;
+import android.content.Context;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCharacteristics;
+import android.hardware.camera2.CameraManager;
+import android.hardware.display.DisplayManager;
+import android.hardware.display.DisplayManager.DisplayListener;
+import android.view.Display;
+import android.view.Surface;
+import android.view.WindowManager;
+import com.google.ar.core.Session;
+
+/**
+ * Helper to track the display rotations. In particular, the 180 degree rotations are not notified
+ * by the onSurfaceChanged() callback, and thus they require listening to the android display
+ * events.
+ */
+public final class DisplayRotationHelper implements DisplayListener {
+  private boolean viewportChanged;
+  private int viewportWidth;
+  private int viewportHeight;
+  private final Display display;
+  private final DisplayManager displayManager;
+  private final CameraManager cameraManager;
+
+  /**
+   * Constructs the DisplayRotationHelper but does not register the listener yet.
+   *
+   * @param context the Android {@link Context}.
+   */
+  public DisplayRotationHelper(Context context) {
+    displayManager = (DisplayManager) context.getSystemService(Context.DISPLAY_SERVICE);
+    cameraManager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
+    WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE);
+    display = windowManager.getDefaultDisplay();
+  }
+
+  /** Registers the display listener. Should be called from {@link Activity#onResume()}. */
+  public void onResume() {
+    displayManager.registerDisplayListener(this, null);
+  }
+
+  /** Unregisters the display listener. Should be called from {@link Activity#onPause()}. */
+  public void onPause() {
+    displayManager.unregisterDisplayListener(this);
+  }
+
+  /**
+   * Records a change in surface dimensions. This will be later used by {@link
+   * #updateSessionIfNeeded(Session)}. Should be called from {@link
+   * android.opengl.GLSurfaceView.Renderer
+   * #onSurfaceChanged(javax.microedition.khronos.opengles.GL10, int, int)}.
+   *
+   * @param width the updated width of the surface.
+   * @param height the updated height of the surface.
+   */
+  public void onSurfaceChanged(int width, int height) {
+    viewportWidth = width;
+    viewportHeight = height;
+    viewportChanged = true;
+  }
+
+  /**
+   * Updates the session display geometry if a change was posted either by {@link
+   * #onSurfaceChanged(int, int)} call or by {@link #onDisplayChanged(int)} system callback. This
+   * function should be called explicitly before each call to {@link Session#update()}. This
+   * function will also clear the 'pending update' (viewportChanged) flag.
+   *
+   * @param session the {@link Session} object to update if display geometry changed.
+   */
+  public void updateSessionIfNeeded(ARCoreSharedCamera session) {
+    if (viewportChanged) {
+      int displayRotation = display.getRotation();
+      session.setDisplayGeometry(displayRotation, viewportWidth, viewportHeight);
+      viewportChanged = false;
+    }
+  }
+
+  /**
+   *  Returns the aspect ratio of the GL surface viewport while accounting for the display rotation
+   *  relative to the device camera sensor orientation.
+   */
+  public float getCameraSensorRelativeViewportAspectRatio(String cameraId) {
+    float aspectRatio;
+    int cameraSensorToDisplayRotation = getCameraSensorToDisplayRotation(cameraId);
+    switch (cameraSensorToDisplayRotation) {
+      case 90:
+      case 270:
+        aspectRatio = (float) viewportHeight / (float) viewportWidth;
+        break;
+      case 0:
+      case 180:
+        aspectRatio = (float) viewportWidth / (float) viewportHeight;
+        break;
+      default:
+        throw new RuntimeException("Unhandled rotation: " + cameraSensorToDisplayRotation);
+    }
+    return aspectRatio;
+  }
+
+  /**
+   * Returns the rotation of the back-facing camera with respect to the display. The value is one of
+   * 0, 90, 180, 270.
+   */
+  public int getCameraSensorToDisplayRotation(String cameraId) {
+    CameraCharacteristics characteristics;
+    try {
+      characteristics = cameraManager.getCameraCharacteristics(cameraId);
+    } catch (CameraAccessException e) {
+      throw new RuntimeException("Unable to determine display orientation", e);
+    }
+
+    // Camera sensor orientation.
+    int sensorOrientation = characteristics.get(CameraCharacteristics.SENSOR_ORIENTATION);
+
+    // Current display orientation.
+    int displayOrientation = toDegrees(display.getRotation());
+
+    // Make sure we return 0, 90, 180, or 270 degrees.
+    return (sensorOrientation - displayOrientation + 360) % 360;
+  }
+
+  private int toDegrees(int rotation) {
+    switch (rotation) {
+      case Surface.ROTATION_0:
+        return 0;
+      case Surface.ROTATION_90:
+        return 90;
+      case Surface.ROTATION_180:
+        return 180;
+      case Surface.ROTATION_270:
+        return 270;
+      default:
+        throw new RuntimeException("Unknown rotation " + rotation);
+    }
+  }
+
+  @Override
+  public void onDisplayAdded(int displayId) {}
+
+  @Override
+  public void onDisplayRemoved(int displayId) {}
+
+  @Override
+  public void onDisplayChanged(int displayId) {
+    viewportChanged = true;
+  }
+}
@@ -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());
+        }
+    }
+
+}
@@ -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);
+	}
+}
@@ -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,88 +28,137 @@ 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);
+    public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
+    
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize, boolean clearDatabase);
+    
+    public static native boolean recover(long nativeApplication, String from, String to);

-    // Release all non OpenGl resources that are allocated from the program.
-    public static native void onPause();
+    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 setDepthFromMotion(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 setPointSize(float value);
-    public static native void setLighting(boolean enabled);
-    public static native void setMeshDecimation(int value);
-    public static native void setMeshAngleTolerance(float value);
-    public static native void setMeshTriangleSize(int value);
-    public static native void setMinClusterSize(int value);
-    public static native void setMaxGainRadius(float 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 void save(long nativeApplication, String outputDatabasePath);
+    public static native void cancelProcessing(long nativeApplication);
    public static native boolean exportMesh(
-    		String filePath,
+    		long nativeApplication, 
    		float cloudVoxelSize,
+    		boolean regenerateCloud,
    		boolean meshing,
    		int textureSize,
+    		int textureCount,
    		int normalK,
    		boolean optimized,
    		float optimizedVoxelSize,
    		int optimizedDepth,
-    		float optimizedDecimationFactor,
+    		int optimizedMaxPolygons,
    		float optimizedColorRadius,
    		boolean optimizedCleanWhitePolygons,
-    		boolean optimizedColorWhitePolygons,
+    		int optimizedMinClusterSize,
+    		float optimizedMaxTextureDistance,
+    		int optimizedMinTextureClusterSize,
    		boolean blockRendering);
-    public static native boolean postExportation(boolean visualize);
-    public static native int postProcessing(int approach);
+    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, double stamp);
+    public static native void postOdometryEvent(long nativeApplication,
+    		float x, float y, float z, float qx, float qy, float qz, float qw,
+    		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+    		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+    		double stamp, 
+    		ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+    		FloatBuffer points, int pointsLen,
+    		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+            float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+            float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
+    public static native void postOdometryEventDepth(long nativeApplication,
+    		float x, float y, float z, float qx, float qy, float qz, float qw,
+    		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+    		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+    		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+    		float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
+    		double rgbStamp,
+    		double depthStamp,
+    		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,
+    		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+            float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+            float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
    
 }
@@ -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,239 @@ 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 DisplayRotationHelper mDisplayRotationHelper = null;
 	
-	private ProgressDialog mProgressDialog;
+	private Vector<TextObject> mTexts;
+
+	private static RTABMapActivity mActivity;
+	public Renderer(RTABMapActivity c) {
+		mActivity = c;
+		mDisplayRotationHelper = new DisplayRotationHelper(/*context=*/ 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;
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.onDisplayChanged(0);
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
+	}

-  // 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)
+			{
+				int step = 0;
+				try
+				{
+					if(mCamera!=null)
+					{
+						step=1;
+						mCamera.updateGL();
+						
+					}
+					
+					step=2;
+					
+					final int value = RTABMapLib.render(mActivity.nativeApplication);
+		
+					if(mTextManager!=null)
+					{
+						step=3;
+						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);
+					}
+					step=4;
+					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.getApplicationContext(), 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.getApplicationContext(), String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+								}
+							}
+						});
+					}
+				}
+				catch(final Exception e)
+				{
+					final int stepF = step;
+					mActivity.runOnUiThread(new Runnable() {
+						public void run() {
+							if(mToast!=null)
+							{
+								String msg = String.format("Rendering error! (exception=%s) step=%d", e.getMessage(), stepF);
+								Log.e("RTABMapActivity", msg);
+								mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).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);
+		}
+		
+		mDisplayRotationHelper.onSurfaceChanged(width, height);
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
+		
+		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);
+		}
+	}
 }
@@ -1,55 +1,255 @@
 package com.introlab.rtabmap;

-import android.app.Activity;
+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.preference.PreferenceManager;
+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 button = findPreference(getString(R.string.pref_key_reset_button));
-        button.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        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)findPreference(getString(R.string.pref_key_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_decimation))).getEntry() + ") "+getString(R.string.pref_summary_decimation));
+        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_arcore_localization_filtering_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
        ((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_decimation_factor))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_decimation_factor))).getValue() + "%%) "+getString(R.string.pref_summary_opt_decimation_factor));
        ((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_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_cluster_size));
+        ((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));
    }
    
@@ -57,28 +257,62 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
        Preference pref = findPreference(key);

        if (pref instanceof ListPreference) {
-        	if(key.compareTo(getString(R.string.pref_key_decimation))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_decimation));
-        	if(key.compareTo(getString(R.string.pref_key_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth));
+        	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_arcore_localization_filtering_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
        	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_decimation_factor))==0) pref.setSummary("("+((ListPreference)pref).getValue() + "%%) "+getString(R.string.pref_summary_opt_decimation_factor));
        	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_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_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));
        
        }
@@ -99,4 +333,57 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
        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;
+			}
+		}
+	}
+}
@@ -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;
+		}
+	}
+}
@@ -0,0 +1,265 @@
+package com.introlab.rtabmap;
+
+
+import android.graphics.ImageFormat;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCaptureSession;
+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.media.ImageReader;
+import android.os.Handler;
+import android.os.HandlerThread;
+import android.support.annotation.NonNull;
+import android.util.Log;
+import android.view.Surface;
+
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
+
+	public static final String TAG = TOF_ImageReader.class.getSimpleName();
+	
+	public int WIDTH;
+	public int HEIGHT;
+	
+    public ImageReader imageReader;
+    public int frameCount = 0;
+    public long timestamp;
+    
+    // Looper handler thread.
+    private HandlerThread backgroundThread;
+    // Looper handler.
+    public Handler backgroundHandler;
+
+    public ByteBuffer depth16_raw;
+    
+    // Camera capture session. 
+ 	private CameraCaptureSession captureSession = null;
+ 	// Camera device. 
+ 	private CameraDevice cameraDevice = null;
+ 	
+ // Camera preview capture request builder
+ 	private CaptureRequest.Builder previewCaptureRequestBuilder;
+
+
+    TOF_ImageReader(){
+    }
+    
+    public void close()
+    {
+    	Log.i(TAG, "close()");
+    	if (captureSession != null) {
+			captureSession.close();
+			captureSession = null;
+		}
+		if (cameraDevice != null) {
+			cameraDevice.close();
+			cameraDevice = null;
+		}
+		
+		if (imageReader != null) {
+			imageReader.close();
+			imageReader = null;
+		}
+    }
+    
+ // Camera device state callback.
+ 	public final CameraDevice.StateCallback cameraDeviceCallback =
+ 			new CameraDevice.StateCallback() {
+ 		@Override
+ 		public void onOpened(@NonNull CameraDevice cameraDevice) {
+ 			Log.d(TAG, "Camera depth ID " + cameraDevice.getId() + " opened.");
+ 			TOF_ImageReader.this.cameraDevice = cameraDevice;
+ 			createCameraPreviewSession();
+ 		}
+
+ 		@Override
+ 		public void onClosed(@NonNull CameraDevice cameraDevice) {
+ 			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+
+ 		@Override
+ 		public void onDisconnected(@NonNull CameraDevice cameraDevice) {
+ 			Log.w(TAG, "Camera depth ID " + cameraDevice.getId() + " disconnected.");
+ 			cameraDevice.close();
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+
+ 		@Override
+ 		public void onError(@NonNull CameraDevice cameraDevice, int error) {
+ 			Log.e(TAG, "Camera depth ID " + cameraDevice.getId() + " error " + error);
+ 			cameraDevice.close();
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+ 	};
+ 	
+ 	private CameraCaptureSession.StateCallback captureStateCallback = 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.");
+		}
+
+		@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);
+ 		}
+ 	};
+ 	
+ 	// 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.");
+		frameCount = 0;
+		try {
+			
+			// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
+			previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
+
+			previewCaptureRequestBuilder.addTarget(imageReader.getSurface());
+
+			// Create camera capture session for camera preview using callback.
+			cameraDevice.createCaptureSession(Arrays.asList(imageReader.getSurface()), captureStateCallback, backgroundHandler);
+
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "CameraAccessException", e);
+		}
+	}
+
+    public void createImageReader(int width, int height){
+    	Log.w(TAG, String.valueOf(width) + "x" + String.valueOf(height));
+    	WIDTH = width;
+    	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(TAG, "onImageAvailable: Skipping null image.");
+            return;
+        }
+        else{
+            if(image.getFormat() == ImageFormat.DEPTH16){
+                synchronized (this) {
+                	 depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+                	 timestamp = image.getTimestamp();
+				}
+                // copy raw undecoded DEPTH16 format depth data to NativeBuffer
+                frameCount++;
+            }
+            else{
+                Log.w(TAG, "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(TAG, "Interrupted while trying to join depth background handler thread", e);
+            }
+        }
+    }
+
+}
+
@@ -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;
+	}
+}
@@ -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;
+	}*/
+}
@@ -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);
+	}
+	
+}
@@ -0,0 +1,68 @@
+# Xcode
+#
+# gitignore contributors: remember to update Global/Xcode.gitignore, Objective-C.gitignore & Swift.gitignore
+
+## User settings
+xcuserdata/
+
+## compatibility with Xcode 8 and earlier (ignoring not required starting Xcode 9)
+*.xcscmblueprint
+*.xccheckout
+
+## compatibility with Xcode 3 and earlier (ignoring not required starting Xcode 4)
+build/
+DerivedData/
+*.moved-aside
+*.pbxuser
+!default.pbxuser
+*.mode1v3
+!default.mode1v3
+*.mode2v3
+!default.mode2v3
+*.perspectivev3
+!default.perspectivev3
+
+## Obj-C/Swift specific
+*.hmap
+
+## App packaging
+*.ipa
+*.dSYM.zip
+*.dSYM
+
+# CocoaPods
+#
+# We recommend against adding the Pods directory to your .gitignore. However
+# you should judge for yourself, the pros and cons are mentioned at:
+# https://guides.cocoapods.org/using/using-cocoapods.html#should-i-check-the-pods-directory-into-source-control
+#
+# Pods/
+#
+# Add this line if you want to avoid checking in source code from the Xcode workspace
+# *.xcworkspace
+
+# Carthage
+#
+# Add this line if you want to avoid checking in source code from Carthage dependencies.
+# Carthage/Checkouts
+
+Carthage/Build/
+
+# fastlane
+#
+# It is recommended to not store the screenshots in the git repo.
+# Instead, use fastlane to re-generate the screenshots whenever they are needed.
+# For more information about the recommended setup visit:
+# https://docs.fastlane.tools/best-practices/source-control/#source-control
+
+fastlane/report.xml
+fastlane/Preview.html
+fastlane/screenshots/**/*.png
+fastlane/test_output
+
+# Code Injection
+#
+# After new code Injection tools there's a generated folder /iOSInjectionProject
+# https://github.com/johnno1962/injectionforxcode
+
+iOSInjectionProject/
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Workspace
+   version = "1.0">
+   <FileRef
+      location = "self:">
+   </FileRef>
+</Workspace>
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>IDEDidComputeMac32BitWarning</key>
+	<true/>
+</dict>
+</plist>
@@ -0,0 +1,16 @@
+{
+  "object": {
+    "pins": [
+      {
+        "package": "Zip",
+        "repositoryURL": "https://github.com/marmelroy/Zip.git",
+        "state": {
+          "branch": null,
+          "revision": "bd19d974e8a38cc8d3a88c90c8a107386c3b8ccf",
+          "version": "2.1.1"
+        }
+      }
+    ]
+  },
+  "version": 1
+}
@@ -0,0 +1,79 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Scheme
+   LastUpgradeVersion = "1230"
+   version = "1.3">
+   <BuildAction
+      parallelizeBuildables = "YES"
+      buildImplicitDependencies = "YES">
+      <BuildActionEntries>
+         <BuildActionEntry
+            buildForTesting = "YES"
+            buildForRunning = "YES"
+            buildForProfiling = "YES"
+            buildForArchiving = "YES"
+            buildForAnalyzing = "YES">
+            <BuildableReference
+               BuildableIdentifier = "primary"
+               BlueprintIdentifier = "4EE015C1259A2AF0008CCE65"
+               BuildableName = "RTABMapApp.app"
+               BlueprintName = "RTABMapApp"
+               ReferencedContainer = "container:RTABMapApp.xcodeproj">
+            </BuildableReference>
+         </BuildActionEntry>
+      </BuildActionEntries>
+   </BuildAction>
+   <TestAction
+      buildConfiguration = "Debug"
+      selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+      selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+      shouldUseLaunchSchemeArgsEnv = "YES">
+      <Testables>
+      </Testables>
+   </TestAction>
+   <LaunchAction
+      buildConfiguration = "Release"
+      selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+      selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+      launchStyle = "0"
+      useCustomWorkingDirectory = "NO"
+      ignoresPersistentStateOnLaunch = "NO"
+      debugDocumentVersioning = "YES"
+      debugServiceExtension = "internal"
+      enableGPUFrameCaptureMode = "2"
+      allowLocationSimulation = "YES">
+      <BuildableProductRunnable
+         runnableDebuggingMode = "0">
+         <BuildableReference
+            BuildableIdentifier = "primary"
+            BlueprintIdentifier = "4EE015C1259A2AF0008CCE65"
+            BuildableName = "RTABMapApp.app"
+            BlueprintName = "RTABMapApp"
+            ReferencedContainer = "container:RTABMapApp.xcodeproj">
+         </BuildableReference>
+      </BuildableProductRunnable>
+   </LaunchAction>
+   <ProfileAction
+      buildConfiguration = "Release"
+      shouldUseLaunchSchemeArgsEnv = "YES"
+      savedToolIdentifier = ""
+      useCustomWorkingDirectory = "NO"
+      debugDocumentVersioning = "YES">
+      <BuildableProductRunnable
+         runnableDebuggingMode = "0">
+         <BuildableReference
+            BuildableIdentifier = "primary"
+            BlueprintIdentifier = "4EE015C1259A2AF0008CCE65"
+            BuildableName = "RTABMapApp.app"
+            BlueprintName = "RTABMapApp"
+            ReferencedContainer = "container:RTABMapApp.xcodeproj">
+         </BuildableReference>
+      </BuildableProductRunnable>
+   </ProfileAction>
+   <AnalyzeAction
+      buildConfiguration = "Debug">
+   </AnalyzeAction>
+   <ArchiveAction
+      buildConfiguration = "Release"
+      revealArchiveInOrganizer = "YES">
+   </ArchiveAction>
+</Scheme>
@@ -0,0 +1,76 @@
+//
+//  AppDelegate.swift
+//  GLKittutorial
+//
+//  Created by Mathieu Labbe on 2020-12-28.
+//
+
+import UIKit
+import ARKit
+
+func setDefaultsFromSettingsBundle() {
+    
+    let plistFiles = ["Root", "Mapping", "Assembling"]
+    
+    for plistName in plistFiles {
+        //Read PreferenceSpecifiers from Root.plist in Settings.Bundle
+        if let settingsURL = Bundle.main.url(forResource: plistName, withExtension: "plist", subdirectory: "Settings.bundle"),
+            let settingsPlist = NSDictionary(contentsOf: settingsURL),
+            let preferences = settingsPlist["PreferenceSpecifiers"] as? [NSDictionary] {
+
+            for prefSpecification in preferences {
+
+                if let key = prefSpecification["Key"] as? String, let value = prefSpecification["DefaultValue"] {
+
+                    //If key doesn't exists in userDefaults then register it, else keep original value
+                    if UserDefaults.standard.value(forKey: key) == nil {
+
+                        UserDefaults.standard.set(value, forKey: key)
+                        NSLog("registerDefaultsFromSettingsBundle: Set following to UserDefaults - (key: \(key), value: \(value), type: \(type(of: value)))")
+                    }
+                }
+            }
+        } else {
+            NSLog("registerDefaultsFromSettingsBundle: Could not find Settings.bundle")
+        }
+    }
+}
+
+@main
+class AppDelegate: UIResponder, UIApplicationDelegate {
+
+    var window: UIWindow?
+    
+    func application(_ application: UIApplication, didFinishLaunchingWithOptions launchOptions: [UIApplication.LaunchOptionsKey: Any]?) -> Bool {
+        
+        // Always set Version to default
+        let defaults = UserDefaults.standard
+        defaults.removeObject(forKey: "Version")
+        
+        setDefaultsFromSettingsBundle()
+        
+        // Override point for customization after application launch.
+        if !ARWorldTrackingConfiguration.supportsFrameSemantics(.sceneDepth) {
+            // Ensure that the device supports scene depth and present
+            //  an error-message view controller, if not.
+            let storyboard = UIStoryboard(name: "Main", bundle: nil)
+            window?.rootViewController = storyboard.instantiateViewController(withIdentifier: "unsupportedDeviceMessage")
+        }
+        return true
+    }
+
+    // MARK: UISceneSession Lifecycle
+
+    func application(_ application: UIApplication, configurationForConnecting connectingSceneSession: UISceneSession, options: UIScene.ConnectionOptions) -> UISceneConfiguration {
+        // Called when a new scene session is being created.
+        // Use this method to select a configuration to create the new scene with.
+        return UISceneConfiguration(name: "Default Configuration", sessionRole: connectingSceneSession.role)
+    }
+
+    func application(_ application: UIApplication, didDiscardSceneSessions sceneSessions: Set<UISceneSession>) {
+        // Called when the user discards a scene session.
+        // If any sessions were discarded while the application was not running, this will be called shortly after application:didFinishLaunchingWithOptions.
+        // Use this method to release any resources that were specific to the discarded scenes, as they will not return.
+    }
+}
+
@@ -0,0 +1,11 @@
+{
+  "colors" : [
+    {
+      "idiom" : "universal"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}
@@ -0,0 +1,116 @@
+{
+  "images" : [
+    {
+      "filename" : "RTAB-Map40-1.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map60.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map58-1.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map87.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map80-1.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map120-1.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map120.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "60x60"
+    },
+    {
+      "filename" : "RTAB-Map180.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "60x60"
+    },
+    {
+      "filename" : "RTAB-Map20.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map40.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map29.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map58.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map40-2.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map80.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map76.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "76x76"
+    },
+    {
+      "filename" : "RTAB-Map152.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "76x76"
+    },
+    {
+      "filename" : "RTAB-Map167.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "83.5x83.5"
+    },
+    {
+      "filename" : "RTAB-Map1024.png",
+      "idiom" : "ios-marketing",
+      "scale" : "1x",
+      "size" : "1024x1024"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}
--- a/Show More
+++ b/Show More