explicitly set deps to install instead of relying on buildfarm (#1012)

* explicitly set deps to install instead of relying on buildfarm rtabmap (in case buildfarm build is failing)

* use rosdep instead

* missing rosdep update

* updated rosdep command

* removed rosdep init

* fixed qt_gui_cpp not found

* revert last commit, should be able to use qt_gui_cpp in rosdep

* removed deprecated 18.04

.appveyor.yml

@@ -33,36 +33,41 @@ install:
   - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
   - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
   # OpenCV
-  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
+  #- appveyor-retry appveyor DownloadFile http://downloads.sourceforge.net/project/opencvlibrary/4.5.2/opencv-4.5.2-vc14_vc15.exe
-  - cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
+  #- 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/build\""
+  - ps: "ls \"C:/Program Files/opencv\""
-  - set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
+  - 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 PCL:"
+  - 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 PCL:"
+  - 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 PCL:"
+  - 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 PCL:"
+  - ECHO "Installed Eigen:"
   - ps: "ls \"C:/Program Files/Eigen\""
   # PCL
-  - ps: wget 'https://dl.dropboxusercontent.com/s/r9tvi9md54zlul2/PCL-1_8_1-July2018-msvc140.exe?dl=0' -outfile PCL_1.8.1.exe
+  - 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\""
@@ -85,12 +90,49 @@ install:
   - 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" -DZLIB_ROOT="C:\Program Files\zlib" -DBUILD_AS_BUNDLE=ON ..
+  - 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
@@ -101,7 +143,7 @@ artifacts:
 notifications:
   - provider: Email
     to:
-      - matlabbe@email.com
+      - matlabbe@gmail.com
     on_build_success: false
     on_build_failure: false
     on_build_status_changed: true

.dockerignore

@@ -0,0 +1 @@
+build/*

.github/workflows/cmake-ros.yml

@@ -0,0 +1,66 @@
+name: CMake-ROS
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+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_distribution }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        ros_distribution: [ noetic, foxy, humble, rolling]
+        include:
+        - ros_distribution: 'noetic'
+          os: ubuntu-20.04
+        - ros_distribution: 'foxy'
+          os: ubuntu-20.04
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-22.04
+
+    steps:
+    - name: Workaround dpkg grub-efi-amd64-signed error
+      run: |
+        sudo apt-mark hold grub-efi-amd64-signed
+    
+    - uses: ros-tooling/setup-ros@v0.5
+      with:
+        required-ros-distributions: ${{ matrix.ros_distribution }}
+    
+    - uses: actions/checkout@v2
+
+    - name: Install dependencies
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        rosdep update
+        rosdep install --from-paths ${{github.workspace}} -y
+
+    - name: Configure CMake
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        ./rtabmap-console --version
+      

.github/workflows/cmake.yml

@@ -0,0 +1,48 @@
+name: CMake
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    name: ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-22.04, ubuntu-20.04]
+
+    steps:    
+    - name: Install dependencies
+      run: |
+        DEBIAN_FRONTEND=noninteractive
+        sudo apt-get update
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
+
+    - uses: actions/checkout@v2
+
+    - name: Configure CMake
+      run: |
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./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}}
+      

.github/workflows/docker.yml

@@ -0,0 +1,131 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      matrix:
+        docker_tag: [xenial, bionic, focal, focal-foxy, jammy, android23, android24, android26, android30]
+        include:
+        - docker_tag: xenial
+          docker_tags: |
+            introlab3it/rtabmap:xenial
+            introlab3it/rtabmap:16.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'xenial'
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_args: |
+            NOT_USED=0
+          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_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal'
+        - docker_tag: focal-foxy
+          docker_tags: |
+            introlab3it/rtabmap:focal-foxy
+            introlab3it/rtabmap:20.04-foxy
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal-foxy'
+        - docker_tag: jammy
+          docker_tags: |
+            introlab3it/rtabmap:jammy
+            introlab3it/rtabmap:22.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_args: |
+            API_VERSION=23
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_apiXX'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_args: |
+            API_VERSION=24
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_apiXX'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_args: |
+            API_VERSION=26
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_apiXX'
+        - docker_tag: android30
+          docker_tags: |
+            introlab3it/rtabmap:android30
+          docker_args: |
+            API_VERSION=30
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_apiXX'
+    
+    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: .
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
+          build-args: |
+            ${{ matrix.docker_args }}
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+

.gitignore

@@ -8,3 +8,7 @@ app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml
 app/android/res/raw/
+compile_flags.txt
+tags
+build_*
+*.bak

.travis.yml

@@ -1,34 +0,0 @@
-sudo: true
-dist: trusty
-language: cpp
-group: deprecated-2017Q3
-
-compiler:
-  - gcc
-  - clang
-
-addons:
-  apt:
-    packages:
-      - cmake
-      - libopencv-dev
-      - libqt4-dev
-      - libsqlite3-dev
-      - libyaml-cpp-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 update && sudo apt-get install dpkg
-    - 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

LICENSE

@@ -1,5 +1,6 @@
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+RTAB-Map - https://github.com/introlab/rtabmap
-All rights reserved.
+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.

README.md

@@ -1,13 +1,12 @@
-rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pixel) 
+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://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
 
-[release-image]: https://img.shields.io/badge/release-0.16.3-green.svg?style=flat
+[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
@@ -15,6 +14,63 @@ Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master
 
 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).
+ * For more information (e.g., papers, major updates), visit [RTAB-Map's home page](http://introlab.github.io/rtabmap).
+ * For installation instructions and examples, visit [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>
+
+#### CI Latest
+
+  <table>
+    <tbody>
+        <tr>
+           <td>Linux</td>
+           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
+           </td>
+        </tr>
+        <tr>
+           <td>Windows</td>
+           <td><a href="https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true" alt="Build Status"/>
+           </td>
+        </tr>
+     </tbody>
+  </table>
+ 
+ #### ROS Binaries
+ 
+ `ros-$ROS_DISTRO-rtabmap`
+ 
+ <table>
+    <tbody>
+        <tr>
+           <td rowspan="2">ROS 1</td>
+           <td>Melodic</td>
+            <td><a href="http://build.ros.org/job/Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Noetic</td>
+            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td rowspan="3">ROS 2</td>
+            <td>Foxy</td>
+            <td><a href="http://build.ros2.org/job/Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Humble</td>
+            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Rolling</td>
+            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+    </tbody>
+</table>
+ 

RTABMapConfig.cmake.in

@@ -1,96 +1,91 @@
-# - Config file for the RTABMap package
+include(CMakeFindDependencyMacro)
-# 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
-#  RTABMap_CORE         - core library
-#  RTABMap_UTILITE      - utilite library
-#  RTABMap_GUI          - gui library (set if RTABMap is built with Qt)
 
-# Compute paths
+# Mandatory dependencies
-get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
-set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
 
-#core lib
+if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+    
-
+    if(@CONF_QT_VERSION@ EQUAL 6)
-IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
+        find_dependency(Qt6 COMPONENTS Widgets Core Gui OpenGL)
-   SET(RTABMap_CORE
+    elseif(@CONF_QT_VERSION@ EQUAL 5)
-      debug ${RTABMap_CORE_DEBUG}
+        find_dependency(Qt5 COMPONENTS Widgets Core Gui OpenGL)
-      optimized ${RTABMap_CORE_RELEASE}
+    else() # Qt4
-   )
+        find_dependency(Qt4 COMPONENTS QtCore QtGui)
-ELSEIF(RTABMap_CORE_DEBUG)
+    endif()
-   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+    set(RTABMap_QT_VERSION @CONF_QT_VERSION@)
 ELSE()
-   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation)
+ENDIF()
+set(RTABMap_DEFINITIONS ${PCL_DEFINITIONS})
+add_definitions(${RTABMap_DEFINITIONS}) # To include -march=native if set
+
+# Optional dependencies
+IF(EXISTS "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
+    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
 ENDIF()
 
-#utilite lib
+IF(@CONF_WITH_REALSENSE2@)
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+    IF(WIN32)
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+        find_dependency(RealSense2)
-
+    ELSE()
-IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
+        find_dependency(realsense2)
-   SET(RTABMap_UTILITE
+    ENDIF()
-      debug ${RTABMap_UTILITE_DEBUG}
-      optimized ${RTABMap_UTILITE_RELEASE}
-   )
-ELSEIF(RTABMap_UTILITE_DEBUG)
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
-ELSE()
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ENDIF()
 
-set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
+IF(@CONF_WITH_K4A@)
+    IF(WIN32)
+        find_dependency(K4A)
+    ELSE()
+        find_dependency(k4a)
+        find_dependency(k4arecord)
+    ENDIF()
+ENDIF()
 
-list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
+IF(@CONF_WITH_OCTOMAP@)
-set(WITH_GUI ON)
+  find_dependency(octomap)
-if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+ENDIF()
-   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
-   if (${_index} EQUAL -1)
-      set(WITH_GUI OFF)
-   endif()
-endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
 
+# Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies)
+set(RTABMap_INCLUDE_DIRS "")
+set(RTABMap_LIBRARIES "")
+set(RTABMap_TARGETS "")
 
-#gui lib (OFF if RTAB-Map is not built with Qt)
+set(_RTABMap_supported_components utilite core 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@)
-   
-   IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
-      SET(RTABMap_GUI
-         debug ${RTABMap_GUI_DEBUG}
-         optimized ${RTABMap_GUI_RELEASE}
-      )
-   ELSEIF(RTABMap_GUI_RELEASE)
-      SET(RTABMap_GUI ${RTABMap_GUI_RELEASE})
-   ELSEIF(RTABMap_GUI_DEBUG)
-      SET(RTABMap_GUI ${RTABMap_GUI_DEBUG})
-   ENDIF()
-   
-   set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
-elseif(${WITH_GUI})
-   MESSAGE(ERROR "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
-endif()
 
-# Dependencies
+foreach(_comp ${_RTABMap_supported_components})
-if(@CONF_VTK_QT@ AND ${WITH_GUI})
+  if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
-   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
+    include("${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
-endif(@CONF_VTK_QT@ AND ${WITH_GUI})
+    set(RTABMap_${_comp}_FOUND True)
-SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
+    set(RTABMap_TARGETS
+      ${RTABMap_TARGETS}
+      rtabmap::${_comp})
+    get_target_property(RTABMap_${_comp}_INCLUDE_DIRS rtabmap::${_comp} INTERFACE_INCLUDE_DIRECTORIES)
+    get_target_property(RTABMap_${_comp}_LIBRARIES rtabmap::${_comp} INTERFACE_LINK_LIBRARIES)
+    set(RTABMap_INCLUDE_DIRS 
+      ${RTABMap_INCLUDE_DIRS}
+      ${RTABMap_${_comp}_INCLUDE_DIRS})
+    set(RTABMap_LIBRARIES 
+      ${RTABMap_LIBRARIES}
+      rtabmap::${_comp})
+    if(RTABMap_${_comp}_LIBRARIES)
+      set(RTABMap_LIBRARIES 
+        ${RTABMap_LIBRARIES}
+        ${RTABMap_${_comp}_LIBRARIES})
+    endif()
+  else()
+    set(RTABMap_${_comp}_FOUND False)
+  endif()
+endforeach()
 
-#backward compatibilities
+include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")
-set(RTABMAP_CORE ${RTABMap_CORE})
-set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-if(RTABMap_GUI)
-   set(RTABMAP_GUI ${RTABMap_GUI})
-   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
-endif(RTABMap_GUI)
 
-include(FindPackageHandleStandardArgs)
+foreach(_comp ${RTABMap_FIND_COMPONENTS})
-find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
+  if (NOT ";${_RTABMap_supported_components};" MATCHES ";${_comp};")
-mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)
+    set(RTABMap_${_comp}_FOUND False)
+    if(${RTABMap_FIND_REQUIRED_${_comp}})
+      set(RTABMap_FOUND False)
+      set(RTABMap_NOT_FOUND_MESSAGE "Unsupported or not found required component: ${_comp}")
+    endif()
+  endif()
+endforeach()

RTABMapConfigVersion.cmake.in

@@ -1,11 +0,0 @@
-set(PACKAGE_VERSION "@RTABMAP_VERSION@")
-
-# Check whether the requested PACKAGE_FIND_VERSION is compatible
-if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
-  set(PACKAGE_VERSION_COMPATIBLE FALSE)
-else()
-  set(PACKAGE_VERSION_COMPATIBLE TRUE)
-  if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
-    set(PACKAGE_VERSION_EXACT TRUE)
-  endif()
-endif()

Version.h.in

@@ -40,32 +40,62 @@ 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_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
 @GTSAM@#define RTABMAP_GTSAM
+@CERES@#define RTABMAP_CERES
+@MRPT@#define RTABMAP_MRPT
 @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
+@OPENGV@#define RTABMAP_OPENGV
+@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
-@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
+@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_ */
 

app/android/.gitignore

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

app/android/AndroidManifest.xml.in

@@ -2,22 +2,26 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="70"
+        android:versionCode="72"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
-    <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
-    <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
     <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-permission android:name="android.permission.ACCESS_WIFI_STATE" />
     <uses-feature android:name="android.hardware.location.gps" />
     <uses-feature android:glEsVersion="0x00020000" />
 
-    <!-- This is the platform API where NativeActivity was introduced. -->
+    <!-- This is the platform API where depth16 support in android was introduced. -->
-    <uses-sdk android:minSdkVersion="19" />
+    <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 
@@ -25,7 +29,9 @@
         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. -->
@@ -41,11 +47,41 @@
             <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" android:screenOrientation="fullSensor"/>
         <activity android:name="SketchfabActivity" android:label="@string/sketchfab" android:screenOrientation="fullSensor"/>
+    
+        <meta-data
+            android:name="com.google.ar.core.min_apk_version"
+            android:value="191106000" /> <!-- This activity is critical for installing ARCore when it is not already present. -->
+        <activity
+            android:name="com.google.ar.core.InstallActivity"
+            android:configChanges="keyboardHidden|orientation|screenSize"
+            android:excludeFromRecents="true"
+            android:exported="false"
+            android:launchMode="singleTop"
+            android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
+            
+        <provider
+            android:name="android.support.v4.content.FileProvider"
+            android:authorities="com.introlab.rtabmap.provider"
+            android:exported="false"
+            android:grantUriPermissions="true">
+            <meta-data
+                android:name="android.support.FILE_PROVIDER_PATHS"
+                android:resource="@xml/provider_paths"/>
+        </provider>
     </application>
 
 </manifest>

app/android/CMakeLists.txt

@@ -1,10 +1,95 @@
 
+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}")
 
-MESSAGE(STATUS "DISABLE_LOG = ${DISABLE_LOG}")
 
 IF(DISABLE_LOG)
    SET(ANDROID_DEBUGGABLE false)
@@ -29,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"

app/android/CameraAvailability.h.in

@@ -1,41 +1,39 @@
-/*
+/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
+All rights reserved.
-
+
-Redistribution and use in source and binary forms, with or without
+Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
+modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
+    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
+      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
+    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
+      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
+      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
+    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
+      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
+      derived from this software without specific prior written permission.
-
+
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+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
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DISCLAIMED. IN NO EVENT SHALL UNIVERTY DE SHERBROOKE BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+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
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
+*/
-
+ 
-#ifndef RTABMAPGUIEXP_H
+#ifndef CAMERAAVAILABILITY_H_
-#define RTABMAPGUIEXP_H
+#define CAMERAAVAILABILITY_H_
+
+// This is auto-generated!
+
+@TANGO@#define RTABMAP_TANGO
+@ARCORE@#define RTABMAP_ARCORE
+@ARENGINE@#define RTABMAP_ARENGINE
+
+
+#endif /* CAMERAAVAILABILITY_H_ */
 
-#if defined(_WIN32)
-  #if defined(rtabmap_gui_EXPORTS)
-    #define RTABMAPGUI_EXP   __declspec( dllexport )
-  #else
-    #define RTABMAPGUI_EXP   __declspec( dllimport )
-  #endif
-#else
-  #define RTABMAPGUI_EXP
-#endif
-
-#endif // RTABMAPGUIEXP_H

app/android/custom_rules.xml

@@ -6,8 +6,8 @@
 		<copy todir="${native.libs.dir}/${android.abi}">
 			<fileset dir="${srcdir}/jni/third-party/lib" includes="*.so"/>
 		</copy>
-                <copy todir="${native.libs.dir}">
+		<copy todir="${native.libs.dir}">
-                        <fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
-                </copy>
+		</copy>
 	</target>
 </project>

app/android/info.txt.in

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

app/android/jni/.gitignore

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

app/android/jni/CMakeLists.txt

@@ -1,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)

app/android/jni/CameraARCore.cpp

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

app/android/jni/CameraARCore.h

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

app/android/jni/CameraAREngine.cpp

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

app/android/jni/CameraAREngine.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAARENGINE_H_
+#define CAMERAARENGINE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+
+#include <huawei_arengine_interface.h>
+
+namespace rtabmap {
+
+class CameraAREngine : public CameraMobile {
+public:
+	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
+	virtual ~CameraAREngine();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly();
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	HwArSession* arSession_ = nullptr;
+	HwArConfig* arConfig_ = nullptr;
+	HwArFrame* arFrame_ = nullptr;
+	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	HwArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	GLuint textureId_;
+	UMutex arSessionMutex_;
+
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARENGINE_H_ */

app/android/jni/CameraMobile.cpp

@@ -0,0 +1,463 @@
+/*
+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);
+            if(pt.z > 0)
+            {
+                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 */

app/android/jni/CameraMobile.h

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

app/android/jni/CameraTango.cpp

@@ -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 {
 
@@ -42,11 +43,22 @@ const int holeSize = 5;
 const float maxDepthError = 0.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);
-	if(app->isRunning() && point_cloud->num_points>0)
+	if(point_cloud->num_points>0)
 	{
 		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 	}
@@ -55,34 +67,32 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(app->isRunning())
-	{
-		cv::Mat tangoImage;
-		if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-		{
-			tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
-		}
-		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else if(color->format == 35)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else
-		{
-			LOGE("Not supported color format : %d.", color->format);
-		}
 
-		if(!tangoImage.empty())
+	cv::Mat tangoImage;
-		{
+	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+	{
-		}
+		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+	}
+	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else if(color->format == 35)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else
+	{
+		LOGE("Not supported color format : %d.", color->format);
+	}
+
+	if(!tangoImage.empty())
+	{
+		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
 	}
 }
 
@@ -91,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);
 	}
 }
 
@@ -104,23 +114,14 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-const float CameraTango::bilateralFilteringSigmaS = 2.0f;
-const float CameraTango::bilateralFilteringSigmaR = 0.075f;
-
 CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
-		Camera(0),
+		CameraMobile(smoothing),
 		tango_config_(0),
-		previousStamp_(0.0),
-		stampEpochOffset_(0.0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),
 		rawScanPublished_(publishRawScan),
-		smoothing_(smoothing),
-		cloudStamp_(0),
 		tangoColorType_(0),
-		tangoColorStamp_(0),
+		tangoColorStamp_(0)
-		colorCameraToDisplayRotation_(ROTATION_0),
-		originUpdate_(false)
 {
 	UASSERT(decimation >= 1);
 }
@@ -350,6 +351,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			pose_data.orientation[1],
 			pose_data.orientation[2],
 			pose_data.orientation[3]);
+	deviceTColorCamera_ = rtabmap_world_T_opengl_world * deviceTColorCamera_;
 
 	// camera intrinsic
 	TangoCameraIntrinsics color_camera_intrinsics;
@@ -408,18 +410,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	model_ = CameraModel(colorCamera_?"color":"fisheye",
 			cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
 			K, D, R, P,
-			tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_); // device to camera optical rotation in rtabmap frame
+			deviceTColorCamera_);
 
 	if(!colorCamera_)
 	{
 		initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
 	}
 
-	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
+	LOGI("deviceTColorCameraRtabmap  =%s", deviceTColorCamera_.prettyPrint().c_str());
-	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
-
-	cameraStartedTime_.restart();
-
 	return true;
 }
 
@@ -433,23 +431,15 @@ void CameraTango::close()
 		TangoService_disconnect();
 		LOGI("TangoService_disconnect() done.");
 	}
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
-	lastKnownGPS_ = GPS();
-	originOffset_ = Transform();
-	originUpdate_ = false;
-}
 
-void CameraTango::resetOrigin()
+	CameraMobile::close();
-{
-	originUpdate_ = true;
 }
 
 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);
 
@@ -466,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();
+				bool notify = !data_.isValid();
-				cloud_ = cloud.clone();
+
-				cloudStamp_ = timestamp;
+				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());
 			}
 		}
 	}
@@ -487,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);
 
@@ -499,40 +717,9 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	}
 }
 
-static rtabmap::Transform opticalRotation(
-								1.0f,  0.0f,  0.0f, 0.0f,
-							    0.0f, -1.0f,  0.0f, 0.0f,
-								0.0f,  0.0f, -1.0f, 0.0f);
-void CameraTango::poseReceived(const Transform & pose)
-{
-	if(!pose.isNull())
-	{
-		// send pose of the camera (without optical rotation), not the device
-		Transform p = pose*deviceTColorCamera_*opticalRotation;
-		if(originUpdate_)
-		{
-			originOffset_ = p.translation().inverse();
-			originUpdate_ = false;
-		}
-		if(!originOffset_.isNull())
-		{
-			this->post(new PoseEvent(originOffset_*p));
-		}
-		else
-		{
-			this->post(new PoseEvent(p));
-		}
-	}
-}
-
 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
 {
-	this->post(new CameraTangoEvent(type, key, value));
+	this->post(new CameraInfoEvent(type, key, value));
-}
-
-bool CameraTango::isCalibrated() const
-{
-	return model_.isValidForProjection();
 }
 
 std::string CameraTango::getSerial() const
@@ -540,11 +727,6 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }
 
-void CameraTango::setGPS(const GPS & gps)
-{
-	lastKnownGPS_ = gps;
-}
-
 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
@@ -587,8 +769,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	}
 	else
 	{
-
+		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
-		pose = tangoPoseToTransform(&pose_start_service_T_device);
 	}
 
 	return pose;
@@ -598,375 +779,116 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 {
 	//LOGI("Capturing image...");
 
-	SensorData data;
+	if(textureId_ == 0)
-	if(!dataReady_.acquire(1, 2000))
 	{
-		if(this->isRunning())
+		glGenTextures(1, &textureId_);
-		{
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
-			LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-			this->post(new CameraTangoEvent(0, "CameraTango", "No frames received since 2 seconds."));
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-			boost::mutex::scoped_lock  lock(dataMutex_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-			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;
 	}
-	else
+
+	// Update Texture (optional, just for first-view rendering)
+	if(colorCamera_ && textureId_)
 	{
-		cv::Mat cloud;
+		double video_overlay_timestamp;
-		cv::Mat tangoImage;
+		TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);
-		cv::Mat rgb;
-		double cloudStamp = 0.0;
-		double rgbStamp = 0.0;
-		int tangoColorType = 0;
 
+		if (status == TANGO_SUCCESS)
 		{
-			boost::mutex::scoped_lock  lock(dataMutex_);
+			if(info)
-			cloud = cloud_;
-			cloudStamp = cloudStamp_;
-			cloud_ = cv::Mat();
-			cloudStamp_ = 0.0;
-			tangoImage = tangoColor_;
-			rgbStamp = tangoColorStamp_;
-			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_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 if(tangoColorType == 35)
-		{
-			cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
-		}
-		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(colorCamera_)
-		{
-			if(decimation_ > 1)
 			{
-				rgb = util2d::decimate(rgb, decimation_);
+				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
-				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
+			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
-		// timestamp.
+			  if (rotation > 3) {
-		cv::Mat depth;
+				  rotation -= 4;
+			  }
 
-		// Calculate the relative pose from color camera frame at timestamp
+			TangoDoubleMatrixTransformData matrix_transform;
-		// color_timestamp t1 and depth
+			status = TangoSupport_getDoubleMatrixTransformAtTime(
-		// camera frame at depth_timestamp t0.
+					video_overlay_timestamp,
-		Transform colorToDepth;
+					TANGO_COORDINATE_FRAME_CAMERA_COLOR,
-		TangoPoseData pose_color_image_t1_T_depth_image_t0;
+					TANGO_COORDINATE_FRAME_START_OF_SERVICE,
-		if (TangoSupport_calculateRelativePose(
+					TANGO_SUPPORT_ENGINE_OPENGL,
-				rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
+					TANGO_SUPPORT_ENGINE_OPENGL,
-			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+					static_cast<TangoSupportRotation>(rotation),
-			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
+					&matrix_transform);
-		{
+			if (matrix_transform.status_code == TANGO_POSE_VALID)
-			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)
 			{
-				float * p = cloud.ptr<float>(0,i);
+				// Get projection matrix
-				cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
+				TangoCameraIntrinsics color_camera_intrinsics;
+				int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
+						TANGO_CAMERA_COLOR,
+						static_cast<TangoSupportRotation>(rotation),
+						&color_camera_intrinsics);
 
-				if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
+				if (ret == TANGO_SUCCESS) {
-				{
+					float image_width = static_cast<float>(color_camera_intrinsics.width);
-					scanData.at(oi++) = pt;
+					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);
 
-				int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+					viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
-				// get the coordinate on image plane.
+					if(!getOriginOffset().isNull())
-				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;
+						viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
 					}
-				}
 
-				bool pixelSet = false;
+					projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
-				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+							image_width, image_height, fx, fy, cx, cy, 0.3, 50);
-				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
+
-				   depth_value)
+					switch(rotation)
-				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
-					if(depthPixel == 0 || depthPixel > depth_value)
 					{
-						depthPixel = depth_value;
+					case ROTATION_90:
-						pixelSet = true;
+						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;
 				}
-				if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+				else
-				   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);
+					UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
-					if(depthPixel == 0 || depthPixel > depth_value)
-					{
-						depthPixel = depth_value;
-						pixelSet = true;
-					}
 				}
-				if(pixelSet)
-				{
-					pixelsSet += 1;
-				}
-			}
-
-			if(closePoints > 100)
-			{
-				this->post(new CameraTangoEvent(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 poseDevice = getPoseAtTimestamp(rgbStamp);
-
-			// adjust origin
-			if(!originOffset_.isNull())
-			{
-				poseDevice = originOffset_ * poseDevice;
-			}
-
-			//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());
-
-			Transform scanLocalTransform = model.localTransform();
-
-			// Rotate image depending on the camera orientation
-			if(colorCameraToDisplayRotation_ == ROTATION_90)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::flip(rgb,rgb,1);
-				cv::transpose(rgb,rgbt);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::flip(depth,depth,1);
-				cv::transpose(depth,deptht);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy(), model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_180)
-			{
-				cv::flip(rgb,rgb,1);
-				cv::flip(rgb,rgb,0);
-				cv::flip(depth,depth,1);
-				cv::flip(depth,depth,0);
-				cv::Size sizet(model.imageWidth(), model.imageHeight());
-				model = CameraModel(
-						model.fx(),
-						model.fy(),
-						model.cx()>0?model.imageWidth()-model.cx():0,
-						model.cy()>0?model.imageHeight()-model.cy():0,
-						model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144*2.0));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_270)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::transpose(rgb,rgbt);
-				cv::flip(rgbt,rgbt,1);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::transpose(depth,deptht);
-				cv::flip(deptht,deptht,1);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy()>0?model.imageHeight()-model.cy():0, model.cx(), model.localTransform()*rtabmap::Transform(0,0,0,0,0,-1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-
-			if(smoothing_)
-			{
-				//UTimer t;
-				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
-				data.setDepthOrRightRaw(depth);
-				//LOGD("Bilateral filtering, time=%fs", t.ticks());
-			}
-
-			if(rawScanPublished_)
-			{
-				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
 			else
 			{
-				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+				UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
-			}
-			data.setGroundTruth(odom);
-
-			if(lastKnownGPS_.stamp() > 0.0 && rgbStamp-lastKnownGPS_.stamp()<1.0)
-			{
-				data.setGPS(lastKnownGPS_);
-			}
-			else if(lastKnownGPS_.stamp()>0.0)
-			{
-				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
 			}
 		}
 		else
 		{
-			LOGE("Could not get depth and rgb images!?!");
+			UERROR("TangoService_updateTextureExternalOes failed!");
+		}
+	}
+
+	SensorData data;
+	if(dataReady_.acquireTry(1))
+	{
+		boost::mutex::scoped_lock  lock(dataMutex_);
+		data = data_;
+		data_ = SensorData();
+		if(info)
+		{
+			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
-			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;
-			}
-			info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
-			LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
-			this->post(new OdometryEvent(data, pose, info));
-			previousPose_ = pose;
-			previousStamp_ = data.stamp();
-		}
-		else if(!this->isKilled())
-		{
-			LOGW("Odometry lost");
-			this->post(new OdometryEvent());
-		}
-	}
-	else
-	{
-		UERROR("Camera not initialized, cannot start thread.");
-		this->kill();
-	}
-}
-
 } /* namespace rtabmap */

app/android/jni/CameraTango.h

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CAMERATANGO_H_
 #define CAMERATANGO_H_
 
+#include "CameraMobile.h"
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
@@ -37,65 +38,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <tango_client_api.h>
 #include <tango_support_api.h>
 
-class TangoPoseData;
-
 namespace rtabmap {
 
-class PoseEvent: public UEvent
+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:
-	static const float bilateralFilteringSigmaS;
-	static const float bilateralFilteringSigmaR;
-
 public:
 	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	void close(); // close Tango connection
+	virtual void close(); // close Tango connection
-	void resetOrigin();
-	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-	const CameraModel & getCameraModel() const {return model_;}
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
 	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
-	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
-	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
-	void setGPS(const GPS & gps);
 
 	void 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:
@@ -104,34 +66,19 @@ protected:
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
 
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-
 private:
 	void * tango_config_;
-	Transform previousPose_;
-	double previousStamp_;
-	UTimer cameraStartedTime_;
-	double stampEpochOffset_;
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	bool smoothing_;
+	SensorData data_;
-	cv::Mat cloud_;
-	double cloudStamp_;
 	cv::Mat tangoColor_;
 	int tangoColorType_;
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	CameraModel model_;
-	Transform deviceTColorCamera_;
-	TangoSupportRotation colorCameraToDisplayRotation_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
-	GPS lastKnownGPS_;
-	Transform originOffset_;
-	bool originUpdate_;
 };
 
 } /* namespace rtabmap */

app/android/jni/ProgressionStatus.h

@@ -11,7 +11,9 @@
 #include <rtabmap/core/ProgressState.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
+#ifdef __ANDROID__
 #include <jni.h>
+#endif
 
 namespace rtabmap {
 
@@ -27,16 +29,29 @@ public:
 class ProgressionStatus: public ProgressState, public UEventsHandler
 {
 public:
-	ProgressionStatus() : count_(0), max_(100), jvm_(0), rtabmap_(0)
+	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)
 	{
@@ -82,6 +97,7 @@ protected:
 			count_ += ((ProgressEvent*)event)->count_;
 			// Call JAVA callback
 			bool success = false;
+#ifdef __ANDROID__
 			if(jvm_ && rtabmap_)
 			{
 				JNIEnv *env = 0;
@@ -103,6 +119,16 @@ protected:
 				}
 				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");
@@ -114,8 +140,13 @@ protected:
 private:
 	int count_;
 	int max_;
+#ifdef __ANDROID__
 	JavaVM *jvm_;
 	jobject rtabmap_;
+#else
+    void * swiftClassPtr_;
+    void(*swiftCallback)(void *, int, int);
+#endif
 };
 
 }

app/android/jni/RTABMapApp.h

@@ -28,14 +28,15 @@ 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 "CameraMobile.h"
 #include "util.h"
 #include "ProgressionStatus.h"
 
@@ -45,44 +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);
 
-  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
-
-  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
-
-  // Explicitly reset motion tracking and restart the pipeline.
-  // Note that this will cause motion tracking to re-initialize.
-  void TangoResetMotionTracking();
-
-  // Tango Service point cloud callback function for depth data. Called when new
-  // new point cloud data is available from the Tango Service.
-  //
-  // @param pose: The current point cloud returned by the service,
-  //              caller allocated.
-  void onPointCloudAvailable(const TangoXYZij* xyz_ij);
-
-  // Tango service pose callback function for pose data. Called when new
-  // information about device pose is available from the Tango Service.
-  //
-  // @param pose: The current pose returned by the service, caller allocated.
-  void onPoseAvailable(const TangoPoseData* pose);
-
-  // Tango service event callback function for event data. Called when new events
-  // are available from the Tango Service.
-  //
-  // @param event: Tango event, caller allocated.
-  void onTangoEventAvailable(const TangoEvent* event);
 
+  bool isBuiltWith(int cameraDriver) const;
+#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();
 
@@ -92,8 +92,7 @@ class RTABMapApp : public UEventsHandler {
   // Main render loop.
   int Render();
 
-  // Release all non-OpenGL allocated resources.
+  void stopCamera();
-  void onPause();
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
@@ -135,22 +134,26 @@ class RTABMapApp : public UEventsHandler {
   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 setMinCloudDepth(float value);
   void setCloudDensityLevel(int value);
   void setMeshAngleTolerance(float value);
+  void setMeshDecimationFactor(float value);
   void setMeshTriangleSize(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);
+  bool recover(const std::string & from, const std::string & to);
   void cancelProcessing();
   bool exportMesh(
 		  float cloudVoxelSize,
@@ -173,21 +176,42 @@ class RTABMapApp : public UEventsHandler {
   bool writeExportedMesh(const std::string & directory, const std::string & name);
   int postProcessing(int approach);
 
+  void postCameraPoseEvent(
+  		float x, float y, float z, float qx, float qy, float qz, float qw, 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 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_;
@@ -196,6 +220,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
+  bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;
@@ -204,14 +229,15 @@ class RTABMapApp : public UEventsHandler {
   int cloudDensityLevel_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
+  float meshDecimationFactor_;
   float clusterRatio_;
   float maxGainRadius_;
   int renderingTextureDecimation_;
   float backgroundColor_;
+  int depthConfidence_;
 
   rtabmap::ParametersMap mappingParameters_;
 
-  bool paused_;
   bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
   bool openingDatabase_;
@@ -222,7 +248,6 @@ class RTABMapApp : public UEventsHandler {
   bool bilateralFilteringOnNextRender_;
   bool takeScreenshotOnNextRender_;
   bool cameraJustInitialized_;
-  int meshDecimation_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
@@ -241,16 +266,18 @@ class RTABMapApp : public UEventsHandler {
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
   Scene main_scene_;
+    
+    UTimer fpsTime_;
 
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
-	std::list<rtabmap::RtabmapEvent*> visLocalizationEvents_;
 	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 visLocalizationMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
@@ -258,12 +285,29 @@ class RTABMapApp : public UEventsHandler {
 
 	USemaphore screenshotReady_;
 
-	std::map<int, Mesh> createdMeshes_;
+	std::map<int, rtabmap::Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
 
 	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_

app/android/jni/background_renderer.cc

@@ -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");
+}
+

app/android/jni/background_renderer.h

@@ -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_

app/android/jni/graph_drawable.cpp

@@ -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,

app/android/jni/graph_drawable.h

@@ -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>

app/android/jni/point_cloud_drawable.cpp

@@ -35,7 +35,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "util.h"
 #include "pcl/common/transforms.h"
 
+#ifdef __ANDROID__
 #include <GLES2/gl2.h>
+#else // __APPLE__
+#include <OpenGLES/ES2/gl.h>
+#endif
 
 #define LOW_DEC 2
 #define LOWLOW_DEC 4
@@ -57,7 +61,7 @@ enum PointCloudShaders
 
 // PointCloud shaders
 const std::string kPointCloudVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
     "attribute vec3 aColor;\n"
@@ -75,7 +79,7 @@ const std::string kPointCloudVertexShader =
     "  vColor = aColor;\n"
     "}\n";
 const std::string kPointCloudLightingVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
 	"attribute vec3 aNormal;\n"
@@ -100,7 +104,7 @@ const std::string kPointCloudLightingVertexShader =
     "}\n";
 
 const std::string kPointCloudFragmentShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
 	"uniform float uGainR;\n"
 	"uniform float uGainG;\n"
@@ -127,7 +131,8 @@ const std::string kPointCloudBlendingFragmentShader =
     "  vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
 	"  float alpha = 1.0;\n"
 	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
-	"  float depth = texture2D(uDepthTexture, coord).r;\n"
+	"  vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+	"  float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
 	"  float num =  (2.0 * uNearZ * uFarZ);\n"
 	"  float diff = (uFarZ - uNearZ);\n"
 	"  float add = (uFarZ + uNearZ);\n"
@@ -141,7 +146,7 @@ const std::string kPointCloudBlendingFragmentShader =
     "}\n";
 
 const std::string kPointCloudDepthPackingVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
     "uniform mat4 uMVP;\n"
@@ -154,15 +159,15 @@ const std::string kPointCloudDepthPackingFragmentShader =
     "precision highp float;\n"
     "precision mediump int;\n"
     "void main() {\n"
-	"  float toFixed = 255.0/256.0;\n"
+	"  vec4 enc = vec4(1.,255.,65025.,16581375.) * gl_FragCoord.z;\n"
-	"  vec4 enc = vec4(1.0, 255.0, 65025.0, 160581375.0) * toFixed * gl_FragCoord.z;\n"
 	"  enc = fract(enc);\n"
+	"  enc -= enc.yzww * vec2(1./255., 0.).xxxy;\n"
 	"  gl_FragColor = enc;\n"
     "}\n";
 
 // Texture shaders
 const std::string kTextureMeshVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
     "attribute vec2 aTexCoord;\n"
@@ -185,7 +190,7 @@ const std::string kTextureMeshVertexShader =
     "  vLightWeighting = 1.0;\n"
     "}\n";
 const std::string kTextureMeshLightingVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
 	"attribute vec3 aNormal;\n"
@@ -214,7 +219,7 @@ const std::string kTextureMeshLightingVertexShader =
     "    vLightWeighting=0.5;\n"
     "}\n";
 const std::string kTextureMeshFragmentShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
 	"uniform sampler2D uTexture;\n"
 	"uniform float uGainR;\n"
@@ -245,7 +250,8 @@ const std::string kTextureMeshBlendingFragmentShader =
     "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
     "  float alpha = 1.0;\n"
 	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
-	"  float depth = texture2D(uDepthTexture, coord).r;\n"
+	"  vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+	"  float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
 	"  float num =  (2.0 * uNearZ * uFarZ);\n"
 	"  float diff = (uFarZ - uNearZ);\n"
 	"  float add = (uFarZ + uNearZ);\n"
@@ -255,7 +261,7 @@ const std::string kTextureMeshBlendingFragmentShader =
 	"  float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
 	"  if(linearFragz > linearDepth + 0.05)\n"
 	"    alpha=0.0;\n"
-	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
+    "  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
     "}\n";
 
 std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
@@ -303,8 +309,8 @@ PointCloudDrawable::PointCloudDrawable(
 		float gainR,
 		float gainG,
 		float gainB) :
-				vertex_buffers_(0),
+				vertex_buffer_(0),
-				textures_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -314,14 +320,16 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(gainG),
 				gainB_(gainB)
 {
+    index_buffers_.resize(6, 0);
+    index_buffers_count_.resize(6, 0);
 	updateCloud(cloud, indices);
 }
 
 PointCloudDrawable::PointCloudDrawable(
-		const Mesh & mesh,
+		const rtabmap::Mesh & mesh,
 		bool createWireframe) :
-				vertex_buffers_(0),
+				vertex_buffer_(0),
-				textures_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -331,64 +339,83 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(1.0f),
 				gainB_(1.0f)
 {
+    index_buffers_.resize(6, 0);
+    index_buffers_count_.resize(6, 0);
 	updateMesh(mesh, createWireframe);
 }
 
 PointCloudDrawable::~PointCloudDrawable()
 {
-	LOGI("Freeing cloud buffer %d", vertex_buffers_);
+	LOGI("Freeing cloud buffer %d", vertex_buffer_);
-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
+		glDeleteBuffers(1, &vertex_buffer_);
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 	}
 
-	if (textures_)
+	if (texture_)
 	{
-		glDeleteTextures(1, &textures_);
+		glDeleteTextures(1, &texture_);
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		textures_ = 0;
+		texture_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+        }
+    }
 }
 
 void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
 {
-	LOGD("Update polygons");
+    for(int i=0; i<4; ++i)
-	polygons_.clear();
+    {
-	polygonLines_.clear();
+        if(index_buffers_[i])
-	polygonsLowRes_.clear();
+        {
-	polygonLinesLowRes_.clear();
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updatePolygons() clearing polygon buffers");
+        }
+    }
+    
+	//LOGD("Update polygons");
 	if(polygons.size() && organizedToDenseIndices_.size())
 	{
-		unsigned int polygonSize = polygons[0].vertices.size();
+		size_t polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
-		polygons_.resize(polygons.size() * polygonSize);
+        std::vector<std::vector<GLuint> > indexes(4);
+        indexes[0].resize(polygons.size() * polygonSize);
 		if(createWireframe)
-			polygonLines_.resize(polygons_.size()*2);
+            indexes[2].resize(indexes[0].size()*2);
 		int oi = 0;
 		int li = 0;
-		for(unsigned int i=0; i<polygons.size(); ++i)
+        for(size_t i=0; i<polygons.size(); ++i)
 		{
 			UASSERT(polygons[i].vertices.size() == polygonSize);
 			for(unsigned int j=0; j<polygonSize; ++j)
 			{
-				polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+                indexes[0][oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
 				if(createWireframe)
 				{
-					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+                    indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
-					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
+                    indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
 				}
 			}
 		}
 
 		if(polygonsLowRes.size())
 		{
-			unsigned int polygonSize = polygonsLowRes[0].vertices.size();
+			size_t polygonSize = polygonsLowRes[0].vertices.size();
 			UASSERT(polygonSize == 3);
-			polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
+            indexes[1].resize(polygonsLowRes.size() * polygonSize);
 			if(createWireframe)
-				polygonLinesLowRes_.resize(polygonsLowRes_.size()*2);
+                indexes[3].resize(indexes[1].size()*2);
 			int oi = 0;
 			int li = 0;
 			for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
@@ -396,15 +423,44 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 				UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
 				for(unsigned int j=0; j<polygonSize; ++j)
 				{
-					polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+                    indexes[1][oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
 					if(createWireframe)
 					{
-						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+                        indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
-						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
+                        indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
 					}
 				}
 			}
 		}
+        
+        // Generate index buffers
+        for(size_t i=0; i<indexes.size(); ++i)
+        {
+            if(!indexes[i].empty())
+            {
+                glGenBuffers(1, &index_buffers_[i]);
+                if(!index_buffers_[i])
+                {
+                    LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                    return;
+                }
+                
+                LOGD("Adding polygon index %ld size=%ld", i, indexes[i].size());
+                
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+                glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * indexes[i].size(), indexes[i].data(), GL_STATIC_DRAW);
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+                index_buffers_count_[i] = (int)indexes[i].size();
+
+                GLint error = glGetError();
+                if(error != GL_NO_ERROR)
+                {
+                    LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                    index_buffers_[i] = 0;
+                    return;
+                }
+            }
+        }
 	}
 }
 
@@ -412,43 +468,51 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 {
 	UASSERT(cloud.get() && !cloud->empty());
 	nPoints_ = 0;
-	polygons_.clear();
-	polygonsLowRes_.clear();
-	verticesLowRes_.clear();
-	verticesLowLowRes_.clear();
 	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
 	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
 
-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
+		glDeleteBuffers(1, &vertex_buffer_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear vertex buffer");
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 	}
 
-	if (textures_)
+	if (texture_)
 	{
-		glDeleteTextures(1, &textures_);
+		glDeleteTextures(1, &texture_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear texture buffer");
-		textures_ = 0;
+		texture_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear index buffer");
+        }
+    }
 
-	glGenBuffers(1, &vertex_buffers_);
+	glGenBuffers(1, &vertex_buffer_);
-	if(!vertex_buffers_)
+	if(!vertex_buffer_)
 	{
 		LOGE("OpenGL: could not generate vertex buffers\n");
 		return;
 	}
 
-	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	LOGI("Creating cloud buffer %d", vertex_buffer_);
 	std::vector<float> vertices;
-	int totalPoints = 0;
+	size_t totalPoints = 0;
+    std::vector<GLuint> verticesLowRes;
+    std::vector<GLuint> verticesLowLowRes;
 	if(indices.get() && indices->size())
 	{
 		totalPoints = indices->size();
 		vertices.resize(indices->size()*4);
-		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
 		int oi_low = 0;
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<indices->size(); ++i)
@@ -465,23 +529,23 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 			{
 				if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+					verticesLowRes[oi_low++] = i;
 				}
 				if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+					verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
+		verticesLowRes.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowLowRes.resize(oi_lowlow);
 	}
 	else
 	{
 		totalPoints = cloud->size();
 		vertices.resize(cloud->size()*4);
-		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
 		int oi_low = 0;
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<cloud->size(); ++i)
@@ -498,19 +562,19 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 			{
 				if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+					verticesLowRes[oi_low++] = i;
 				}
 				if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+					verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
+		verticesLowRes.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowLowRes.resize(oi_lowlow);
 	}
 
-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
 	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
 
@@ -518,26 +582,65 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	if(error != GL_NO_ERROR)
 	{
 		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 		return;
 	}
+    
+    // vertex index buffers
+    for(size_t i=4; i<5; ++i)
+    {
+        if((i==4 && !verticesLowRes.empty()) ||
+           (i==5 && !verticesLowLowRes.empty()))
+        {
+            glGenBuffers(1, &index_buffers_[i]);
+            if(!index_buffers_[i])
+            {
+                LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                return;
+            }
+            
+            index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
+            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+            glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
+            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+            
 
-	nPoints_ = totalPoints;
+            GLint error = glGetError();
+            if(error != GL_NO_ERROR)
+            {
+                LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                index_buffers_[i] = 0;
+                return;
+            }
+        }
+    }
+
+	nPoints_ = (int)totalPoints;
 }
 
-void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
+void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;
 	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
 	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
 
-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
+		glDeleteBuffers(1, &vertex_buffer_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear vertex buffer");
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear index buffer");
+        }
+    }
 
 	gainR_ = mesh.gains[0];
 	gainG_ = mesh.gains[1];
@@ -546,17 +649,17 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 	bool textureUpdate = false;
 	if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
 	{
-		if (textures_)
+		if (texture_)
 		{
-			glDeleteTextures(1, &textures_);
+			glDeleteTextures(1, &texture_);
-			tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+			tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear texture buffer");
-			textures_ = 0;
+			texture_ = 0;
 		}
 		textureUpdate = true;
 	}
 
-	glGenBuffers(1, &vertex_buffers_);
+	glGenBuffers(1, &vertex_buffer_);
-	if(!vertex_buffers_)
+	if(!vertex_buffer_)
 	{
 		LOGE("OpenGL: could not generate vertex buffers\n");
 		return;
@@ -564,10 +667,10 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 
 	if(textureUpdate)
 	{
-		glGenTextures(1, &textures_);
+		glGenTextures(1, &texture_);
-		if(!textures_)
+		if(!texture_)
 		{
-			vertex_buffers_ = 0;
+			vertex_buffer_ = 0;
 			LOGE("OpenGL: could not generate texture buffers\n");
 			return;
 		}
@@ -578,18 +681,20 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 	int totalPoints = 0;
 	std::vector<pcl::Vertices> polygons = mesh.polygons;
 	std::vector<pcl::Vertices> polygonsLowRes;
-	hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
+    hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
 	UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
 	if(mesh.cloud->isOrganized()) // assume organized mesh
 	{
 		polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
 		organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
-		totalPoints = mesh.indices->size();
+		totalPoints = (int)mesh.indices->size();
-		verticesLowRes_.resize(totalPoints);
+        std::vector<GLuint> verticesLowRes;
-		verticesLowLowRes_.resize(totalPoints);
+        std::vector<GLuint> verticesLowLowRes;
+        verticesLowRes.resize(totalPoints);
+        verticesLowLowRes.resize(totalPoints);
 		int oi_low = 0;
 		int oi_lowlow = 0;
-		if(textures_ && polygons.size())
+		if(texture_ && polygons.size())
 		{
 			int items = hasNormals_?9:6;
 			vertices = std::vector<float>(mesh.indices->size()*items);
@@ -622,11 +727,11 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 
 				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+                    verticesLowRes[oi_low++] = i;
 				}
 				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+                    verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
@@ -657,20 +762,48 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 
 				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+                    verticesLowRes[oi_low++] = i;
 				}
 				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+                    verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
+        verticesLowRes.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+        verticesLowLowRes.resize(oi_lowlow);
+        
+        // vertex index buffers
+        for(size_t i=4; i<5; ++i)
+        {
+            if((i==4 && !verticesLowRes.empty()) ||
+               (i==5 && !verticesLowLowRes.empty()))
+            {
+                glGenBuffers(1, &index_buffers_[i]);
+                if(!index_buffers_[i])
+                {
+                    LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                    return;
+                }
+                
+                index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+                glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+
+                GLint error = glGetError();
+                if(error != GL_NO_ERROR)
+                {
+                    LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                    index_buffers_[i] = 0;
+                    return;
+                }
+            }
+        }
 	}
 	else // assume dense mesh with texCoords set to polygons
 	{
-		if(textures_ && polygons.size() && mesh.normals->size())
+		if(texture_ && polygons.size())
 		{
 			//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
 			//		(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
@@ -679,14 +812,14 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 			//  tex_coordinates should be linked to points, not
 			//  polygon vertices. Points linked to multiple different texCoords (different textures) should
 			//  be duplicated.
-			totalPoints = mesh.texCoords.size();
+			totalPoints = (int)mesh.texCoords.size();
-			vertices = std::vector<float>(mesh.texCoords.size()*9);
+            int items = hasNormals_?9:6;
+			vertices = std::vector<float>(mesh.texCoords.size()*items);
 			organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
 
 			UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
 					uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
 
-			int items = hasNormals_?9:6;
 			unsigned int oi=0;
 			for(unsigned int i=0; i<polygons.size(); ++i)
 			{
@@ -737,7 +870,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 		}
 		else
 		{
-			totalPoints = mesh.cloud->size();
+			totalPoints = (int)mesh.cloud->size();
 			//LOGD("Dense mesh");
 			int items = hasNormals_?7:4;
 			organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
@@ -765,7 +898,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 		}
 	}
 
-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
 	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
 
@@ -773,11 +906,11 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 	if(error != GL_NO_ERROR)
 	{
 		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 		return;
 	}
 
-	if(textures_ && textureUpdate)
+	if(texture_ && textureUpdate)
 	{
 		//GLint maxTextureSize = 0;
 		//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
@@ -787,13 +920,13 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 		//LOGW("maxTextureUnits=%d", maxTextureUnits);
 
 		// gen texture from image
-		glBindTexture(GL_TEXTURE_2D, textures_);
+		glBindTexture(GL_TEXTURE_2D, texture_);
 		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);
 		cv::Mat rgbImage;
-		cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGBA);
+		cv::cvtColor(mesh.texture, rgbImage, cv::COLOR_BGR2RGBA);
 
 		glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
 		//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
@@ -805,20 +938,17 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 		if(error != GL_NO_ERROR)
 		{
 			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
-			textures_ = 0;
+			texture_ = 0;
 
-			glDeleteBuffers(1, &vertex_buffers_);
+			glDeleteBuffers(1, &vertex_buffer_);
-			vertex_buffers_ = 0;
+			vertex_buffer_ = 0;
 			return;
 		}
 	}
 
 	nPoints_ = totalPoints;
 
-	if(polygons_.size() != polygons.size())
+    updatePolygons(polygons, polygonsLowRes, createWireframe);
-	{
-		updatePolygons(polygons, polygonsLowRes, createWireframe);
-	}
 
 	if(!pose_.isNull())
 	{
@@ -880,14 +1010,14 @@ void PointCloudDrawable::Render(
 		bool packDepthToColorChannel,
 		bool wireFrame) const
 {
-	if(vertex_buffers_ && nPoints_ && visible_ && !shaderPrograms_.empty())
+	if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
 		if(packDepthToColorChannel || !hasNormals_)
 		{
 			lighting = false;
 		}
 
-		if(packDepthToColorChannel || !(meshRendering && textureRendering && textures_))
+		if(packDepthToColorChannel || !(meshRendering && textureRendering && texture_))
 		{
 			textureRendering = false;
 		}
@@ -990,7 +1120,7 @@ void PointCloudDrawable::Render(
 				// Texture activate unit 0
 				glActiveTexture(GL_TEXTURE0);
 				// Bind the texture to this unit.
-				glBindTexture(GL_TEXTURE_2D, textures_);
+				glBindTexture(GL_TEXTURE_2D, texture_);
 				// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
 				GLuint texture_handle = glGetUniformLocation(program, "uTexture");
 				glUniform1i(texture_handle, 0);
@@ -1006,8 +1136,8 @@ void PointCloudDrawable::Render(
 		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");
 
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
-		if(textures_)
+		if(texture_)
 		{
 			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
 			if(textureRendering)
@@ -1038,53 +1168,49 @@ void PointCloudDrawable::Render(
 		tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");
 
 		UTimer drawTime;
-		if(textureRendering)
+		if((textureRendering || meshRendering) && index_buffers_[0])
 		{
-			if(distanceToCameraSqr<16.0f || polygonsLowRes_.empty())
+            float dist = meshRendering?50.0f:16.0f;
+			if(distanceToCameraSqr<dist || index_buffers_[1]==0)
 			{
-				wireFrame = wireFrame && polygonLines_.size();
+				wireFrame = wireFrame && index_buffers_[2];
 				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[2]);
+                    glDrawElements(GL_LINES, index_buffers_count_[2], GL_UNSIGNED_INT, 0);
+                }
 				else
-					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[0]);
+                    glDrawElements(GL_TRIANGLES, index_buffers_count_[0], GL_UNSIGNED_INT, 0);
+                }
 			}
 			else
 			{
-				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				wireFrame = wireFrame && index_buffers_[3];
 				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[3]);
+                    glDrawElements(GL_LINES, index_buffers_count_[3], GL_UNSIGNED_INT, 0);
+                }
 				else
-					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[1]);
+                    glDrawElements(GL_TRIANGLES, index_buffers_count_[1], GL_UNSIGNED_INT, 0);
+                }
 			}
 		}
-		else if(meshRendering && polygons_.size())
+		else if(index_buffers_[4])
 		{
-			if(distanceToCameraSqr<50.0f || polygonsLowRes_.empty())
+			if(distanceToCameraSqr>600.0f && index_buffers_[5])
 			{
-				wireFrame = wireFrame && polygonLines_.size();
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[5]);
-				if(wireFrame)
+                glDrawElements(GL_POINTS, index_buffers_count_[5], GL_UNSIGNED_INT, 0);
-					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
-				else
-					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
-			}
-			else
-			{
-				wireFrame = wireFrame && polygonLinesLowRes_.size();
-				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
-				else
-					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
-			}
-		}
-		else if(!verticesLowRes_.empty())
-		{
-			if(distanceToCameraSqr>600.0f)
-			{
-				glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
 			}
 			else if(distanceToCameraSqr>150.0f)
 			{
-				glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[4]);
+                glDrawElements(GL_POINTS, index_buffers_count_[4], GL_UNSIGNED_INT, 0);
 			}
 			else
 			{
@@ -1100,6 +1226,7 @@ void PointCloudDrawable::Render(
 
 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
+        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
 
 		glUseProgram(0);
 		tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");

app/android/jni/point_cloud_drawable.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>
@@ -55,21 +57,21 @@ private:
 		  float gainG = 1.0f,
 		  float gainB = 1.0f);
   PointCloudDrawable(
-    	  const Mesh & mesh,
+    	  const rtabmap::Mesh & mesh,
 		  bool createWireframe = false);
   virtual ~PointCloudDrawable();
 
   void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
   void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
-  void updateMesh(const Mesh & mesh, bool createWireframe = false);
+  void updateMesh(const rtabmap::Mesh & mesh, bool createWireframe = false);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
   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 hasMesh() const {return index_buffers_[0] != 0;}
-  bool hasTexture() const {return textures_ != 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_;}
@@ -113,14 +115,10 @@ private:
 
  private:
   // Vertex buffer of the point cloud geometry.
-  GLuint vertex_buffers_;
+  GLuint vertex_buffer_;
-  GLuint textures_;
+  GLuint texture_;
-  std::vector<GLuint> polygons_;
+  std::vector<GLuint> index_buffers_;
-  std::vector<GLuint> polygonsLowRes_;
+  std::vector<int> index_buffers_count_;
-  std::vector<GLuint> polygonLines_;
-  std::vector<GLuint> polygonLinesLowRes_;
-  std::vector<GLuint> verticesLowRes_;
-  std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
   rtabmap::Transform pose_;
   glm::mat4 poseGl_;

app/android/jni/scene.cpp

@@ -69,6 +69,7 @@ const std::string kGraphFragmentShader =
 
 
 Scene::Scene() :
+		background_renderer_(0),
 		gesture_camera_(0),
 		axis_(0),
 		frustum_(0),
@@ -79,14 +80,15 @@ Scene::Scene() :
 		graphVisible_(true),
 		gridVisible_(true),
 		traceVisible_(true),
-		color_camera_to_display_rotation_(ROTATION_0),
+		frustumVisible_(true),
+		color_camera_to_display_rotation_(rtabmap::ROTATION_0),
 		currentPose_(0),
 		graph_shader_program_(0),
 		blending_(true),
 		mapRendering_(true),
 		meshRendering_(true),
 		meshRenderingTexture_(true),
-		pointSize_(5.0f),
+		pointSize_(10.0f),
 		boundingBoxRendering_(false),
 		lighting_(false),
 		backfaceCulling_(true),
@@ -95,19 +97,21 @@ Scene::Scene() :
 		g_(0.0f),
 		b_(0.0f),
 		fboId_(0),
-		depthTexture_(0),
+		rboId_(0),
 		screenWidth_(0),
 		screenHeight_(0),
 		doubleTapOn_(false)
 {
+    depthTexture_ = 0;
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
-	      tango_gl::GestureCamera::kFirstPerson);
+	      tango_gl::GestureCamera::kThirdPersonFollow);
 }
 
 Scene::~Scene() {
 	DeleteResources();
 	delete gesture_camera_;
+	delete currentPose_;
 }
 
 //Should only be called in OpenGL thread!
@@ -126,7 +130,6 @@ void Scene::InitGLContent()
 	trace_ = new tango_gl::Trace();
 	grid_ = new tango_gl::Grid();
 	box_ = new BoundingBoxDrawable();
-	currentPose_ = new rtabmap::Transform();
 
 
 	axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
@@ -158,8 +161,9 @@ void Scene::DeleteResources() {
 		delete frustum_;
 		delete trace_;
 		delete grid_;
-		delete currentPose_;
 		delete box_;
+		delete background_renderer_;
+		background_renderer_ = 0;
 	}
 
 	PointCloudDrawable::releaseShaderPrograms();
@@ -173,8 +177,10 @@ void Scene::DeleteResources() {
 	{
 		glDeleteFramebuffers(1, &fboId_);
 		fboId_ = 0;
-		glDeleteTextures(1, &depthTexture_);
+		glDeleteRenderbuffers(1, &rboId_);
-		depthTexture_ = 0;
+		rboId_ = 0;
+        glDeleteTextures(1, &depthTexture_);
+        depthTexture_ = 0;
 	}
 
 	clear();
@@ -188,6 +194,10 @@ void Scene::clear()
 	{
 		delete iter->second;
 	}
+	for(std::map<int, tango_gl::Axis*>::iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
+	{
+		delete iter->second;
+	}
 	if(trace_)
 	{
 		trace_->ClearVertexArray();
@@ -198,6 +208,7 @@ void Scene::clear()
 		graph_ = 0;
 	}
 	pointClouds_.clear();
+	markers_.clear();
 	if(grid_)
 	{
 		grid_->SetPosition(kHeightOffset);
@@ -209,19 +220,32 @@ void Scene::SetupViewPort(int w, int h) {
 	if (h == 0) {
 		LOGE("Setup graphic height not valid");
 	}
+    
 	UASSERT(gesture_camera_ != 0);
 	gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
 	glViewport(0, 0, w, h);
-	if(screenWidth_ != w || fboId_ == 0)
+	if(screenWidth_ != w || screenHeight_ != h || fboId_ == 0)
 	{
+        UINFO("Setup viewport OpenGL: %dx%d", w, h);
+        
 		if(fboId_>0)
 		{
 			glDeleteFramebuffers(1, &fboId_);
 			fboId_ = 0;
+			glDeleteRenderbuffers(1, &rboId_);
+			rboId_ = 0;
 			glDeleteTextures(1, &depthTexture_);
-			depthTexture_ = 0;
+            depthTexture_ = 0;
 		}
 
+        GLint originid = 0;
+        glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
+        
+		// regenerate fbo texture
+		// create a framebuffer object, you need to delete them when program exits.
+		glGenFramebuffers(1, &fboId_);
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
 		// Create depth texture
 		glGenTextures(1, &depthTexture_);
 		glBindTexture(GL_TEXTURE_2D, depthTexture_);
@@ -229,24 +253,22 @@ void Scene::SetupViewPort(int w, int h) {
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-		glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, w, h, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_INT, NULL);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
 		glBindTexture(GL_TEXTURE_2D, 0);
+        
+		glGenRenderbuffers(1, &rboId_);
+		glBindRenderbuffer(GL_RENDERBUFFER, rboId_);
+		glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, w, h);
+		glBindRenderbuffer(GL_RENDERBUFFER, 0);
 
-		// regenerate fbo texture
+		// Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
-		// create a framebuffer object, you need to delete them when program exits.
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
-		glGenFramebuffers(1, &fboId_);
+		glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
-		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
-
-		// Set the texture to be at the depth attachment point of the FBO
-		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depthTexture_, 0);
 
 		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
-		if ( status != GL_FRAMEBUFFER_COMPLETE)
+        UASSERT ( status == GL_FRAMEBUFFER_COMPLETE);
-		{
+		glBindFramebuffer(GL_FRAMEBUFFER, originid);
-			LOGE("Frame buffer cannot be generated! Status: %in", status);
+    }
-		}
-		glBindFramebuffer(GL_FRAMEBUFFER,0);
-	}
 	screenWidth_ = w;
 	screenHeight_ = h;
 }
@@ -319,8 +341,9 @@ std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normaliz
 /**
  * Tells whether or not b is intersecting f.
  * http://www.txutxi.com/?p=584
- * @param f Viewing frustum.
+ * @param planes Viewing frustum.
- * @param b An axis aligned bounding box.
+ * @param boxMin The axis aligned bounding box min.
+ * @param boxMax The axis aligned bounding box max.
  * @return True if b intersects f, false otherwise.
  */
 bool intersectFrustumAABB(
@@ -359,9 +382,14 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render() {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping)
+{
 	UASSERT(gesture_camera_ != 0);
 
+	if(currentPose_ == 0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
 	Eigen::Quaternionf quat = currentPose_->getQuaternionf();
 	glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
@@ -386,6 +414,25 @@ int Scene::Render() {
 	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
 	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
 
+	bool renderBackgroundCamera =
+			background_renderer_ &&
+            gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
+            !rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
+            uvsTransformed;
+
+	if(renderBackgroundCamera)
+	{
+        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.3)
+        {
+            projectionMatrix = arProjectionMatrix;
+            viewMatrix = arViewMatrix;
+        }
+        else
+        {
+            renderBackgroundCamera = false;
+        }
+	}
+
 	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
@@ -433,28 +480,45 @@ int Scene::Render() {
 		glDisable(GL_CULL_FACE);
 	}
 
-	UTimer timer;
+	bool onlineBlending =
+        (!meshRendering_ &&
+         occlusionMesh.cloud.get() &&
+         occlusionMesh.cloud->size()) ||
+        (blending_ &&
+         gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho &&
+         mapRendering_ && meshRendering_ &&
+         (cloudsToDraw.size() > 1 || (renderBackgroundCamera && wireFrame_)));
 
-	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
 	if(onlineBlending && fboId_)
 	{
+        GLint originid = 0;
+        glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
+        
 		// set the rendering destination to FBO
 		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
 
-		glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
+		glClearColor(0, 0, 0, 0);
-		glClearColor(1, 1, 1, 1);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
+        
-		// Draw scene
+        // Draw scene
-		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+        for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
-		{
+        {
-			// set large distance to cam to use low res polygons for fast processing
+            Eigen::Vector3f cloudToCamera(
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+                      (*iter)->getPose().x() - openglCamera.x(),
-		}
+                      (*iter)->getPose().y() - openglCamera.y(),
-
+                      (*iter)->getPose().z() - openglCamera.z());
+            float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
+            (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
+        }
+        
+        if(!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
+        {
+            PointCloudDrawable drawable(occlusionMesh);
+            drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 0, 0, 0, 0, 0, 0, true);
+        }
+        
 		// back to normal window-system-provided framebuffer
-		glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
+		glBindFramebuffer(GL_FRAMEBUFFER, originid); // unbind
-		glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
 	}
 
 	if(doubleTapOn_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -462,16 +526,21 @@ int Scene::Render() {
 		glClearColor(0, 0, 0, 0);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
+        // FIXME: we could use the depthTexture if already computed!
 		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
 		{
-			// set large distance to cam to use low res polygons for fast processing
+            Eigen::Vector3f cloudToCamera(
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+                      (*iter)->getPose().x() - openglCamera.x(),
+                      (*iter)->getPose().y() - openglCamera.y(),
+                      (*iter)->getPose().z() - openglCamera.z());
+            float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
+            
+            (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
 		}
 
 		GLubyte zValue[4];
 		glReadPixels(doubleTapPos_.x*screenWidth_, screenHeight_-doubleTapPos_.y*screenHeight_, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, zValue);
-		float fromFixed = 256.0f/255.0f;
+		float zValueF = float(zValue[0]/255.0f) + float(zValue[1]/255.0f)/255.0f + float(zValue[2]/255.0f)/65025.0f + float(zValue[3]/255.0f)/160581375.0f;
-		float zValueF = float(zValue[0]/255.0f)*fromFixed + float(zValue[1]/255.0f)*fromFixed/255.0f + float(zValue[2]/255.0f)*fromFixed/65025.0f + float(zValue[3]/255.0f)*fromFixed/160581375.0f;
 
 		if(zValueF != 0.0f)
 		{
@@ -485,10 +554,19 @@ int Scene::Render() {
 
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+    
+    if(renderBackgroundCamera && (!onlineBlending || !meshRendering_))
+    {
+        background_renderer_->Draw(uvsTransformed, 0, screenWidth_, screenHeight_, false);
+
+        //To debug occlusion image:
+        //PointCloudDrawable drawable(occlusionMesh);
+        //drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+    }
 
 	if(!currentPose_->isNull())
 	{
-		if (gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
 		{
 			frustum_->SetPosition(position);
 			frustum_->SetRotation(rotation);
@@ -497,8 +575,13 @@ int Scene::Render() {
 			frustum_->SetScale(kFrustumScale);
 			frustum_->Render(projectionMatrix, viewMatrix);
 
-			axis_->SetPosition(position);
+			rtabmap::Transform cameraFrame = *currentPose_*rtabmap::optical_T_opengl*rtabmap::CameraMobile::opticalRotationInv;
-			axis_->SetRotation(rotation);
+			glm::vec3 positionCamera(cameraFrame.x(), cameraFrame.y(), cameraFrame.z());
+			Eigen::Quaternionf quatCamera = cameraFrame.getQuaternionf();
+			glm::quat rotationCamera(quatCamera.w(), quatCamera.x(), quatCamera.y(), quatCamera.z());
+
+			axis_->SetPosition(positionCamera);
+			axis_->SetRotation(rotationCamera);
 			axis_->Render(projectionMatrix, viewMatrix);
 		}
 
@@ -507,11 +590,15 @@ int Scene::Render() {
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+        else
+        {
+            trace_->ClearVertexArray();
+        }
+	}
 
-		if(gridVisible_)
+	if(gridVisible_ && !renderBackgroundCamera)
-		{
+	{
-			grid_->Render(projectionMatrix, viewMatrix);
+		grid_->Render(projectionMatrix, viewMatrix);
-		}
 	}
 
 	if(graphVisible_ && graph_)
@@ -547,9 +634,20 @@ int Scene::Render() {
 
 	if(onlineBlending)
 	{
+        if(renderBackgroundCamera && meshRendering_)
+        {
+            background_renderer_->Draw(uvsTransformed, depthTexture_, screenWidth_, screenHeight_, mapping);
+        }
+        
 		glDisable (GL_BLEND);
 		glDepthMask(GL_TRUE);
 	}
+    
+	//draw markers on foreground
+	for(std::map<int, tango_gl::Axis*>::const_iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
+	{
+		iter->second->Render(projectionMatrix, viewMatrix);
+	}
 
 	return (int)cloudsToDraw.size();
 }
@@ -560,8 +658,11 @@ void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {
 
 void Scene::SetCameraPose(const rtabmap::Transform & pose)
 {
-	UASSERT(currentPose_ != 0);
 	UASSERT(!pose.isNull());
+	if(currentPose_ ==0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	*currentPose_ = pose;
 }
 
@@ -589,7 +690,7 @@ rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 	{
 		*fov = gesture_camera_->getFOV();
 	}
-	return glmToTransform(gesture_camera_->GetTransformationMatrix());
+	return rtabmap::glmToTransform(gesture_camera_->GetTransformationMatrix());
 }
 
 void Scene::OnTouchEvent(int touch_count,
@@ -618,18 +719,10 @@ void Scene::updateGraph(
 		const std::multimap<int, rtabmap::Link> & links)
 {
 	LOGI("updateGraph");
-	if(graph_)
-	{
-		delete graph_;
-		graph_ = 0;
-	}
-
 	//create
-	if(graphVisible_)
+    UASSERT(graph_shader_program_ != 0);
-	{
+    delete graph_;
-		UASSERT(graph_shader_program_ != 0);
+    graph_ = new GraphDrawable(graph_shader_program_, poses, links);
-		graph_ = new GraphDrawable(graph_shader_program_, poses, links);
-	}
 }
 
 void Scene::setGraphVisible(bool visible)
@@ -647,7 +740,59 @@ void Scene::setTraceVisible(bool visible)
 	traceVisible_ = visible;
 }
 
+void Scene::setFrustumVisible(bool visible)
+{
+	frustumVisible_ = visible;
+}
+
 //Should only be called in OpenGL thread!
+void Scene::addMarker(
+		int id,
+		const rtabmap::Transform & pose)
+{
+	LOGI("add marker %d", id);
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter == markers_.end())
+	{
+		//create
+		tango_gl::Axis * drawable = new tango_gl::Axis();
+		drawable->SetScale(glm::vec3(0.05f,0.05f,0.05f));
+		drawable->SetLineWidth(5);
+		markers_.insert(std::make_pair(id, drawable));
+	}
+	setMarkerPose(id, pose);
+}
+void Scene::setMarkerPose(int id, const rtabmap::Transform & pose)
+{
+	UASSERT(!pose.isNull());
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter != markers_.end())
+	{
+		glm::vec3 position(pose.x(), pose.y(), pose.z());
+		Eigen::Quaternionf quat = pose.getQuaternionf();
+		glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
+		iter->second->SetPosition(position);
+		iter->second->SetRotation(rotation);
+	}
+}
+bool Scene::hasMarker(int id) const
+{
+	return markers_.find(id) != markers_.end();
+}
+void Scene::removeMarker(int id)
+{
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter != markers_.end())
+	{
+		delete iter->second;
+		markers_.erase(iter);
+	}
+}
+std::set<int> Scene::getAddedMarkers() const
+{
+	return uKeysSet(markers_);
+}
+
 void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -670,7 +815,7 @@ void Scene::addCloud(
 
 void Scene::addMesh(
 		int id,
-		const Mesh & mesh,
+		const rtabmap::Mesh & mesh,
 		const rtabmap::Transform & pose,
 		bool createWireframe)
 {
@@ -789,7 +934,7 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
 	}
 }
 
-void Scene::updateMesh(int id, const Mesh & mesh)
+void Scene::updateMesh(int id, const rtabmap::Mesh & mesh)
 {
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())

app/android/jni/scene.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,9 +37,10 @@
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>
 
-#include <point_cloud_drawable.h>
+#include "point_cloud_drawable.h"
-#include <graph_drawable.h>
+#include "graph_drawable.h"
-#include <bounding_box_drawable.h>
+#include "bounding_box_drawable.h"
+#include "background_renderer.h"
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -60,7 +63,8 @@ class Scene {
   int getViewPortWidth() const {return screenWidth_;}
   int getViewPortHeight() const {return screenHeight_;}
 
-  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
+  rtabmap::ScreenRotation getScreenRotation() const {return color_camera_to_display_rotation_;}
+  void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 
   void clear(); // removed all point clouds
 
@@ -70,15 +74,16 @@ 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 = 0) const;
 
@@ -101,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,
@@ -109,7 +121,7 @@ class Scene {
   		  const rtabmap::Transform & pose);
   void addMesh(
   		int id,
-  		const Mesh & mesh,
+  		const rtabmap::Mesh & mesh,
   		const rtabmap::Transform & pose,
 		bool createWireframe = false);
 
@@ -120,7 +132,7 @@ class Scene {
   bool hasTexture(int id) const;
   std::set<int> getAddedClouds() const;
   void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
-  void updateMesh(int id, const Mesh & mesh);
+  void updateMesh(int id, const rtabmap::Mesh & mesh);
   void updateGains(int id, float gainR, float gainG, float gainB);
 
   void setBlending(bool enabled) {blending_ = enabled;}
@@ -143,6 +155,9 @@ class Scene {
   float getPointSize() const {return pointSize_;}
   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.
@@ -166,8 +181,11 @@ class Scene {
   bool graphVisible_;
   bool gridVisible_;
   bool traceVisible_;
+  bool frustumVisible_;
 
-  TangoSupportRotation color_camera_to_display_rotation_;
+  std::map<int, tango_gl::Axis*> markers_;
+
+  rtabmap::ScreenRotation color_camera_to_display_rotation_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
 
@@ -189,7 +207,8 @@ class Scene {
   float g_;
   float b_;
   GLuint fboId_;
-  GLuint depthTexture_;
+  GLuint rboId_;
+  GLuint depthTexture_; // 0=objects+occlusion
   GLsizei screenWidth_;
   GLsizei screenHeight_;
   bool doubleTapOn_;

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

@@ -24,8 +24,8 @@ Camera::Camera() {
   aspect_ratio_ = 4.0f / 3.0f;
   width_ = 800.0f;
   height_ = 600.0f;
-  near_clip_plane_ = 0.2f;
+  near_clip_plane_ = 0.5f;
-  far_clip_plane_ = 1000.0f;
+  far_clip_plane_ = 50.0f;
   ortho_ = false;
   orthoScale_ = 2.0f;
   orthoCropFactor_ = -1.0f;

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

@@ -18,11 +18,14 @@
 namespace tango_gl {
 
 static const float float_vertices[] = {
-    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  1.0f,
+    0.0f,  0.0f,  0.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,  0.0f,  0.0f,  1.0f, 1.0f,  -1.0f,
-    0.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,
-    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,
-    -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, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
 
 Frustum::Frustum() : Line(3.0f, GL_LINES) {
   SetShader();

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

@@ -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.
+// First person camera's FOV is 85 degrees.
-const float kHighFov = 65.0f;
+const float kHighestFov = 120.0f;
+const float kHighFov = 85.0f;
 const float kLowFov = 65.0f;
+const float kLowestFov = 40.0f;
 }
 
 namespace tango_gl {
@@ -111,7 +113,7 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 
 			if(camera_type_ == kFirstPerson)
 			{
-				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, kLowestFov, kHighestFov));
 			}
 			else
 			{
@@ -179,8 +181,9 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
   camera_type_ = camera_index;
   switch (camera_index) {
     case kFirstPerson:
-    	SetOrthoMode(false);
+      SetOrthoMode(false);
-      SetFieldOfView(kLowFov);
+      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;
@@ -193,14 +196,15 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       break;
     case kThirdPerson:
     case kThirdPersonFollow:
-    	SetOrthoMode(false);
+      SetOrthoMode(false);
-    	SetFieldOfView(kHighFov);
+      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 / 6.0f;
+      cam_cur_angle_.x = -M_PI / 12.0f;
-      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;
@@ -208,7 +212,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       SetOrthoMode(false);
-      SetFieldOfView(kHighFov);
+      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;
@@ -222,6 +227,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
 	  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;

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

@@ -31,13 +31,17 @@ Grid::Grid(float density, int qx, int qy) : Line(1.0f, GL_LINES) {
 
   // Horizontal line.
   for (int i = 0; i < (qy + 1); i++) {
-    vec_vertices_.push_back(glm::vec3(-width, 0.0f, -height + i * density));
+	  for (int j = 0; j < (qx + 1); j++) {
-    vec_vertices_.push_back(glm::vec3(width, 0.0f, -height + i * density));
+		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));
+	  for (int j = 0; j < (qy + 1); j++) {
-    vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, height));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + j*density));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + (j+1)*density));
+	  }
   }
 }
 }  // namespace tango_gl

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

@@ -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_;

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

@@ -21,11 +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>
-#include <tango_support_api.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"
@@ -34,16 +39,28 @@
 #include "glm/gtx/matrix_decompose.hpp"
 
 #define LOG_TAG "rtabmap"
-#ifdef DISABLE_LOG
+#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
@@ -84,32 +101,6 @@ namespace util {
 
   glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec);
 
-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @param display: integer value of display orientation, values available
-  // are 0, 1, 2 ,3. Followed by Android display orientation standard:
-  // https://developer.android.com/reference/android/view/Display.html#getRotation()
-  // @param color_camera: integer value of color camera oreintation, values
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      int display_rotation, int color_camera_rotation);
-
-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @param display: the device display orientation.
-  // @param color_camera: integer value of color camera oreintation, values
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      TangoSupportRotation display_rotation, int color_camera_rotation);
-
 }  // namespace util
 }  // namespace tango_gl
 #endif  // TANGO_GL_RENDERER_GL_UTIL

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

@@ -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 {

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

@@ -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() {

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

@@ -238,23 +238,4 @@ glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
   return glm::vec3(mat * glm::vec4(vec, 1.0f));
 }
 
-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    int display_rotation, int color_camera_rotation) {
-  TangoSupportRotation r =
-      static_cast<TangoSupportRotation>(display_rotation);
-  return util::GetAndroidRotationFromColorCameraToDisplay(
-      r, color_camera_rotation);
-}
-
-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    TangoSupportRotation display_rotation, int color_camera_rotation) {
-  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
-
-  int ret = static_cast<int>(display_rotation) - color_camera_n;
-  if (ret < 0) {
-    ret += 4;
-  }
-  return static_cast<TangoSupportRotation>(ret % 4);
-}
-
 }  // namespace tango_gl

app/android/jni/util.h

@@ -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,13 +37,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/Vertices.h>
 #include <pcl/pcl_base.h>
 
+namespace rtabmap {
+
 class LogHandler : public UEventsHandler
 {
 public:
 	LogHandler()
 	{
-		ULogger::setLevel(ULogger::kDebug);
+#ifdef DISABLE_LOG
-		ULogger::setEventLevel(ULogger::kDebug);
+		ULogger::setLevel(ULogger::kWarning);
+		ULogger::setEventLevel(ULogger::kWarning);
+#else
+        ULogger::setLevel(ULogger::kDebug);
+        ULogger::setEventLevel(ULogger::kDebug);
+#endif
 		ULogger::setPrintThreadId(true);
 
 		registerToEventsManager();
@@ -53,31 +58,39 @@ public:
 protected:
 	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,
@@ -88,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(
+static const rtabmap::Transform rtabmap_world_T_opengl_world(
-		 0.0f, 0.0f, -1.0f, 0.0f,
+		 0.0f,  0.0f,-1.0f, 0.0f,
-		-1.0f, 0.0f,  0.0f, 0.0f,
+		-1.0f,  0.0f, 0.0f, 0.0f,
-		 0.0f, 1.0f,  0.0f, 0.0f);
+		 0.0f,  1.0f, 0.0f, 0.0f);
 
 inline glm::mat4 glmFromTransform(const rtabmap::Transform & transform)
 {
@@ -173,4 +191,81 @@ public:
 	cv::Mat texture;
 };
 
+typedef enum {
+  /// Not apply any rotation.
+  ROTATION_IGNORED = -1,
+
+  /// 0 degree rotation (natural orientation)
+  ROTATION_0 = 0,
+
+  /// 90 degree rotation.
+  ROTATION_90 = 1,
+
+  /// 180 degree rotation.
+  ROTATION_180 = 2,
+
+  /// 270 degree rotation.
+  ROTATION_270 = 3
+} ScreenRotation;
+
+inline int NormalizedColorCameraRotation(int camera_rotation) {
+  int camera_n = 0;
+  switch (camera_rotation) {
+    case 90:
+      camera_n = 1;
+      break;
+    case 180:
+      camera_n = 2;
+      break;
+    case 270:
+      camera_n = 3;
+      break;
+    default:
+      camera_n = 0;
+      break;
+  }
+  return camera_n;
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @param display: the device display orientation.
+// @param color_camera: integer value of color camera oreintation, values
+// available are 0, 90, 180, 270. Followed by Android camera orientation
+// standard:
+// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
+		ScreenRotation display_rotation, int color_camera_rotation) {
+  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
+
+  int ret = static_cast<int>(display_rotation) - color_camera_n;
+  if (ret < 0) {
+    ret += 4;
+  }
+  return static_cast<ScreenRotation>(ret % 4);
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @param display: integer value of display orientation, values available
+// are 0, 1, 2 ,3. Followed by Android display orientation standard:
+// https://developer.android.com/reference/android/view/Display.html#getRotation()
+// @param color_camera: integer value of color camera 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_ */

app/android/libs/.gitignore

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

app/android/res/layout/activity_rtabmap.xml

@@ -27,6 +27,26 @@
         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_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"
@@ -80,16 +100,6 @@
         android:text="@string/camera_button"
         android:spinnerMode="dropdown"/>
 
-    <ToggleButton
-        android:id="@+id/pause_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-		android:layout_marginRight="5dp"
-        android:layout_alignParentTop="true"
-        android:layout_marginTop="10dp"
-        android:layout_alignParentRight="true"
-        android:textOff="@string/pause"
-        android:textOn="@string/resume" />
 
     <Button
         android:id="@+id/button_shareToSketchfab"
@@ -138,7 +148,23 @@
         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>

app/android/res/layout/activity_settings.xml

@@ -1,5 +1,13 @@
 <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 
@@ -78,7 +86,7 @@
             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"
@@ -90,6 +98,18 @@
 	            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"
@@ -191,6 +211,20 @@
 		    		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">
@@ -209,6 +243,11 @@
 		            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"

app/android/res/layout/activity_sketchfab.xml

@@ -62,20 +62,11 @@
         android:checked="true"
         android:text="Draft Mode" />
 
-    <LinearLayout
+    <Button
+        android:id="@+id/button_ok"
         android:layout_width="match_parent"
-        android:layout_height="0dip"
+        android:layout_height="wrap_content"
-        android:layout_weight="1"
+        android:text="Upload" />
-        android:gravity="right|bottom"
-        android:orientation="horizontal" >
-
-        <Button
-            android:id="@+id/button_ok"
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:layout_weight="0.06"
-            android:text="Upload" />
-    </LinearLayout>
 
 </LinearLayout>
 </ScrollView>

app/android/res/menu/optionmenu.xml

@@ -7,26 +7,12 @@
 		  <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/new_scan" android:title="New Scan"/>
-       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
+       <item android:id="@+id/open" android:title="Library" android:showAsAction="ifRoom"/>
-          <menu>
+	   <item android:id="@+id/save" android:title="Save"/>
-              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
-                  <menu>
-                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
-                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
-                  </menu>
-              </item>
-	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
-			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
-		          </menu>
-	          </item>
-          </menu>
-       </item>
        
-        <item android:id="@+id/post_processing" android:title="Optimize" android:showAsAction="ifRoom">
+        <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..." >
@@ -43,7 +29,22 @@
           </menu>
        </item>
        
-       <item android:id="@+id/open" android:title="Open" android:showAsAction="ifRoom"/>
+       <item android:id="@+id/export" android:title="Assemble">
+          <menu>
+              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
+                  <menu>
+                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
+                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
+                  </menu>
+              </item>
+	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
+	              <menu>
+		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
+			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
+		          </menu>
+	          </item>
+          </menu>
+       </item>
        
        <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
@@ -68,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>

app/android/res/values/dimens.xml

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

app/android/res/values/strings.xml

@@ -9,15 +9,17 @@
     <string name="status">"Status: "</string>
     <string name="words">"Words: "</string>
     <string name="camera_button">First View</string>
-    <string name="pause">Pause</string>
+    <string name="library">Library</string>
-    <string name="resume">Resume</string>
+    <string name="new_scan">New Scan</string>
+    <string name="pause">Stop</string>
+    <string name="resume">Start</string>
     <string name="backface_on">Backface</string>
     <string name="backface_off">Backface</string>
     <string name="light_on">Lighting</string>
     <string name="light_off">Lighting</string>
     <string name="wireframe">Wireframe</string>
     <string name="close_visualization">Close Visualization</string>
-    <string name="save_to_file">Export to File&#8230;</string>
+    <string name="save_to_file">Export OBJ/PLY&#8230;</string>
     <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
     <string name="start">Start</string>
     <string name="nodes">"Nodes (WM): "</string>
@@ -36,6 +38,7 @@
     <string name="fps">"FPS (rendering): "</string>
     <string name="distance">"Distance travelled: "</string>
     <string name="gps">"GPS (long,lat,alt,bearing,err): "</string>
+    <string name="env_sensors">"Sensors: "</string>
     <string name="time">"Time: "</string>
     
     <!-- Preference keys: BEGIN -->
@@ -48,7 +51,7 @@
     <string name="pref_key_remove_button">pref_key_remove_button</string>
     <string name="pref_key_reset_button">pref_key_reset_button</string>
     <string name="pref_key_density">pref_key_density</string>
-    <string name="pref_default_density">1</string>
+    <string name="pref_default_density">2</string>
     <string name="pref_key_min_depth">pref_key_min_depth</string>
     <string name="pref_default_min_depth">0</string>
     <string name="pref_key_depth">pref_key_depth</string>
@@ -72,16 +75,22 @@
     <string name="pref_key_resolution">pref_key_resolution</string>
     <string name="pref_default_resolution">false</string>
     <string name="pref_key_smoothing">pref_key_smoothing</string>
-    <string name="pref_default_smoothing">true</string>
+    <string name="pref_default_smoothing">false</string>
     <string name="pref_key_fisheye">pref_key_fisheye</string>
     <string name="pref_default_fisheye">false</string>
     
+    <string name="pref_key_camera_driver">pref_key_camera_driver</string>
+    <string name="pref_default_camera_driver">-1</string>
+    <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
+    <string name="pref_default_depth_from_motion">false</string>
+    <string name="pref_key_arcore_localization_filtering_speed">pref_key_arcore_localization_filtering_speed</string>
+    <string name="pref_default_arcore_localization_filtering_speed">1</string>
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
     <string name="pref_key_max_speed">pref_key_max_speed</string>
     <string name="pref_default_max_speed">0</string>
     <string name="pref_key_time_thr">pref_key_time_thr</string>
-    <string name="pref_default_time_thr">1000</string>
+    <string name="pref_default_time_thr">0</string>
     <string name="pref_key_mem_thr">pref_key_mem_thr</string>
     <string name="pref_default_mem_thr">0</string>
     <string name="pref_key_loop_thr">pref_key_loop_thr</string>
@@ -91,7 +100,7 @@
     <string name="pref_key_min_inliers">pref_key_min_inliers</string>
     <string name="pref_default_min_inliers">25</string>
     <string name="pref_key_opt_error">pref_key_opt_error</string>
-    <string name="pref_default_opt_error">2</string>
+    <string name="pref_default_opt_error">1</string>
     <string name="pref_key_features_voc">pref_key_features_voc</string>
     <string name="pref_default_features_voc">200</string>
     <string name="pref_key_features">pref_key_features</string>
@@ -102,12 +111,18 @@
     <string name="pref_default_optimizer">2</string>
     <string name="pref_key_optimize_end">pref_key_optimize_end</string>
     <string name="pref_default_optimize_end">true</string>
+    <string name="pref_key_marker_detection">pref_key_marker_detection</string>
+    <string name="pref_default_marker_detection">-1</string>
+    <string name="pref_key_marker_detection_depth_error">pref_key_marker_detection_depth_error</string>
+    <string name="pref_default_marker_detection_depth_error">0.1</string>
     <string name="pref_key_keep_all_db">pref_key_keep_all_db</string>
     <string name="pref_default_keep_all_db">true</string>
     <string name="pref_key_raw_scan_saved">pref_key_raw_scan_saved</string>
     <string name="pref_default_raw_scan_saved">false</string>
     <string name="pref_key_gps_saved">pref_key_gps_saved</string>
     <string name="pref_default_gps_saved">false</string>
+    <string name="pref_key_env_sensors_saved">pref_key_env_sensors_saved</string>
+    <string name="pref_default_env_sensors_saved">false</string>
     <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
     <string name="pref_default_db_in_memory">false</string>
     
@@ -149,7 +164,7 @@
     <string name="pref_title_triangle">Mesh Triangle Size</string>
     <string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string>
     <string name="pref_title_rendering_texture_decimation">Texture Resolution</string>
-    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string>
+    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Assembling results.</string>
     <string name="pref_title_min_depth">Min Depth</string>
     <string name="pref_summary_min_depth">Points under the minimum depth are not rendered.</string>
     <string name="pref_title_depth">Max Depth</string>
@@ -310,6 +325,12 @@
     <string name="pref_summary_mapping">Advanced mapping parameters for fine tuning.</string>
     <string name="pref_title_mapping_core">Core</string>
     <string name="pref_title_mapping_database">Database</string>
+    <string name="pref_title_camera_driver">Camera Driver</string>
+    <string name="pref_summary_camera_driver">AR sdk used for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
+    <string name="pref_title_depth_from_motion">Depth From Motion</string>
+    <string name="pref_summary_depth_from_motion">Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver.</string>
+    <string name="pref_title_arcore_localization_filtering_speed">ARCore Localization Filtering Speed</string>
+    <string name="pref_summary_arcore_localization_filtering_speed">Filter ARCore\'s localizations to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set a speed threshold to detect those events.</string>
     <string name="pref_title_append">Append Mode</string>
     <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
     <string name="pref_title_resolution">HD Mode</string>
@@ -344,15 +365,35 @@
     <string name="pref_summary_optimizer">Graph optimization approach.</string>
     <string name="pref_title_optimize_end">Optimization from Graph End</string>
     <string name="pref_summary_optimize_end">The map\'s graph is optimized from the last node. The map is moved when a loop closure happens instead of jumping the current pose back to localized area. </string>
+    <string name="pref_title_marker_detection">ArUco Marker Detection</string>
+    <string name="pref_summary_marker_detection">ArUco markers can be detected for localization and graph optimization.</string>
+    <string name="pref_title_marker_detection_depth_error">Marker Depth Error Estimation</string>
+    <string name="pref_summary_marker_detection_depth_error">Size of markers are automatically initialized on the first marker seen. All markers should have the same size. This value is the maximum depth error to do the initialization to get accurate size of the tag. The lower it is, the more perpendicular the camera should be from the marker to do initialization, but size estimated would be more accurate.</string>
     <string name="pref_title_keep_all_db">Save All Frames in Database</string>
     <string name="pref_summary_keep_all_db">Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop.</string>
     <string name="pref_title_raw_scan_saved">Save Raw Scan</string>
     <string name="pref_summary_raw_scan_saved">Save raw point clouds in database.</string>
     <string name="pref_title_gps_saved">Save GPS</string>
-    <string name="pref_summary_gps_saved">Save GPS in database.</string>
+    <string name="pref_summary_gps_saved">Save GPS to database.</string>
-    <string name="pref_title_db_in_memory">Database In Memory</string>
+    <string name="pref_title_env_sensors_saved">Save Environmental Sensors</string>
+    <string name="pref_summary_env_sensors_saved">Save Wifi strength, temperature, air pressure, light intensity and relative humidity to database.</string>
+    <string name="pref_title_db_in_memory">Database in Memory</string>
     <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
 
+    <string-array name="pref_camera_driver_keys">
+        <item>"Auto"</item>
+        <item>"Google Tango NDK"</item>
+        <item>"ARCore NDK"</item>
+        <item>"AREngine NDK"</item>
+        <item>"ARCore Java"</item>
+    </string-array>
+    <string-array name="pref_camera_driver_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+    </string-array>
     <string-array name="pref_update_rate_keys">
         <item>"Max"</item>
         <item>"5 Hz"</item>
@@ -371,6 +412,24 @@
         <item>"1"</item>
         <item>"0.5"</item>
     </string-array>
+    <string-array name="pref_arcore_localization_filtering_speed_keys">
+        <item>"Disabled"</item>
+        <item>"5 m/s"</item>
+        <item>"4 m/s"</item>
+        <item>"3 m/s"</item>
+        <item>"2 m/s"</item>
+        <item>"1 m/s"</item>
+        <item>"0.5 m/s"</item>
+    </string-array>
+    <string-array name="pref_arcore_localization_filtering_speed_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0.5"</item>
+    </string-array>
     <string-array name="pref_max_speed_keys">
         <item>"No Limit"</item>
         <item>"High"</item>
@@ -398,7 +457,7 @@
         <item>"700 ms"</item>
         <item>"600 ms"</item>
         <item>"500 ms"</item>
-        <item>"400"</item>
+        <item>"400 ms"</item>
     </string-array>
     <string-array name="pref_time_thr_values">
         <item>"0"</item>
@@ -494,29 +553,25 @@
         <item>"10"</item>
     </string-array>
     <string-array name="pref_opt_error_keys">
-        <item>"10x"</item>
-        <item>"9x"</item>
-        <item>"8x"</item>
-        <item>"7x"</item>
-        <item>"6x"</item>
-        <item>"5x"</item>
         <item>"4x"</item>
+        <item>"3.5x"</item>
         <item>"3x"</item>
+        <item>"2.5x"</item>
         <item>"2x"</item>
+        <item>"1.5x"</item>
         <item>"1x"</item>
+        <item>"0.5x"</item>
         <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_opt_error_values">
-        <item>"10"</item>
-        <item>"9"</item>
-        <item>"8"</item>
-        <item>"7"</item>
-        <item>"6"</item>
-        <item>"5"</item>
         <item>"4"</item>
+        <item>"3.5"</item>
         <item>"3"</item>
+        <item>"2.5"</item>
         <item>"2"</item>
+        <item>"1.5"</item>
         <item>"1"</item>
+        <item>"0.5"</item>
         <item>"0"</item>
     </string-array>
     <string-array name="pref_features_voc_keys">
@@ -591,10 +646,95 @@
         <item>"1"</item>
         <item>"0"</item>
     </string-array>
+    <string-array name="pref_marker_detection_keys">
+        <item>"Disabled"</item>
+        <item>"4X4_50"</item>
+        <item>"4X4_100"</item>
+        <item>"4X4_250"</item>
+        <item>"4X4_1000"</item>
+        <item>"5X5_50"</item>
+        <item>"5X5_100"</item>
+        <item>"5X5_250"</item>
+        <item>"5X5_1000"</item>
+        <item>"6X6_50"</item>
+        <item>"6X6_100"</item>
+        <item>"6X6_250"</item>
+        <item>"6X6_1000"</item>
+        <item>"7X7_50"</item>
+        <item>"7X7_100"</item>
+        <item>"7X7_250"</item>
+        <item>"7X7_1000"</item>
+        <item>"ARUCO_ORIGINAL"</item>
+        <item>"APRILTAG_16h5"</item>
+        <item>"APRILTAG_25h9"</item>
+        <item>"APRILTAG_36h10"</item>
+        <item>"APRILTAG_36h11"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+        <item>"4"</item>
+        <item>"5"</item>
+        <item>"6"</item>
+        <item>"7"</item>
+        <item>"8"</item>
+        <item>"9"</item>
+        <item>"10"</item>
+        <item>"11"</item>
+        <item>"12"</item>
+        <item>"13"</item>
+        <item>"14"</item>
+        <item>"15"</item>
+        <item>"16"</item>
+        <item>"17"</item>
+        <item>"18"</item>
+        <item>"19"</item>
+        <item>"20"</item>
+    </string-array>
+    
+    <string-array name="pref_marker_detection_depth_error_keys">
+        <item>"1 cm"</item>
+        <item>"2 cm"</item>
+        <item>"3 cm"</item>
+        <item>"4 cm"</item>
+        <item>"5 cm"</item>
+        <item>"6 cm"</item>
+        <item>"7 cm"</item>
+        <item>"8 cm"</item>
+        <item>"9 cm"</item>
+        <item>"10 cm"</item>
+        <item>"15 cm"</item>
+        <item>"20 cm"</item>
+        <item>"30 cm"</item>
+        <item>"40 cm"</item>
+        <item>"50 cm"</item>
+        <item>"100 cm"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_depth_error_values">
+        <item>"0.01"</item>
+        <item>"0.02"</item>
+        <item>"0.03"</item>
+        <item>"0.04"</item>
+        <item>"0.05"</item>
+        <item>"0.06"</item>
+        <item>"0.07"</item>
+        <item>"0.08"</item>
+        <item>"0.09"</item>
+        <item>"0.1"</item>
+        <item>"0.15"</item>
+        <item>"0.20"</item>
+        <item>"0.30"</item>
+        <item>"0.40"</item>
+        <item>"0.50"</item>
+        <item>"1"</item>
+    </string-array>
 
-    <string name="pref_title_export_sub">Exporting&#8230;</string>
+    <string name="pref_title_export_sub">Assembling&#8230;</string>
-    <string name="pref_title_export">Exporting</string>
+    <string name="pref_title_export">Assembling</string>
-    <string name="pref_summary_export">Advanced parameters used when exporting the map.</string>
+    <string name="pref_summary_export">Advanced parameters used when assembling the map.</string>
     <string name="pref_title_cloud_voxel">Voxel Size</string>
     <string name="pref_summary_cloud_voxel">If you don\'t need a very precise point cloud, you can set this to reduce the output point cloud size. This is also used for optimized mesh.</string>
     <string name="pref_title_texture_size">Texture Size</string>
@@ -607,8 +747,8 @@
     <string name="pref_summary_max_texture_distance">Maximum distance from a camera for polygons to be textured by this camera.</string>
     <string name="pref_title_min_texture_cluster_size">Min Texture Cluster Size</string>
     <string name="pref_summary_min_texture_cluster_size">Minimum polygon cluster size to be textured by a camera. This helps to filter sparse textured polygons.</string>
-    <string name="pref_title_block_render">Block Rendering Thread While Exporting</string>
+    <string name="pref_title_block_render">Block Rendering Thread While Assembling</string>
-    <string name="pref_summary_block_render">This decreases exporting time, but freezes rendering while exporting. This also clears temporary the rendered clouds/meshes from memory during exporting, this can be useful to avoid out of memory errors.</string>
+    <string name="pref_summary_block_render">This decreases assembling time, but freezes rendering while assembling. This also clears temporary the rendered clouds/meshes from memory during assembling, this can be useful to avoid out of memory errors.</string>
 
     <string-array name="pref_cloud_voxel_keys">
         <item>"0.2 m"</item>
@@ -714,7 +854,7 @@
     <string name="pref_title_opt_depth">Reconstruction Depth</string>
     <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth). \"Auto\" means that depth is chosen so that polygon size is just under 3 cm.</string>
     <string name="pref_title_opt_color_radius">Color Radius</string>
-    <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
+    <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When assembling with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
     <string name="pref_title_opt_clean_white">Clean Mesh</string>
     <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
 	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>
@@ -748,10 +888,10 @@
         <item>"0.2 m"</item>
         <item>"0.1 m"</item>
         <item>"0.05 m"</item>
-        <item>"0.025"</item>
+        <item>"0.025 m"</item>
-        <item>"0.02"</item>
+        <item>"0.02 m"</item>
-        <item>"0.015"</item>
+        <item>"0.015 m"</item>
-        <item>"0.01"</item>
+        <item>"0.01 m"</item>
         <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_opt_color_radius_values">

app/android/res/xml/provider_paths.xml

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

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

@@ -0,0 +1,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;
+	}
+
+}

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

@@ -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;
+  }
+}

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

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

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

@@ -1,5 +1,10 @@
 
 package com.introlab.rtabmap;
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+
+import android.app.Activity;
+import android.content.Context;
 import android.os.IBinder;
 import android.view.KeyEvent;
 import android.util.Log;
@@ -15,7 +20,7 @@ public class RTABMapLib
         // the correct library first.
         if (TangoInitializationHelper.loadTangoSharedLibrary() ==
                 TangoInitializationHelper.ARCH_ERROR) {
-            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+            Log.w(RTABMapActivity.class.getSimpleName(), "WArning! Unable to load libtango_client_api.so! This can be safely ignored if RTAB-Map NDK is not build with tango support.");
         }
         System.loadLibrary("NativeRTABMap");
     }
@@ -23,87 +28,86 @@ 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 setScreenRotation(int displayRotation, int cameraRotation);
+    public static native void destroyNativeApplication(long nativeApplication);
     
-    public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
-    public static native int openDatabase2(String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
     
-    /*
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize, boolean clearDatabase);
-     * Called when the Tango service is connected.
+    
-     *
+    public static native boolean recover(long nativeApplication, String from, String to);
-     * @param binder The native binder object.
-     */
-    public static native boolean onTangoServiceConnected(IBinder binder);
 
-    // Release all non OpenGl resources that are allocated from the program.
+    public static native boolean isBuiltWith(long nativeApplication, int cameraDriver);
-    public static native void onPause();
+    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 setPausedMapping(boolean paused);
+    public static native void setOnlineBlending(long nativeApplication, boolean enabled);
-    public static native void setOnlineBlending(boolean enabled);
+    public static native void setMapCloudShown(long nativeApplication, boolean shown);
-    public static native void setMapCloudShown(boolean shown);
+    public static native void setOdomCloudShown(long nativeApplication, boolean shown);
-    public static native void setOdomCloudShown(boolean shown);
+    public static native void setMeshRendering(long nativeApplication, boolean enabled, boolean withTexture);
-    public static native void setMeshRendering(boolean enabled, boolean withTexture);
+    public static native void setLocalizationMode(long nativeApplication, boolean enabled);
-    public static native void setLocalizationMode(boolean enabled);
+    public static native void setTrajectoryMode(long nativeApplication, boolean enabled);
-    public static native void setTrajectoryMode(boolean enabled);
+    public static native void setGraphOptimization(long nativeApplication, boolean enabled);
-    public static native void setGraphOptimization(boolean enabled);
+    public static native void setNodesFiltering(long nativeApplication, boolean enabled);
-    public static native void setNodesFiltering(boolean enabled);
+    public static native void setGraphVisible(long nativeApplication, boolean visible);
-    public static native void setGraphVisible(boolean visible);
+    public static native void setGridVisible(long nativeApplication, boolean visible);
-    public static native void setGridVisible(boolean visible);
+    public static native void setRawScanSaved(long nativeApplication, boolean enabled);
-    public static native void setRawScanSaved(boolean enabled);
+    public static native void setFullResolution(long nativeApplication, boolean enabled);
-    public static native void setFullResolution(boolean enabled);
+    public static native void setSmoothing(long nativeApplication, boolean enabled);
-    public static native void setSmoothing(boolean enabled);
+    public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
-    public static native void setCameraColor(boolean enabled);
+    public static native void setCameraColor(long nativeApplication, boolean enabled);
-    public static native void setAppendMode(boolean enabled);
+    public static native void setAppendMode(long nativeApplication, boolean enabled);
-    public static native void setDataRecorderMode(boolean enabled);
+    public static native void setDataRecorderMode(long nativeApplication, boolean enabled);
-    public static native void setMaxCloudDepth(float value);
+    public static native void setMaxCloudDepth(long nativeApplication, float value);
-    public static native void setMinCloudDepth(float value);
+    public static native void setMinCloudDepth(long nativeApplication, float value);
-    public static native void setPointSize(float value);
+    public static native void setPointSize(long nativeApplication, float value);
-    public static native void setFOV(float value);
+    public static native void setFOV(long nativeApplication, float value);
-    public static native void setOrthoCropFactor(float value);
+    public static native void setOrthoCropFactor(long nativeApplication, float value);
-    public static native void setGridRotation(float value);
+    public static native void setGridRotation(long nativeApplication, float value);
-    public static native void setLighting(boolean enabled);
+    public static native void setLighting(long nativeApplication, boolean enabled);
-    public static native void setBackfaceCulling(boolean enabled);
+    public static native void setBackfaceCulling(long nativeApplication, boolean enabled);
-    public static native void setWireframe(boolean enabled);
+    public static native void setWireframe(long nativeApplication, boolean enabled);
-    public static native void setCloudDensityLevel(int value);
+    public static native void setCloudDensityLevel(long nativeApplication, int value);
-    public static native void setMeshAngleTolerance(float value);
+    public static native void setMeshAngleTolerance(long nativeApplication, float value);
-    public static native void setMeshTriangleSize(int value);
+    public static native void setMeshTriangleSize(long nativeApplication, int value);
-    public static native void setClusterRatio(float value);
+    public static native void setClusterRatio(long nativeApplication, float value);
-    public static native void setMaxGainRadius(float value);
+    public static native void setMaxGainRadius(long nativeApplication, float value);
-    public static native void setRenderingTextureDecimation(int value);
+    public static native void setRenderingTextureDecimation(long nativeApplication, int value);
-    public static native void setBackgroundColor(float gray);
+    public static native void setBackgroundColor(long nativeApplication, float gray);
-    public static native int setMappingParameter(String key, String value);
+    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 resetMapping();
+    public static native void save(long nativeApplication, String outputDatabasePath);
-    public static native void save(String outputDatabasePath);
+    public static native void cancelProcessing(long nativeApplication);
-    public static native void cancelProcessing();
     public static native boolean exportMesh(
+    		long nativeApplication, 
     		float cloudVoxelSize,
     		boolean regenerateCloud,
     		boolean meshing,
@@ -120,15 +124,41 @@ public class RTABMapLib
     		float optimizedMaxTextureDistance,
     		int optimizedMinTextureClusterSize,
     		boolean blockRendering);
-    public static native boolean writeExportedMesh(String directory, String name);
+    public static native boolean writeExportedMesh(long nativeApplication, String directory, String name);
-    public static native boolean postExportation(boolean visualize);
+    public static native boolean postExportation(long nativeApplication, boolean visualize);
-    public static native int postProcessing(int approach);
+    public static native int postProcessing(long nativeApplication, int approach);
     
-    public static native String getStatus();
+    public static native String getStatus(long nativeApplication);
-    public static native int getTotalNodes();
+    public static native int getTotalNodes(long nativeApplication);
-    public static native int getTotalWords();
+    public static native int getTotalWords(long nativeApplication);
-    public static native int getTotalPoints();
+    public static native int getTotalPoints(long nativeApplication);
-    public static native float getUpdateTime();
+    public static native float getUpdateTime(long nativeApplication);
-    public static native int getLoopClosureId();
+    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
     
 }

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

@@ -43,12 +43,15 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private float mSurfaceHeight = 0.0f;
 	private float mTextColor = 1.0f;
 	private int mOffset = 0;
+	private ARCoreSharedCamera mCamera = null;
+	private DisplayRotationHelper mDisplayRotationHelper = null;
 	
 	private Vector<TextObject> mTexts;
 
 	private static RTABMapActivity mActivity;
 	public Renderer(RTABMapActivity c) {
 		mActivity = c;
+		mDisplayRotationHelper = new DisplayRotationHelper(/*context=*/ c);
 	}
 
 	private ProgressDialog mProgressDialog = null;
@@ -71,91 +74,129 @@ public class Renderer implements GLSurfaceView.Renderer {
 	{
 		mOffset = offset;
 	}
+	
+	public void setCamera(ARCoreSharedCamera camera)
+	{
+		mCamera = camera;
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.onDisplayChanged(0);
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
+	}
 
 	// Render loop of the Gl context.
 	public void onDrawFrame(GL10 useGLES20instead) {
 
-		try
+		synchronized (this) {
-		{
+			if(mActivity.nativeApplication != 0)
-			final int value = RTABMapLib.render();
-
-			if(mTextManager!=null)
 			{
-				if(mTextChanged)
+				int step = 0;
+				try
 				{
-					mTextChanged = false;
+					if(mCamera!=null)
-					Vector<TextObject> txtcollection = new Vector<TextObject>();
-					
-					mTextLock.lock();
-				    try {
-				    	if(mTexts.size() > 0)
-				    	{
-				    		txtcollection.addAll(mTexts);
-				    	}
-				    } finally {
-				    	mTextLock.unlock();
-				    }
-				    
-					// Prepare the text for rendering
-					mTextManager.PrepareDraw(txtcollection);
-				}
-				
-				float[] mvp = new float[16];
-				Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
-				mTextManager.Draw(mvp);
-			}
-
-			if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
-						mProgressDialog.dismiss();
-						mActivity.resetNoTouchTimer();
-					}
-				});
-			}
-			if(value==-1)
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(mToast!=null)
-						{
-							mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
-						}
-					}
-				});
-			}
-			else if(value==-2)
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(mToast!=null)
-						{
-							mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
-						}
-					}
-				});
-			}
-		}
-		catch(final Exception e)
-		{
-			mActivity.runOnUiThread(new Runnable() {
-				public void run() {
-					if(mToast!=null)
 					{
-						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+						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) {
 		
-		RTABMapLib.setupGraphic(width, height);
+		if(mActivity.nativeApplication!=0)
+		{
+			RTABMapLib.setupGraphic(mActivity.nativeApplication, width, height);
+		}
+		
+		mDisplayRotationHelper.onSurfaceChanged(width, height);
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
 		
 		mSurfaceHeight = (float)height;
 
@@ -180,7 +221,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 	// Called when the surface is created or recreated.
 	public void onSurfaceCreated(GL10 useGLES20instead, EGLConfig config) {
 
-		RTABMapLib.initGlContent();
+		if(mActivity.nativeApplication != 0)
+		{
+			RTABMapLib.initGlContent(mActivity.nativeApplication);
+		}
 		
 		// Create our text manager
 		mTextManager = new TextManager(mActivity);

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

@@ -7,11 +7,14 @@ 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;
@@ -19,6 +22,7 @@ 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 {
 	
@@ -34,7 +38,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	@Override
         	public boolean onPreferenceClick(Preference preference) {   
         		getPreferenceScreen().getSharedPreferences().edit().clear().commit();
-        		
+        	
                 recreate();
         		
         		return true;
@@ -184,7 +188,31 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	}
         });
         
+        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));
@@ -194,6 +222,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((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));
@@ -206,6 +235,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((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));
@@ -226,6 +257,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         Preference pref = findPreference(key);
 
         if (pref instanceof ListPreference) {
+        	if(key.compareTo(getString(R.string.pref_key_camera_driver))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_camera_driver));
         	if(key.compareTo(getString(R.string.pref_key_density))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_density));
         	if(key.compareTo(getString(R.string.pref_key_depth))==0) 
     		{
@@ -253,6 +285,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	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));
@@ -265,6 +298,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	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));
@@ -327,4 +362,28 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
 		}
 		ed.commit(); //save it.
     }
+    
+    @Override
+	public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
+
+		switch (requestCode) {
+			case PermissionHelper.ACCESS_FINE_LOCATION_CODE: {
+				// If request is cancelled, the result arrays are empty.
+				if (results.length > 0 && results[0] == PackageManager.PERMISSION_GRANTED) {
+					// permission was granted, yay! Do the
+					// contacts-related task you need to do.
+					Preference buttonGPS = findPreference(getString(R.string.pref_key_gps_saved));
+					((CustomSwitchPreference)buttonGPS).setChecked(true);
+				} else {
+					// permission denied, boo! Disable the
+					// functionality that depends on this permission.
+					Toast.makeText(this, "Location permission is needed to use GPS functionality", Toast.LENGTH_LONG).show();
+					if (!PermissionHelper.shouldShowRequestPermissionRationale(this, Manifest.permission.ACCESS_FINE_LOCATION)) {
+						PermissionHelper.launchPermissionSettings(this); // Permission denied with checking "Do not ask again".
+					}
+				}
+				return;
+			}
+		}
+	}
 }

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

@@ -66,6 +66,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		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);
@@ -142,7 +143,8 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		if(!isNetworkAvailable())
 		{
 			// Visualize the result?
-			new AlertDialog.Builder(this)
+			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() {
@@ -154,7 +156,9 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 				public void onClick(DialogInterface dialog, int which) {
 				}
 			})
-			.show();
+			.create();
+			ad.setCanceledOnTouchOutside(false);
+			ad.show();
 			return;
 		}	
 
@@ -165,6 +169,8 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 
 			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);
@@ -200,7 +206,6 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 			});
 			mAuthDialog.show();
 			mAuthDialog.setTitle("Authorize RTAB-Map");
-			mAuthDialog.setCancelable(true);
 			web.loadUrl(auth_url);
 		}
 		else
@@ -213,6 +218,8 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	{		
 		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() {
@@ -238,7 +245,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 					File exportDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_EXPORT_DIR);
 					exportDir.mkdirs();
 					
-					if(RTABMapLib.writeExportedMesh(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
+					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
@@ -308,7 +315,10 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 											// do nothing...
 										}
 									});
-									builder.show();
+									AlertDialog ad = builder.create();
+									ad.setCancelable(false);
+									ad.setCanceledOnTouchOutside(false);
+									ad.show();
 								}
 							});
 						}
@@ -371,6 +381,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 						finish();
 					}
 				}).create();
+				d.setCanceledOnTouchOutside(false);
 				d.show();
 				((TextView)d.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
 			}

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

@@ -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);
+            }
+        }
+    }
+
+}
+

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

@@ -17,6 +17,7 @@ import android.graphics.Typeface;
 import android.opengl.GLES20;
 import android.opengl.GLUtils;
 import android.text.TextPaint;
+import android.util.Log;
 
 public class TextManager {
 	
@@ -50,7 +51,7 @@ public class TextManager {
 	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 = '~'+1;
+	public static final char RI_TEXT_STOP = '\u00B0'+1;
 	
 	public float getMaxTextHeight() {return mTextHeight;}
 	
@@ -336,6 +337,8 @@ public class TextManager {
 		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++)
 		{

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

@@ -7,7 +7,11 @@ import java.io.FileInputStream;
 import java.io.FileOutputStream;
 import java.io.FilenameFilter;
 import java.io.IOException;
+import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 import java.util.zip.ZipEntry;
 import java.util.zip.ZipOutputStream;
 
@@ -55,7 +59,7 @@ public class Util {
 		}
 	}
 
-	public static String[] loadFileList(String directory, final boolean databasesOnly) {
+	public static String[] loadFileList(final String directory, final boolean databasesOnly) {
 		File path = new File(directory); 
 		String fileList[];
 		try {
@@ -83,6 +87,25 @@ public class Util {
 			};
 			fileList = path.list(filter);
 			Arrays.sort(fileList);
+			List<String> fileListt = new ArrayList<String>(Arrays.asList(fileList));
+			Collections.sort(fileListt, new Comparator<String>() {
+
+		        @Override
+		        public int compare(String filename1, String filename2) {
+		        	File file1 = new File(directory+"/"+filename1);
+		        	File file2 = new File(directory+"/"+filename2);
+		            long k = file1.lastModified() - file2.lastModified();
+		            if(k > 0){
+		               return -1;
+		            }else if(k == 0){
+		               return 0;
+		            }else{
+		              return 1;
+		           }
+		        }
+		    });
+			fileListt.toArray(fileList);
+			
 		}
 		else {
 			fileList = new String[0];

app/ios/.gitignore

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

app/ios/RTABMapApp.xcodeproj/project.xcworkspace/contents.xcworkspacedata

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Workspace
+   version = "1.0">
+   <FileRef
+      location = "self:">
+   </FileRef>
+</Workspace>

app/ios/RTABMapApp.xcodeproj/project.xcworkspace/xcshareddata/IDEWorkspaceChecks.plist

@@ -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>

app/ios/RTABMapApp.xcodeproj/project.xcworkspace/xcshareddata/swiftpm/Package.resolved

@@ -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
+}

app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme

@@ -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>

app/ios/RTABMapApp/.gitignore

@@ -0,0 +1 @@
+Libraries

app/ios/RTABMapApp/AppDelegate.swift

@@ -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.
+    }
+}
+

app/ios/RTABMapApp/Assets.xcassets/AccentColor.colorset/Contents.json

@@ -0,0 +1,11 @@
+{
+  "colors" : [
+    {
+      "idiom" : "universal"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

app/ios/RTABMapApp/Assets.xcassets/AppIcon.appiconset/Contents.json

@@ -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
+  }
+}