merged master->jazzy

We are shortly going to be removing octomap the package
from ROS 2 Rolling; that's because it's ABI conflicts with
the system package.  Instead, switch rtabmap to use the system
package, which should work fine.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>

.appveyor.yml

@@ -17,6 +17,9 @@ init:
   - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
 
 install:
+  # To download from google drive
+  - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH%
+  - ps: py -m pip --disable-pip-version-check install gdown>=5.1.0 
   # Qt
   - set QTDIR=C:\Qt\5.10.1\msvc2015_64
   # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
@@ -73,7 +76,7 @@ install:
   - ps: "ls \"C:/Program Files/PCL\""
   - set PATH=%PATH%;C:\Program Files\PCL\bin
   # zlib
-  - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
+  - ps: gdown -q 0B46akLGdg-uaYm9MTTI4MUtUcmc
   - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
   - ECHO "Installed zlib:"
   - ps: "ls \"C:/Program Files/zlib\""

.devcontainer/android/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:android-deps",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
+        }
+      }
+}

.devcontainer/bionic/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:18.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

.devcontainer/focal/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:20.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

.devcontainer/jammy/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:22.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

.devcontainer/noble/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:24.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

.dockerignore

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

.github/workflows/cmake-ros.yml

@@ -0,0 +1,64 @@
+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:
+      fail-fast: false
+      matrix:
+        ros_distribution: [ noetic, humble, iron]
+        include:
+        - ros_distribution: 'noetic'
+          os: ubuntu-20.04
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04
+        - ros_distribution: 'iron'
+          os: ubuntu-22.04
+        # Currently CI has some errors on setup-ros with this OS, disabling for now
+        #- ros_distribution: 'jazzy'
+        #  os: ubuntu-24.04
+
+    steps:   
+    - uses: ros-tooling/setup-ros@v0.6
+      with:
+        required-ros-distributions: ${{ matrix.ros_distribution }}
+    
+    - uses: actions/checkout@v4
+
+    - 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,49 @@
+name: CMake
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    name: ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os: [ubuntu-24.04, 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@v4
+
+    - 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,200 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker_deps:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps, noble-deps]
+        include:
+        - docker_tag: focal-deps
+          docker_tags: |
+            introlab3it/rtabmap:focal-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+            linux/arm/v7
+          docker_path: 'focal/deps'
+        - docker_tag: jammy-deps
+          docker_tags: |
+            introlab3it/rtabmap:jammy-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy/deps'
+        - docker_tag: jammy-iron-deps
+          docker_tags: |
+            introlab3it/rtabmap:jammy-iron-deps
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'jammy-iron/deps'
+        - docker_tag: noble-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble/deps'
+    
+    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
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+  
+  docker:
+    needs: docker_deps
+    runs-on: ubuntu-latest
+    
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [bionic, focal, jammy, jammy-iron, noble, android23, android24, android26, android30]
+        include:
+        - 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
+            linux/arm/v7
+          docker_path: 'focal'
+        - 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: jammy-iron
+          docker_tags: |
+            introlab3it/rtabmap:jammy-iron
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'jammy-iron'
+        - docker_tag: noble
+          docker_tags: |
+            introlab3it/rtabmap:noble
+            introlab3it/rtabmap:24.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble'
+        - 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
+

.github/workflows/scheduled-stats.yml

@@ -0,0 +1,16 @@
+name: RTAB-Map Scheduled Stats Extraction From GitHub
+
+on:
+    workflow_dispatch:
+    schedule:
+        - cron: '0 5 * * *'
+jobs:
+    get_stats:
+        runs-on: ubuntu-latest
+        steps:
+            - name: Update Stats
+              uses: introlab/github-stats-action@v1
+              with:
+                  github-stats-token: ${{ secrets.STATS_TOKEN }}
+                  google-application-credentials: ${{ secrets.GOOGLE_APPLICATION_CREDENTIALS }}
+                  spreadsheet-id: ${{ secrets.SPREADSHEET_ID }}

.gitignore

@@ -10,3 +10,5 @@ app/android/AndroidManifest.xml
 app/android/res/raw/
 compile_flags.txt
 tags
+build_*
+*.bak

.travis.yml

@@ -1,80 +0,0 @@
-language: cpp
-   
-jobs:
-  include:
-#    - name: osx
-#      compiler: clang
-#      os: osx
-#      install:
-#        - brew install sqlite
-#        - brew install pcl
-#        - brew install opencv@3
-    
-#    - name: linux-trusty
-#      compiler: gcc
-#      os: linux
-#      dist: trusty
-#      install:
-#        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-#        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-#        - sudo apt-get update
-#        - sudo apt-get update && sudo apt-get install dpkg
-#        - sudo apt-get -y install ros-indigo-rtabmap-ros
-#        - sudo apt-get -y remove ros-indigo-rtabmap
-#
-#      before_script: 
-#        - source /opt/ros/indigo/setup.bash
-
-    - name: linux-xenial
-      compiler: gcc
-      os: linux
-      dist: xenial
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get update && sudo apt-get install dpkg
-        - sudo apt-get -y install ros-kinetic-rtabmap-ros
-        - sudo apt-get -y remove ros-kinetic-rtabmap
-
-      before_script: 
-        - source /opt/ros/kinetic/setup.bash
-
-    - name: linux-bionic
-      compiler: gcc
-      os: linux
-      dist: bionic
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get update && sudo apt-get install dpkg
-        - sudo apt-get -y install ros-melodic-rtabmap-ros
-        - sudo apt-get -y remove ros-melodic-rtabmap
-
-      before_script: 
-        - source /opt/ros/melodic/setup.bash
-
-    - name: linux-focal
-      compiler: gcc
-      os: linux
-      dist: focal
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get update && sudo apt-get install dpkg
-        - sudo apt-get -y install ros-noetic-rtabmap-ros
-        - sudo apt-get -y remove ros-noetic-rtabmap
-
-      before_script: 
-        - source /opt/ros/noetic/setup.bash
-
-script:
-  - mkdir -p build && cd build
-  - cmake .. 
-  - make
-
-notifications:
-  email:
-    - matlabbe@gmail.com

README.md

@@ -1,21 +1,25 @@
-rtabmap ![Analytics](https://ga-beacon-279122.nn.r.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]
+[![Downloads][downloads-image]][downloads]
 [![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.20.7-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.21.4-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
+[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
+[downloads]: https://github.com/introlab/rtabmap/releases
+
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
 [license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
 
 RTAB-Map library and standalone application.
 
-For more information, visit the [RTAB-Map's home page](http://introlab.github.io/rtabmap) or the [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
+ * 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.
 
@@ -25,3 +29,54 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
 <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="1">ROS 1</td>
+            <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>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>Iron</td>
+            <td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_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>
+        <tr>
+           <td>Docker</td>
+           <td>
+             <a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
+           </td>
+           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap.svg?label=pulls" alt="Docker Pulls"/></td>
+        </tr>
+    </tbody>
+</table>

RTABMapConfig.cmake.in

@@ -1,96 +1,106 @@
-# - Config file for the RTABMap package
-# Components:
-#  core    (required)
-#  gui     (optional)
-#  utilite (required)
-# It defines the following variables
-#  RTABMap_INCLUDE_DIRS - include directories for RTABMap
-#  RTABMap_LIBRARIES    - libraries to link against
-#  RTABMap_CORE         - core library
-#  RTABMap_UTILITE      - utilite library
-#  RTABMap_GUI          - gui library (set if RTABMap is built with Qt)
+include(CMakeFindDependencyMacro)
 
-# Compute paths
-get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
-set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
+# Mandatory dependencies
+find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
 
-#core lib
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-
-IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
-   SET(RTABMap_CORE
-      debug ${RTABMap_CORE_DEBUG}
-      optimized ${RTABMap_CORE_RELEASE}
-   )
-ELSEIF(RTABMap_CORE_DEBUG)
-   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
+    
+    if(@CONF_QT_VERSION@ EQUAL 6)
+        find_dependency(Qt6 COMPONENTS Widgets Core Gui OpenGL)
+    elseif(@CONF_QT_VERSION@ EQUAL 5)
+        find_dependency(Qt5 COMPONENTS Widgets Core Gui OpenGL)
+    else() # Qt4
+        find_dependency(Qt4 COMPONENTS QtCore QtGui)
+    endif()
+    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
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-
-IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
-   SET(RTABMap_UTILITE
-      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})
+IF(@CONF_WITH_REALSENSE2@)
+    IF(WIN32)
+        find_dependency(RealSense2)
+    ELSE()
+        find_dependency(realsense2)
+    ENDIF()
 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)
-set(WITH_GUI ON)
-if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
-   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
-   if (${_index} EQUAL -1)
-      set(WITH_GUI OFF)
-   endif()
-endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+IF(@CONF_WITH_DEPTH_AI@)
+  find_dependency(depthai 2.24)
+ENDIF()
 
+IF(@CONF_WITH_XVSDK@)
+  find_dependency(xvsdk)
+ENDIF()
 
-#gui lib (OFF if RTAB-Map is not built with Qt)
-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(WARNING "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
-endif()
+IF(@CONF_WITH_OCTOMAP@)
+  find_dependency(octomap)
+ENDIF()
 
-# Dependencies
-if(@CONF_VTK_QT@ AND ${WITH_GUI})
-   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
-endif(@CONF_VTK_QT@ AND ${WITH_GUI})
-SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
+IF(@CONF_WITH_GRIDMAP@)
+  find_dependency(grid_map_core)
+ENDIF()
 
-#backward compatibilities
-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)
+IF(@CONF_WITH_PYTHON@)
+  find_dependency(Python3 COMPONENTS Interpreter Development NumPy)
+ENDIF()
 
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
-mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)
+# Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies)
+set(RTABMap_INCLUDE_DIRS "")
+set(RTABMap_LIBRARIES "")
+set(RTABMap_TARGETS "")
+
+set(_RTABMap_supported_components utilite core gui)
+
+foreach(_comp ${_RTABMap_supported_components})
+  if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    include("${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    set(RTABMap_${_comp}_FOUND True)
+    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()
+
+include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")
+
+foreach(_comp ${RTABMap_FIND_COMPONENTS})
+  if (NOT RTABMap_${_comp}_FOUND)
+    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,10 +40,12 @@ 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
+@OPENNI@#define RTABMAP_OPENNI
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
@@ -52,9 +54,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @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
@@ -64,7 +69,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @REALSENSE2@#define RTABMAP_REALSENSE2
 @MYNTEYE@#define RTABMAP_MYNTEYE
 @DEPTHAI@#define RTABMAP_DEPTHAI
+@XVSDK@#define RTABMAP_XVSDK
 @OCTOMAP@#define RTABMAP_OCTOMAP
+@GRIDMAP@#define RTABMAP_GRIDMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
 @ALICE_VISION@#define RTABMAP_ALICE_VISION
 @OPENCHISEL@#define RTABMAP_OPENCHISEL

app/android/AndroidManifest.xml.in

@@ -17,6 +17,11 @@
 
     <!-- This is the platform API where depth16 support in android was introduced. -->
     <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
+    
+    <queries>
+        <package android:name="com.google.ar.core" />
+        <package android:name="com.huawei.ar.engine" /> 
+    </queries>
 
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 
@@ -42,7 +47,16 @@
             <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"/>

app/android/jni/CMakeLists.txt

@@ -3,6 +3,7 @@ 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}

app/android/jni/CameraARCore.cpp

@@ -334,11 +334,12 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR
 	}
 }
 
-SensorData CameraARCore::captureImage(CameraInfo * info)
+SensorData CameraARCore::updateDataOnRender(Transform & pose)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");
 
+	pose.setNull();
 	SensorData data;
 	if(!arSession_)
 	{
@@ -370,7 +371,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	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,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
 				transformed_uvs_);
 		UASSERT(transformed_uvs_);
 		uvs_initialized_ = true;
@@ -393,7 +394,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
-	Transform pose;
 	CameraModel model;
 	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
 	{
@@ -401,24 +401,13 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		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 = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		poseArCore = rtabmap::rtabmap_world_T_opengl_world * poseArCore * rtabmap::opengl_world_T_rtabmap_world;
 
-		Transform poseArCore = pose;
-		if(pose.isNull())
+		if(poseArCore.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;
@@ -520,11 +509,11 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							cv::Mat yuv(height+height/2, width, CV_8UC1);
 							memcpy(yuv.data, plane_data, data_length);
 							memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
-							cv::cvtColor(yuv, rgb, CV_YUV2BGR_NV21);
+							cv::cvtColor(yuv, rgb, cv::COLOR_YUV2BGR_NV21);
 						}
 						else
 						{
-							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, cv::COLOR_YUV2BGR_NV21);
 						}
 
 						std::vector<cv::KeyPoint> kpts;
@@ -551,6 +540,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 
 						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
+
+						if(!poseArCore.isNull())
+						{
+							pose = poseArCore;
+							this->poseReceived(pose, stamp);
+							// adjust origin
+							if(!getOriginOffset().isNull())
+							{
+								pose = getOriginOffset() * pose;
+							}
+						}
 					}
 				}
 				else
@@ -571,134 +571,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	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

@@ -63,23 +63,14 @@ 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 void close(); // close ARCore 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();
+	virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);

app/android/jni/CameraAREngine.cpp

@@ -117,9 +117,6 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri
 
 	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!");
@@ -169,38 +166,87 @@ void CameraAREngine::close()
 	CameraMobile::close();
 }
 
-SensorData CameraAREngine::captureImage(CameraInfo * info)
+void CameraAREngine::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;
+		  }
+
+		HwArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
+SensorData CameraAREngine::updateDataOnRender(Transform & pose)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");
 
+	pose.setNull();
 	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)
+		HwArSession_setCameraTextureName(arSession_, textureId_);
+
 	// 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;
 	}
 
+	// 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;
+	HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		HwArFrame_transformDisplayUvCoords(
+				arSession_, arFrame_,
+				BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView,
+				transformed_uvs_);
+		UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)", 
+			transformed_uvs_[0], transformed_uvs_[1],
+			transformed_uvs_[2], transformed_uvs_[3],
+			transformed_uvs_[4], transformed_uvs_[5],
+			transformed_uvs_[6], transformed_uvs_[7]);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	HwArCamera* ar_camera;
 	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
 
+	HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	HwArCamera_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_));
+	}
+
 	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;
@@ -244,7 +290,7 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 				cv::Mat outputRGB;
 				if(imageData != nullptr && len>0)
 				{
-					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, cv::COLOR_YUV2BGR_NV21);
 				}
 
 				//Depth
@@ -274,6 +320,26 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 					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);
+
+					// 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())
+					{
+						LOGE("CameraAREngine: Pose is null");
+					}
+					else
+					{
+						pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+						this->poseReceived(pose, stamp);
+						// adjust origin
+						if(!getOriginOffset().isNull())
+						{
+							pose = getOriginOffset() * pose;
+						}
+					}
 				}
 			}
 			else
@@ -291,66 +357,8 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 	}
 
 	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

@@ -38,6 +38,7 @@ 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 <background_renderer.h>
 
 #include <huawei_arengine_interface.h>
 
@@ -48,13 +49,14 @@ public:
 	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
 	virtual ~CameraAREngine();
 
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual void close(); // close Tango connection
+	virtual void close(); // close AREngine connection
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	virtual void capturePoseOnly();
+	virtual SensorData updateDataOnRender(Transform & pose);
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -69,7 +71,6 @@ private:
 	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
 	HwArPose * arPose_ = nullptr;
 	bool arInstallRequested_;
-	GLuint textureId_;
 	UMutex arSessionMutex_;
 
 };

app/android/jni/CameraMobile.cpp

@@ -55,10 +55,8 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
 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),
@@ -79,13 +77,12 @@ bool CameraMobile::init(const std::string &, const std::string &)
 
 void CameraMobile::close()
 {
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
+	firstFrame_ = true;
 	lastKnownGPS_ = GPS();
 	lastEnvSensors_.clear();
 	originOffset_ = Transform();
 	originUpdate_ = false;
-	pose_ = Transform();
+	dataPose_ = Transform();
 	data_ = SensorData();
 
     if(textureId_ != 0)
@@ -97,35 +94,107 @@ void CameraMobile::close()
 
 void CameraMobile::resetOrigin()
 {
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
+	firstFrame_ = true;
 	lastKnownGPS_ = GPS();
 	lastEnvSensors_.clear();
-	pose_ = Transform();
+	dataPose_ = Transform();
 	data_ = SensorData();
 	originUpdate_ = true;
 }
 
-void CameraMobile::poseReceived(const Transform & pose)
+bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+	pose.setNull();
+
+	int maxWaitTimeMs = maxWaitTime * 1000;
+
+	// Interpolate pose
+	if(!poseBuffer_.empty())
+	{
+		poseMutex_.lock();
+		int waitTry = 0;
+		while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		{
+			poseMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			poseMutex_.lock();
+		}
+		if(poseBuffer_.rbegin()->first < stamp)
+		{
+			if(maxWaitTimeMs > 0)
+			{
+				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+			}
+			else
+			{
+				UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first);
+			}
+		}
+		else
+		{
+			std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
+			std::map<double, Transform>::const_iterator iterA = iterB;
+			if(iterA != poseBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == poseBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				pose = iterA->second;
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
+			}
+			else // stamp < iterA->first
+			{
+				UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+			}
+		}
+		poseMutex_.unlock();
+	}
+	return !pose.isNull();
+}
+
+void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
 {
 	if(!pose.isNull())
 	{
-		// send pose of the camera (without optical rotation)
-		Transform p = pose*deviceTColorCamera_;
+		Transform p = pose;
 		if(originUpdate_)
 		{
 			originOffset_ = p.translation().inverse();
 			originUpdate_ = false;
 		}
+		
+		if(stampEpochOffset_ == 0.0)
+		{
+			stampEpochOffset_ = UTimer::now() - deviceStamp;
+		}
+        
+		double epochStamp = stampEpochOffset_ + deviceStamp;
 
 		if(!originOffset_.isNull())
 		{
-			this->post(new PoseEvent(originOffset_*p));
+			p = originOffset_*p;
 		}
-		else
+
 		{
-			this->post(new PoseEvent(p));
+			UScopeMutex lock(poseMutex_);
+			poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p));
+			if(poseBuffer_.size() > 1000)
+			{
+				poseBuffer_.erase(poseBuffer_.begin());
+			}
 		}
+
+		// send pose of the camera (with optical rotation)
+		this->post(new PoseEvent(p * deviceTColorCamera_));
 	}
 }
 
@@ -139,11 +208,20 @@ 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)
+void CameraMobile::addEnvSensor(int type, float value)
 {
-	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
+void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
+{
+	UScopeMutex lock(dataMutex_);
+
+	bool notify = !data_.isValid();
+
+	LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
 	data_ = data;
-    pose_ = pose;
+    dataPose_ = pose;
 
     viewMatrix_ = viewMatrix;
     projectionMatrix_ = projectionMatrix;
@@ -151,7 +229,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
     // adjust origin
     if(!originOffset_.isNull())
     {
-        pose_ = originOffset_ * pose_;
+        dataPose_ = originOffset_ * dataPose_;
         viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
     }
     
@@ -166,12 +244,12 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
         uvs_initialized_ = true;
     }
     
-    LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
+    LOGD("CameraMobile::update textureId_=%d", (int)textureId_);
 
     if(textureId_ != 0 && texCoord != 0)
     {
         cv::Mat rgbImage;
-        cv::cvtColor(data.imageRaw(), rgbImage, CV_BGR2RGBA);
+        cv::cvtColor(data.imageRaw(), rgbImage, cv::COLOR_BGR2RGBA);
         
         glBindTexture(GL_TEXTURE_2D, textureId_);
         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
@@ -193,78 +271,63 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
             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())
+	postUpdate();
+	
+	if(notify)
 	{
-		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;
-			}
-		}*/
+		dataReady_.release();
+	}
+}
 
-		if(!ignoreFrame)
+void CameraMobile::updateOnRender()
+{
+	UScopeMutex lock(dataMutex_);
+	bool notify = !data_.isValid();
+
+	data_ = updateDataOnRender(dataPose_);
+
+	if(data_.isValid())
+	{
+		postUpdate();
+
+		if(notify)
 		{
-			spinOnceFrameRateTimer_.start();
-			spinOncePreviousStamp_ = now;
-			mainLoop();
-		}
-		else
-		{
-			// just send pose
-			capturePoseOnly();
+			dataReady_.release();
 		}
 	}
 }
 
-void CameraMobile::mainLoopBegin()
+SensorData CameraMobile::updateDataOnRender(Transform & pose)
 {
-	double t = cameraStartedTime_.elapsed();
-	if(t < 5.0)
-	{
-		uSleep((5.0-t)*1000); // just to make sure that the camera is started
-	}
+	LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() "
+	     "should be overridden by inherited classes. Returning empty data!\n");
+	return SensorData();
 }
 
-void CameraMobile::mainLoop()
+void CameraMobile::postUpdate()
 {
-	CameraInfo info;
-	SensorData data = this->captureImage(&info);
-
-	if(data.isValid() && !info.odomPose.isNull())
+	if(data_.isValid())
 	{
-		if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
+		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
 		{
-			data.setGPS(lastKnownGPS_);
+			data_.setGPS(lastKnownGPS_);
 		}
 		else if(lastKnownGPS_.stamp()>0.0)
 		{
-			LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
+			LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
 		}
 
 		if(lastEnvSensors_.size())
 		{
-			data.setEnvSensors(lastEnvSensors_);
+			data_.setEnvSensors(lastEnvSensors_);
 			lastEnvSensors_.clear();
 		}
 
-		if(smoothing_ && !data.depthRaw().empty())
+		if(smoothing_ && !data_.depthRaw().empty())
 		{
 			//UTimer t;
-			data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
+			data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
 			//LOGD("Bilateral filtering, time=%fs", t.ticks());
 		}
 
@@ -273,15 +336,15 @@ void CameraMobile::mainLoop()
 		{
 			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::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::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];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageHeight(), model.imageWidth());
 			model = CameraModel(
 					model.fy(),
@@ -290,25 +353,25 @@ void CameraMobile::mainLoop()
 					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);
+			data_.setRGBDImage(rgb, depth, model);
 
-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			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;
+				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());
+			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(data_.imageRaw(),rgb,1);
 			cv::flip(rgb,rgb,0);
-			cv::flip(data.depthOrRightRaw(),depth,1);
+			cv::flip(data_.depthOrRightRaw(),depth,1);
 			cv::flip(depth,depth,0);
-			CameraModel model = data.cameraModels()[0];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageWidth(), model.imageHeight());
 			model = CameraModel(
 					model.fx(),
@@ -317,26 +380,26 @@ void CameraMobile::mainLoop()
 					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);
+			data_.setRGBDImage(rgb, depth, model);
 
-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			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;
+				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());
+			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::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::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];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageHeight(), model.imageWidth());
 			model = CameraModel(
 					model.fy(),
@@ -345,61 +408,54 @@ void CameraMobile::mainLoop()
 					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);
+			data_.setRGBDImage(rgb, depth, model);
 
-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			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;
+				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());
+			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)
+SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
 {
-	if(info)
+	SensorData data;
+	if(dataReady_.acquire(1, 5000))
 	{
-		info->odomPose = pose_;
+		UScopeMutex lock(dataMutex_);
+		data = data_;
+		data_ = SensorData();
 	}
-	return data_;
+	if(data.isValid())
+	{
+		data.setGroundTruth(Transform());
+		data.setStamp(stampEpochOffset_ + data.stamp());
+
+		if(info)
+		{
+			// linear cov = 0.0001
+			info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001);
+			if(!firstFrame_)
+			{
+				// angular cov = 0.000001
+				info->odomCovariance.at<double>(3,3) *= 0.01;
+				info->odomCovariance.at<double>(4,4) *= 0.01;
+				info->odomCovariance.at<double>(5,5) *= 0.01;
+			}
+			info->odomPose = dataPose_;
+		}
+
+		firstFrame_ = false;
+	}
+	else
+	{
+		UWARN("CameraMobile::captureImage() invalid data!");
+	}
+	return data;
 }
 
 LaserScan CameraMobile::scanFromPointCloudData(
@@ -432,23 +488,25 @@ LaserScan CameraMobile::scanFromPointCloudData(
             //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))
+            if(pt.z > 0)
             {
-                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;
+                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);
 

app/android/jni/CameraMobile.h

@@ -68,7 +68,7 @@ private:
 	Transform pose_;
 };
 
-class CameraMobile : public Camera, public UThread, public UEventsSender {
+class CameraMobile : public Camera, public UEventsSender {
 public:
 	static const float bilateralFilteringSigmaS;
 	static const float bilateralFilteringSigmaR;
@@ -93,14 +93,20 @@ public:
 
 	// 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 void close(); // inherited classes should call its parent at the end of their close().
 	virtual std::string getSerial() const {return "CameraMobile";}
 
+	void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
+	void updateOnRender();
+
 	const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
 	void resetOrigin();
 	virtual bool isCalibrated() const;
 
-	void poseReceived(const Transform & pose); // in rtabmap frame
+	virtual bool odomProvided() const { return true; }
+	virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time
+	void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
+	double getStampEpochOffset() const {return stampEpochOffset_;}
 
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
@@ -108,10 +114,7 @@ public:
 	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_;}
@@ -122,17 +125,15 @@ public:
     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 SensorData updateDataOnRender(Transform & pose);
 
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
+private:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+	void postUpdate(); // Should be called while being protected by dataMutex_
 
 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_;
@@ -141,9 +142,7 @@ protected:
     bool uvs_initialized_ = false;
 
 private:
-	Transform previousPose_;
-	double previousStamp_;
-	UTimer cameraStartedTime_;
+	bool firstFrame_;
 	double stampEpochOffset_;
 	bool smoothing_;
 	ScreenRotation colorCameraToDisplayRotation_;
@@ -152,8 +151,13 @@ private:
 	Transform originOffset_;
 	bool originUpdate_;
 
+	USemaphore dataReady_;
+	UMutex dataMutex_;
 	SensorData data_;
-	Transform pose_;
+	Transform dataPose_;
+
+	UMutex poseMutex_;
+	std::map<double, Transform> poseBuffer_; // <stamp, Pose>
     
     cv::Mat occlusionImage_;
     CameraModel occlusionModel_;

app/android/jni/CameraTango.cpp

@@ -101,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(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
+		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp);
 	}
 }
 
@@ -444,7 +444,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
 
 		UASSERT(cloud.type() == CV_32FC4);
-		boost::mutex::scoped_lock  lock(dataMutex_);
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
 
 		// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
 		// "In the current version of Project Tango Tablet RGB IR camera
@@ -463,7 +463,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 
 			if(dt >= 0.0 && dt < 0.5)
 			{
-				bool notify = !data_.isValid();
+				bool notify = !tangoData_.isValid();
 
 				cv::Mat tangoImage = tangoColor_;
 				cv::Mat rgb;
@@ -478,15 +478,15 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 				LOGD("tangoColorType=%d", tangoColorType);
 				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_RGBA2BGR);
 				}
 				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_YV12);
 				}
 				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_NV21);
 				}
 				else if(tangoColorType == 35)
 				{
@@ -495,7 +495,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 				else
 				{
 					LOGE("Not supported color format : %d.", tangoColorType);
-					data_ = SensorData();
+					tangoData_ = SensorData();
 					return;
 				}
 
@@ -678,24 +678,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 
 					if(rawScanPublished_)
 					{
-						data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
 					else
 					{
-						data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
-					data_.setGroundTruth(odom);
+					tangoData_.setGroundTruth(odom);
 				}
 				else
 				{
 					LOGE("Could not get depth and rgb images!?!");
-					data_ = SensorData();
+					tangoData_ = SensorData();
 					return;
 				}
 
 				if(notify)
 				{
-					dataReady_.release();
+					tangoDataReady_.release();
 				}
 				LOGD("process cloud received %fs", timer.ticks());
 			}
@@ -709,7 +709,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	{
 		//LOGD("RGB received! %fs", timestamp);
 
-		boost::mutex::scoped_lock  lock(dataMutex_);
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
 
 		tangoColor_ = tangoImage.clone();
 		tangoColorStamp_ = timestamp;
@@ -775,10 +775,11 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	return pose;
 }
 
-SensorData CameraTango::captureImage(CameraInfo * info)
+SensorData CameraTango::updateDataOnRender(Transform & pose)
 {
 	//LOGI("Capturing image...");
 
+	pose.setNull();
 	if(textureId_ == 0)
 	{
 		glGenTextures(1, &textureId_);
@@ -797,10 +798,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 		if (status == TANGO_SUCCESS)
 		{
-			if(info)
-			{
-				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
-			}
+			pose = getPoseAtTimestamp(video_overlay_timestamp);
 
 			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
 			  if (rotation > 3) {
@@ -876,16 +874,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 	}
 
 	SensorData data;
-	if(dataReady_.acquireTry(1))
+	if(tangoDataReady_.acquireTry(1))
 	{
-		boost::mutex::scoped_lock  lock(dataMutex_);
-		data = data_;
-		data_ = SensorData();
-		if(info)
-		{
-			info->odomPose = data.groundTruth();
-			data.setGroundTruth(Transform());
-		}
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
+		data = tangoData_;
+		tangoData_ = SensorData();
+		pose = data.groundTruth();
+		data.setGroundTruth(Transform());
 	}
 	return data;
 

app/android/jni/CameraTango.h

@@ -52,7 +52,6 @@ public:
 	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
-	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 
@@ -61,7 +60,7 @@ public:
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData updateDataOnRender(Transform & pose);
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -71,12 +70,12 @@ private:
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	SensorData data_;
+	SensorData tangoData_;
 	cv::Mat tangoColor_;
 	int tangoColorType_;
 	double tangoColorStamp_;
-	boost::mutex dataMutex_;
-	USemaphore dataReady_;
+	boost::mutex tangoDataMutex_;
+	USemaphore tangoDataReady_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
 };

app/android/jni/RTABMapApp.cpp

@@ -64,6 +64,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/GainCompensator.h>
 #include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Recovery.h>
+#include <rtabmap/core/lidar/LidarVLP16.h>
 #include <pcl/common/common.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
@@ -82,6 +84,43 @@ static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
 #endif
 
+#ifdef __ANDROID__
+#ifndef DISABLE_LOG
+//ref: https://codelab.wordpress.com/2014/11/03/how-to-use-standard-output-streams-for-logging-in-android-apps/
+static int pfd[2];
+static pthread_t thr;
+static void *thread_func(void*)
+{
+    ssize_t rdsz;
+    char buf[128];
+    while((rdsz = read(pfd[0], buf, sizeof buf - 1)) > 0) {
+        if(buf[rdsz - 1] == '\n') --rdsz;
+        buf[rdsz] = 0;  /* add null-terminator */
+        __android_log_write(ANDROID_LOG_DEBUG, LOG_TAG, buf);
+    }
+    return 0;
+}
+
+int start_logger()
+{
+    /* make stdout line-buffered and stderr unbuffered */
+    setvbuf(stdout, 0, _IOLBF, 0);
+    setvbuf(stderr, 0, _IONBF, 0);
+
+    /* create the pipe and redirect stdout and stderr */
+    pipe(pfd);
+    dup2(pfd[1], 1);
+    dup2(pfd[1], 2);
+
+    /* spawn the logging thread */
+    if(pthread_create(&thr, 0, thread_func, 0) == -1)
+        return -1;
+    pthread_detach(thr);
+    return 0;
+}
+#endif
+#endif
+
 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 {
 	rtabmap::ParametersMap parameters;
@@ -96,6 +135,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAggressiveLoopThr(), "0.0"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
@@ -164,6 +204,7 @@ RTABMapApp::RTABMapApp() :
 #endif
 		cameraDriver_(0),
 		camera_(0),
+		sensorCaptureThread_(0),
 		rtabmapThread_(0),
 		rtabmap_(0),
 		logHandler_(0),
@@ -178,6 +219,7 @@ RTABMapApp::RTABMapApp() :
 		cameraColor_(true),
 		fullResolution_(false),
 		appendMode_(true),
+		useExternalLidar_(false),
 		maxCloudDepth_(2.5),
 		minCloudDepth_(0.0),
 		cloudDensityLevel_(1),
@@ -243,6 +285,12 @@ RTABMapApp::RTABMapApp() :
 
 	this->registerToEventsManager();
 	LOGI("RTABMapApp::RTABMapApp() end");
+
+#ifdef __ANDROID__
+#ifndef DISABLE_LOG
+	start_logger();
+#endif
+#endif
 }
 
 #ifndef __ANDROID__ // __APPLE__
@@ -305,9 +353,9 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	}
 }
 
-int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource)
+int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
-	LOGW("Opening database %s (inMemory=%d, optimize=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0);
+	LOGW("Opening database %s (inMemory=%d, optimize=%d, clearDatabase=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0, clearDatabase?1:0);
 	this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
 	rtabmapMutex_.lock();
@@ -360,11 +408,11 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	if(!databaseSource.empty())
+	if(!databasePath.empty() && UFile::exists(databasePath) && !clearDatabase)
 	{
 		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
 		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
-		if(driver->openConnection(databaseSource))
+		if(driver->openConnection(databasePath))
 		{
 			cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
@@ -416,13 +464,11 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	}
 
 	UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading database..."));
-	LOGI("Erasing database \"%s\"...", databasePath.c_str());
-	UFile::erase(databasePath);
-	if(!databaseSource.empty())
-	{
-		LOGI("Copying database source \"%s\" to \"%s\"...", databaseSource.c_str(), databasePath.c_str());
-		UFile::copy(databaseSource, databasePath);
-	}
+	if(clearDatabase)
+    {
+        LOGI("Erasing database \"%s\"...", databasePath.c_str());
+        UFile::erase(databasePath);
+    }
 
 	//Rtabmap
 	mapToOdom_.setIdentity();
@@ -448,7 +494,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			poses,
 			links,
 			true,
-			true,
+			false, // Make sure poses are the same than optimized mesh (in case we switched RGBD/OptimizedFromGraphEnd)
 			&signatures,
 			true,
 			true,
@@ -495,7 +541,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							// Voxelize and filter depending on the previous cloud?
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
-							if(!data.imageRaw().empty() && !data.depthRaw().empty())
+							if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
                                 
@@ -593,7 +639,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
                                     if(renderingTextureDecimation_>1)
                                     {
                                         cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
-                                        cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+                                        cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
                                     }
                                     else
                                     {
@@ -607,9 +653,9 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 								UWARN("Cloud %d is empty", id);
 							}
 						}
-						else
+						else if(!data.depthOrRightCompressed().empty() || !data.laserScanCompressed().isEmpty())
 						{
-							UERROR("Failed to uncompress data!");
+							UERROR("Failed to uncompress data! (rgb=%d, depth=%d, scan=%d)", data.imageCompressed().cols, data.depthOrRightCompressed().cols, data.laserScanCompressed().size());
 							status=-2;
 						}
 					}
@@ -843,7 +889,7 @@ bool RTABMapApp::startCamera()
 #endif
 	LOGW("startCamera() camera driver=%d", cameraDriver_);
 	boost::mutex::scoped_lock  lock(cameraMutex_);
-    
+
 	if(cameraDriver_ == 0) // Tango
 	{
 #ifdef RTABMAP_TANGO
@@ -895,6 +941,19 @@ bool RTABMapApp::startCamera()
 
 		LOGI("Start camera thread");
 		cameraJustInitialized_ = true;
+		if(useExternalLidar_)
+		{
+			rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
+			lidar->init();
+			camera_->setImageRate(0); // if lidar, to get close camera synchronization
+			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
+            sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true);
+		}
+		else
+		{
+			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_);
+		}
+		sensorCaptureThread_->start();
 		return true;
 	}
 	UERROR("Failed camera initialization!");
@@ -906,13 +965,12 @@ void RTABMapApp::stopCamera()
 	LOGI("stopCamera()");
 	{
 		boost::mutex::scoped_lock  lock(cameraMutex_);
-		if(camera_!=0)
+		if(sensorCaptureThread_!=0)
 		{
-			camera_->join(true);
-			camera_->close();
-			delete camera_;
+			sensorCaptureThread_->join(true);
+			delete sensorCaptureThread_; // camera_ is closed and deleted inside
+			sensorCaptureThread_ = 0;
 			camera_ = 0;
-			poseBuffer_.clear();
 		}
 	}
     {
@@ -1199,7 +1257,12 @@ int RTABMapApp::Render()
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
 	try
 	{
-		UTimer fpsTime;
+        if(sensorCaptureThread_ == 0)
+        {
+            // We are not doing continous drawing, just measure single draw
+            fpsTime_.restart();
+        }
+        
 #ifdef DEBUG_RENDERING_PERFORMANCE
 		UTimer time;
 #endif
@@ -1225,49 +1288,45 @@ int RTABMapApp::Render()
 			{
 				if(cameraDriver_ <= 2)
 				{
-					camera_->spinOnce();
+					camera_->updateOnRender();
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
-				LOGW("Camera spinOnce %fs", time.ticks());
+				LOGW("Camera updateOnRender %fs", time.ticks());
 #endif
-
-				if(cameraDriver_ != 2)
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
 				{
-					if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
+					main_scene_.background_renderer_ = new BackgroundRenderer();
+					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
+				}
+				if(camera_->uvsInitialized())
+				{
+					uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
+					((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+					if(graphOptimization_ && !mapToOdom_.isIdentity())
 					{
-						main_scene_.background_renderer_ = new BackgroundRenderer();
-						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
+						rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
+						arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
 					}
-					if(camera_->uvsInitialized())
-					{
-						uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
-						((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
-						if(graphOptimization_ && !mapToOdom_.isIdentity())
-						{
-							rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
-							arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
-						}
-					}
-					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
-					{
-						rtabmap::CameraModel occlusionModel;
-						cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
+				}
+				if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+				{
+					rtabmap::CameraModel occlusionModel;
+					cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
 
-						if(occlusionModel.isValidForProjection())
-						{
-							pcl::IndicesPtr indices(new std::vector<int>);
-							int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
-							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
-							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
-							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
-							occlusionMesh.indices = indices;
-							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
-						}
-						else if(!occlusionImage.empty())
-						{
-							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
-						}
+					if(occlusionModel.isValidForProjection())
+					{
+						pcl::IndicesPtr indices(new std::vector<int>);
+						int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
+						cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
+						occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
+						pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
+						occlusionMesh.indices = indices;
+						occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
+					}
+					else if(!occlusionImage.empty())
+					{
+						UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
 					}
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
@@ -1287,14 +1346,14 @@ int RTABMapApp::Render()
 			}
 		}
 
-		rtabmap::OdometryEvent odomEvent;
+		rtabmap::SensorEvent sensorEvent;
 		{
-			boost::mutex::scoped_lock  lock(odomMutex_);
-			if(odomEvents_.size())
+			boost::mutex::scoped_lock  lock(sensorMutex_);
+			if(sensorEvents_.size())
 			{
-				LOGI("Process odom events");
-				odomEvent = odomEvents_.back();
-				odomEvents_.clear();
+				LOGI("Process sensor events");
+				sensorEvent = sensorEvents_.back();
+				sensorEvents_.clear();
 				if(cameraJustInitialized_)
 				{
 					notifyCameraStarted = true;
@@ -1314,7 +1373,7 @@ int RTABMapApp::Render()
 			{
 				main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl);
 			}
-			if(camera_!=0 && cameraJustInitialized_)
+			if(sensorCaptureThread_!=0 && cameraJustInitialized_)
 			{
 				notifyCameraStarted = true;
 				cameraJustInitialized_ = false;
@@ -1399,7 +1458,14 @@ int RTABMapApp::Render()
                     }
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && landmark!=0)
 					{
-						main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        if(rejected)
+                        {
+                            main_scene_.setBackgroundColor(0.5, 0.325f, 0); // dark orange
+                        }
+                        else
+                        {
+                            main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        }
 					}
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && rejected>0)
 					{
@@ -1439,12 +1505,12 @@ int RTABMapApp::Render()
 
 			main_scene_.setMeshRendering(main_scene_.hasMesh(g_optMeshId), main_scene_.hasTexture(g_optMeshId));
 
-			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
 			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix);
-			if(renderingTime_ < fpsTime.elapsed())
+            double fpsTime = fpsTime_.ticks();
+			if(renderingTime_ < fpsTime)
 			{
-				renderingTime_ = fpsTime.elapsed();
+				renderingTime_ = fpsTime;
 			}
 
 			// revert state
@@ -1508,9 +1574,9 @@ int RTABMapApp::Render()
 			if(clearSceneOnNextRender_)
 			{
 				LOGI("Clearing all rendering data...");
-				odomMutex_.lock();
-				odomEvents_.clear();
-				odomMutex_.unlock();
+				sensorMutex_.lock();
+				sensorEvents_.clear();
+				sensorMutex_.unlock();
 
 				poseMutex_.lock();
 				poseEvents_.clear();
@@ -1571,7 +1637,7 @@ int RTABMapApp::Render()
 									{
 										cv::Size reducedSize(textureRaw.cols/renderingTextureDecimation_, textureRaw.rows/renderingTextureDecimation_);
 										LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
-										cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+										cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
 									}
 									else
 									{
@@ -1746,7 +1812,7 @@ int RTABMapApp::Render()
 										// Voxelize and filter depending on the previous cloud?
 										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 										pcl::IndicesPtr indices(new std::vector<int>);
-										if(!data.imageRaw().empty() && !data.depthRaw().empty())
+										if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 										{
                                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
 											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -1856,7 +1922,7 @@ int RTABMapApp::Render()
                                                 if(renderingTextureDecimation_ > 1)
                                                 {
                                                     cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
-                                                    cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+                                                    cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
 #ifdef DEBUG_RENDERING_PERFORMANCE
                                                     LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks());
 #endif
@@ -1950,26 +2016,26 @@ int RTABMapApp::Render()
 			}
 			else
 			{
-				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && camera_!=0);
+				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0);
 
 				//just process the last one
-				if(!odomEvent.pose().isNull())
+				if(!sensorEvent.info().odomPose.isNull())
 				{
 					if(odomCloudShown_ && !trajectoryMode_)
 					{
-						if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) || !odomEvent.data().laserScanRaw().isEmpty())
+						if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty())
 						{
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
-							if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()))
+							if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
 							{
-                                int meshDecimation = updateMeshDecimation(odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows);
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+                                int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 							}
 							else
 							{
 								//scan
-								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
+								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
 								indices->resize(cloud->size());
 								for(unsigned int i=0; i<cloud->size(); ++i)
 								{
@@ -1980,10 +2046,10 @@ int RTABMapApp::Render()
 							if(cloud->size() && indices->size())
 							{
 								LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
-										odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
-										odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
+										sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows,
+										sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows,
 									   (int)cloud->width, (int)cloud->height);
-								main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
+								main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose);
 								main_scene_.setCloudVisible(-1, true);
 							}
 							else
@@ -2046,13 +2112,13 @@ int RTABMapApp::Render()
 				notifyDataLoaded = true;
 			}
 
-			fpsTime.restart();
-			main_scene_.setFrustumVisible(camera_!=0);
+            main_scene_.setFrustumVisible(camera_!=0);
 			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh, true);
-			if(renderingTime_ < fpsTime.elapsed())
+            double fpsTime = fpsTime_.ticks();
+            if(renderingTime_ < fpsTime)
 			{
-				renderingTime_ = fpsTime.elapsed();
-			}
+				renderingTime_ = fpsTime;
+            }
 
 			if(rtabmapEvents.size())
 			{
@@ -2073,10 +2139,10 @@ int RTABMapApp::Render()
 
 				lastPostRenderEventTime_ = UTimer::now();
 
-				if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
+				if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
 				{
 					UERROR("TangoPoseEventNotReceived");
-					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_)));
+					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
 				}
 			}
 		}
@@ -2089,7 +2155,7 @@ int RTABMapApp::Render()
 			cv::Mat image(h, w, CV_8UC4);
 			glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data);
 			cv::flip(image, image, 0);
-			cv::cvtColor(image, image, CV_RGBA2BGRA);
+			cv::cvtColor(image, image, cv::COLOR_RGBA2BGRA);
 			cv::Mat roi;
 			if(w>h)
 			{
@@ -2108,16 +2174,6 @@ int RTABMapApp::Render()
 			screenshotReady_.release();
 		}
 
-		if((openingDatabase_ && !visualizingMesh_) || exporting_ || postProcessing_)
-		{
-			// throttle rendering max 5Hz if we are doing some processing
-			double renderTime = fpsTime.elapsed();
-			if(0.2 - renderTime > 0.0)
-			{
-				uSleep((0.2 - renderTime)*1000);
-			}
-		}
-
 		if((rtabmapThread_==0 || !rtabmapThread_->isRunning()) && lastPostRenderEventTime_ > 0.0)
 		{
 			double interval = UTimer::now() - lastPostRenderEventTime_;
@@ -2275,7 +2331,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
-	if((camera_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
+	if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
@@ -2550,6 +2606,26 @@ void RTABMapApp::save(const std::string & databasePath)
 	}
 }
 
+bool RTABMapApp::recover(const std::string & from, const std::string & to)
+{
+    std::string errorMsg;
+    if(!databaseRecovery(from, false, &errorMsg, &progressionStatus_))
+    {
+        LOGE("Recovery Error: %s", errorMsg.c_str());
+        return false;
+    }
+    else
+    {
+        LOGI("Renaming %s to %s", from.c_str(), to.c_str());
+        if(UFile::rename(from, to) != 0)
+        {
+            LOGE("Failed renaming %s to %s", from.c_str(), to.c_str());
+            return false;
+        }
+        return true;
+    }
+}
+
 void RTABMapApp::cancelProcessing()
 {
 	UWARN("Processing canceled!");
@@ -3645,19 +3721,12 @@ void RTABMapApp::postCameraPoseEvent(
 		if(qx==0 && qy==0 && qz==0 && qw==0)
 		{
 			// Lost! clear buffer
-			poseBuffer_.clear();
 			camera_->resetOrigin(); // we are lost, create new session on next valid frame
 			return;
 		}
 		rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
 		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-		camera_->poseReceived(pose);
-
-		poseBuffer_.insert(std::make_pair(stamp, pose));
-		if(poseBuffer_.size() > 1000)
-		{
-			poseBuffer_.erase(poseBuffer_.begin());
-		}
+		camera_->poseReceived(pose, stamp);
 	}
 }
 
@@ -3704,14 +3773,14 @@ void RTABMapApp::postOdometryEvent(
 					cv::Mat yuv(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1);
 					memcpy(yuv.data, yPlane, yPlaneLen);
 					memcpy(yuv.data+yPlaneLen, vPlane, rgbHeight/2*rgbWidth);
-					cv::cvtColor(yuv, outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(yuv, outputRGB, cv::COLOR_YUV2BGR_NV21);
 				}
 				else
 				{
 #ifdef __ANDROID__
-					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2BGR_NV21);
 #else // __APPLE__
-                    cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, CV_YUV2RGB_NV21);
+					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2RGB_NV21);
 #endif
 				}
 
@@ -3769,66 +3838,41 @@ void RTABMapApp::postOdometryEvent(
 				{
 					pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 
+					rtabmap::Transform poseWithOriginOffset = pose;
+					if(!camera_->getOriginOffset().isNull())
+					{
+						poseWithOriginOffset = camera_->getOriginOffset() * pose;
+					}
+
 					// Registration depth to rgb
 					if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
 					{
 						UTimer time;
 						rtabmap::Transform motion = rtabmap::Transform::getIdentity();
-						if(depthStamp != stamp && !poseBuffer_.empty())
+						if(depthStamp != stamp)
 						{
 							// Interpolate pose
-							if(!poseBuffer_.empty())
+							rtabmap::Transform poseDepth;
+							cv::Mat cov;
+							if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
+							{
+								UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
+							}
+							else
 							{
-								if(poseBuffer_.rbegin()->first < depthStamp)
-								{
-									UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first);
-								}
-								else
-								{
-									std::map<double, rtabmap::Transform >::const_iterator iterB = poseBuffer_.lower_bound(depthStamp);
-									std::map<double, rtabmap::Transform >::const_iterator iterA = iterB;
-									rtabmap::Transform poseDepth;
-									if(iterA != poseBuffer_.begin())
-									{
-										iterA = --iterA;
-									}
-									if(iterB == poseBuffer_.end())
-									{
-										iterB = --iterB;
-									}
-									if(iterA == iterB && depthStamp == iterA->first)
-									{
-										poseDepth = iterA->second;
-									}
-									else if(depthStamp >= iterA->first && depthStamp <= iterB->first)
-									{
-										poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
-									}
-									else if(depthStamp < iterA->first)
-									{
-										UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first);
-									}
-									else
-									{
-										UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first);
-									}
-									if(!poseDepth.isNull())
-									{
 #ifndef DISABLE_LOG
-										UDEBUG("poseRGB  =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp);
-										UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
+								UDEBUG("poseRGB  =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp);
+								UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
 #endif
-										motion = pose.inverse()*poseDepth;
-										// transform in camera frame
+								motion = poseWithOriginOffset.inverse()*poseDepth;
+								// transform in camera frame
 #ifndef DISABLE_LOG
-										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+								UDEBUG("motion=%s", motion.prettyPrint().c_str());
 #endif
-										motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
+								motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
 #ifndef DISABLE_LOG
-										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+								UDEBUG("motion=%s", motion.prettyPrint().c_str());
 #endif
-									}
-								}
 							}
 						}
 						rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
@@ -3877,11 +3921,6 @@ void RTABMapApp::postOdometryEvent(
                     
                     if(!outputDepth.empty())
                     {
-                    	rtabmap::Transform poseWithOriginOffset = pose;
-                    	if(!camera_->getOriginOffset().isNull())
-                    	{
-                    		poseWithOriginOffset = camera_->getOriginOffset() * pose;
-                    	}
                         rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
                         depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
                         camera_->setOcclusionImage(outputDepth, depthModel);
@@ -3907,8 +3946,7 @@ void RTABMapApp::postOdometryEvent(
                     texCoords[5] = t5;
                     texCoords[6] = t6;
                     texCoords[7] = t7;
-					camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
-					camera_->spinOnce();
+					camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
 				}
 			}
 		}
@@ -3925,17 +3963,17 @@ void RTABMapApp::postOdometryEvent(
 
 bool RTABMapApp::handleEvent(UEvent * event)
 {
-	if(camera_!=0)
+	if(sensorCaptureThread_!=0)
 	{
 		// called from events manager thread, so protect the data
-		if(event->getClassName().compare("OdometryEvent") == 0)
+		if(event->getClassName().compare("SensorEvent") == 0)
 		{
-			LOGI("Received OdometryEvent!");
-			if(odomMutex_.try_lock())
+			LOGI("Received SensorEvent!");
+			if(sensorMutex_.try_lock())
 			{
-				odomEvents_.clear();
-				odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
-				odomMutex_.unlock();
+				sensorEvents_.clear();
+				sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event)));
+				sensorMutex_.unlock();
 			}
 		}
 		if(event->getClassName().compare("RtabmapEvent") == 0)

app/android/jni/RTABMapApp.h

@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "util.h"
 #include "ProgressionStatus.h"
 
+#include <rtabmap/core/SensorCaptureThread.h>
 #include <rtabmap/core/RtabmapThread.h>
+#include <rtabmap/core/SensorEvent.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <boost/thread/mutex.hpp>
 #include <pcl/pcl_base.h>
@@ -75,7 +77,7 @@ class RTABMapApp : public UEventsHandler {
 
   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 isBuiltWith(int cameraDriver) const;
 #ifdef __ANDROID__
@@ -153,6 +155,7 @@ class RTABMapApp : public UEventsHandler {
   void addEnvSensor(int type, float value);
 
   void save(const std::string & databasePath);
+  bool recover(const std::string & from, const std::string & to);
   void cancelProcessing();
   bool exportMesh(
 		  float cloudVoxelSize,
@@ -208,6 +211,7 @@ class RTABMapApp : public UEventsHandler {
  private:
   int cameraDriver_;
   rtabmap::CameraMobile * camera_;
+  rtabmap::SensorCaptureThread * sensorCaptureThread_;
   rtabmap::RtabmapThread * rtabmapThread_;
   rtabmap::Rtabmap * rtabmap_;
   rtabmap::LogHandler * logHandler_;
@@ -223,6 +227,7 @@ class RTABMapApp : public UEventsHandler {
   bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;
+  bool useExternalLidar_;
   float maxCloudDepth_;
   float minCloudDepth_;
   int cloudDensityLevel_;
@@ -265,18 +270,19 @@ 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::OdometryEvent> odomEvents_;
+	std::list<rtabmap::SensorEvent> sensorEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
-	std::map<double, rtabmap::Transform> poseBuffer_;
 
 	rtabmap::Transform mapToOdom_;
 
 	boost::mutex cameraMutex_;
 	boost::mutex rtabmapMutex_;
 	boost::mutex meshesMutex_;
-	boost::mutex odomMutex_;
+	boost::mutex sensorMutex_;
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;
 

app/android/jni/background_renderer.cc

@@ -155,7 +155,7 @@ void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
 }
 
 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");
+  static_assert(std::extent<decltype(BackgroundRenderer_kVerticesDevice)>::value == kNumVertices * 2, "Incorrect kVertices length");
 
   GLuint program = shaderPrograms_[depthTexture>0?1:0];
     
@@ -170,7 +170,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
   else
 #endif
     glBindTexture(GL_TEXTURE_2D, texture_id_);
-    
+
   if(depthTexture>0)
   {
     // Texture activate unit 1
@@ -191,7 +191,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
   GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
   GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
 
-  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice);
   glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
 
   glEnableVertexAttribArray(attributeVertices);

app/android/jni/background_renderer.h

@@ -28,9 +28,15 @@
 
 #include "util.h"
 
-static const GLfloat BackgroundRenderer_kVertices[] = {
+static const GLfloat BackgroundRenderer_kVerticesDevice[] = {
 	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
 };
+//static const GLfloat BackgroundRenderer_kVerticesView[] = {
+//	0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f,
+//};
+static const GLfloat BackgroundRenderer_kVerticesView[] = {
+	0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+};
 static const GLfloat BackgroundRenderer_kTexCoord[] = {
    1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
 };

app/android/jni/jni_interface.cpp

@@ -89,13 +89,13 @@ Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(
 
 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
-    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize)
+    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
 	std::string databasePathC;
 	GetJStringContent(env,databasePath,databasePathC);
 	if(native_application)
 	{
-		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize);
+		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, clearDatabase);
 	}
 	else
 	{
@@ -104,17 +104,17 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	}
 }
 
-JNIEXPORT int JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
-    JNIEnv* env, jclass, jlong native_application, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_recover(
+    JNIEnv* env, jclass, jlong native_application, jstring from, jstring to)
 {
 	if(native_application)
 	{
-		std::string databasePathC;
-		GetJStringContent(env,databasePath,databasePathC);
-		std::string databaseSourceC;
-		GetJStringContent(env,databaseSource,databaseSourceC);
-		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+		std::string toC;
+		GetJStringContent(env,to,toC);
+		std::string fromC;
+		GetJStringContent(env,from,fromC);
+		return native(native_application)->recover(fromC, toC);
 	}
 	else
 	{

app/android/jni/point_cloud_drawable.cpp

@@ -309,8 +309,8 @@ PointCloudDrawable::PointCloudDrawable(
 		float gainR,
 		float gainG,
 		float gainB) :
-				vertex_buffers_(0),
-				textures_(0),
+				vertex_buffer_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -320,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 rtabmap::Mesh & mesh,
 		bool createWireframe) :
-				vertex_buffers_(0),
-				textures_(0),
+				vertex_buffer_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -337,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_);
-	if (vertex_buffers_)
+	LOGI("Freeing cloud buffer %d", vertex_buffer_);
+	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)
 {
+    for(int i=0; i<4; ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updatePolygons() clearing polygon buffers");
+        }
+    }
+    
 	//LOGD("Update polygons");
-	polygons_.clear();
-	polygonLines_.clear();
-	polygonsLowRes_.clear();
-	polygonLinesLowRes_.clear();
 	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]);
-					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
+                    indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+                    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)
@@ -402,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]);
-						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
+                        indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+                        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;
+                }
+            }
+        }
 	}
 }
 
@@ -418,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_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		vertex_buffers_ = 0;
+		glDeleteBuffers(1, &vertex_buffer_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear vertex buffer");
+		vertex_buffer_ = 0;
 	}
 
-	if (textures_)
+	if (texture_)
 	{
-		glDeleteTextures(1, &textures_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		textures_ = 0;
+		glDeleteTextures(1, &texture_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear texture buffer");
+		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_);
-	if(!vertex_buffers_)
+	glGenBuffers(1, &vertex_buffer_);
+	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);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.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)
@@ -471,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);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowRes.resize(oi_low);
+		verticesLowLowRes.resize(oi_lowlow);
 	}
 	else
 	{
 		totalPoints = cloud->size();
 		vertices.resize(cloud->size()*4);
-		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.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)
@@ -504,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);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowRes.resize(oi_low);
+		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);
 
@@ -524,11 +582,40 @@ 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 rtabmap::Mesh & mesh, bool createWireframe)
@@ -538,12 +625,22 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
 	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
 
-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		vertex_buffers_ = 0;
+		glDeleteBuffers(1, &vertex_buffer_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear vertex buffer");
+		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];
@@ -552,17 +649,17 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	bool textureUpdate = false;
 	if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
 	{
-		if (textures_)
+		if (texture_)
 		{
-			glDeleteTextures(1, &textures_);
-			tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-			textures_ = 0;
+			glDeleteTextures(1, &texture_);
+			tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear texture buffer");
+			texture_ = 0;
 		}
 		textureUpdate = true;
 	}
 
-	glGenBuffers(1, &vertex_buffers_);
-	if(!vertex_buffers_)
+	glGenBuffers(1, &vertex_buffer_);
+	if(!vertex_buffer_)
 	{
 		LOGE("OpenGL: could not generate vertex buffers\n");
 		return;
@@ -570,10 +667,10 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 
 	if(textureUpdate)
 	{
-		glGenTextures(1, &textures_);
-		if(!textures_)
+		glGenTextures(1, &texture_);
+		if(!texture_)
 		{
-			vertex_buffers_ = 0;
+			vertex_buffer_ = 0;
 			LOGE("OpenGL: could not generate texture buffers\n");
 			return;
 		}
@@ -591,11 +688,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
 		organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
 		totalPoints = (int)mesh.indices->size();
-		verticesLowRes_.resize(totalPoints);
-		verticesLowLowRes_.resize(totalPoints);
+        std::vector<GLuint> verticesLowRes;
+        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);
@@ -628,11 +727,11 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 
 				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;
 				}
 			}
 		}
@@ -663,20 +762,48 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 
 				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);
-		verticesLowLowRes_.resize(oi_lowlow);
+        verticesLowRes.resize(oi_low);
+        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())
+		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);
@@ -743,7 +870,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		}
 		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);
@@ -771,7 +898,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		}
 	}
 
-	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);
 
@@ -779,11 +906,11 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	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);
@@ -793,13 +920,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		//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);
@@ -811,20 +938,17 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		if(error != GL_NO_ERROR)
 		{
 			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
-			textures_ = 0;
+			texture_ = 0;
 
-			glDeleteBuffers(1, &vertex_buffers_);
-			vertex_buffers_ = 0;
+			glDeleteBuffers(1, &vertex_buffer_);
+			vertex_buffer_ = 0;
 			return;
 		}
 	}
 
 	nPoints_ = totalPoints;
 
-	if(polygons_.size() != polygons.size())
-	{
-		updatePolygons(polygons, polygonsLowRes, createWireframe);
-	}
+    updatePolygons(polygons, polygonsLowRes, createWireframe);
 
 	if(!pose_.isNull())
 	{
@@ -886,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;
 		}
@@ -996,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);
@@ -1012,8 +1136,8 @@ void PointCloudDrawable::Render(
 		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");
 
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		if(textures_)
+		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
+		if(texture_)
 		{
 			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
 			if(textureRendering)
@@ -1044,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();
-				if(wireFrame)
-					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());
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[5]);
+                glDrawElements(GL_POINTS, index_buffers_count_[5], GL_UNSIGNED_INT, 0);
 			}
 			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
 			{
@@ -1106,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

@@ -70,8 +70,8 @@ private:
   rtabmap::Transform getPose() const {return pose_;}
   const glm::mat4 & getPoseGl() const {return poseGl_;}
   bool isVisible() const {return visible_;}
-  bool hasMesh() const {return polygons_.size()!=0;}
-  bool hasTexture() const {return textures_ != 0;}
+  bool hasMesh() const {return index_buffers_[0] != 0;}
+  bool hasTexture() const {return texture_ != 0;}
   float getMinHeight() const {return minHeight_;}
   const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
   const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
@@ -115,14 +115,10 @@ private:
 
  private:
   // Vertex buffer of the point cloud geometry.
-  GLuint vertex_buffers_;
-  GLuint textures_;
-  std::vector<GLuint> polygons_;
-  std::vector<GLuint> polygonsLowRes_;
-  std::vector<GLuint> polygonLines_;
-  std::vector<GLuint> polygonLinesLowRes_;
-  std::vector<GLuint> verticesLowRes_;
-  std::vector<GLuint> verticesLowLowRes_;
+  GLuint vertex_buffer_;
+  GLuint texture_;
+  std::vector<GLuint> index_buffers_;
+  std::vector<int> index_buffers_count_;
   int nPoints_;
   rtabmap::Transform pose_;
   glm::mat4 poseGl_;

app/android/jni/scene.cpp

@@ -102,7 +102,7 @@ Scene::Scene() :
 		screenHeight_(0),
 		doubleTapOn_(false)
 {
-    depthTextures_[0] = depthTextures_[1] = 0;
+    depthTexture_ = 0;
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
 	      tango_gl::GestureCamera::kThirdPersonFollow);
@@ -179,8 +179,8 @@ void Scene::DeleteResources() {
 		fboId_ = 0;
 		glDeleteRenderbuffers(1, &rboId_);
 		rboId_ = 0;
-        glDeleteTextures(2, depthTextures_);
-        depthTextures_[0] = depthTextures_[1] = 0;
+        glDeleteTextures(1, &depthTexture_);
+        depthTexture_ = 0;
 	}
 
 	clear();
@@ -234,8 +234,8 @@ void Scene::SetupViewPort(int w, int h) {
 			fboId_ = 0;
 			glDeleteRenderbuffers(1, &rboId_);
 			rboId_ = 0;
-			glDeleteTextures(2, depthTextures_);
-            depthTextures_[0] = depthTextures_[1] = 0;
+			glDeleteTextures(1, &depthTexture_);
+            depthTexture_ = 0;
 		}
 
         GLint originid = 0;
@@ -247,8 +247,8 @@ void Scene::SetupViewPort(int w, int h) {
 		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
 
 		// Create depth texture
-		glGenTextures(2, depthTextures_);
-		glBindTexture(GL_TEXTURE_2D, depthTextures_[0]);
+		glGenTextures(1, &depthTexture_);
+		glBindTexture(GL_TEXTURE_2D, depthTexture_);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@@ -256,21 +256,13 @@ void Scene::SetupViewPort(int w, int h) {
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
 		glBindTexture(GL_TEXTURE_2D, 0);
         
-        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-        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_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);
 
 		// Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
-		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTextures_[0], 0);
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
 		glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
 
 		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
@@ -349,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 b An axis aligned bounding box.
+ * @param planes Viewing frustum.
+ * @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(
@@ -389,7 +382,8 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping) {
+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)
@@ -428,7 +422,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 
 	if(renderBackgroundCamera)
 	{
-        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.2)
+        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.3)
         {
             projectionMatrix = arProjectionMatrix;
             viewMatrix = arViewMatrix;
@@ -486,9 +480,15 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		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 = (!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_);
 	if(onlineBlending && fboId_)
 	{
         GLint originid = 0;
@@ -499,7 +499,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 
 		glClearColor(0, 0, 0, 0);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
+        
         // Draw scene
         for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
         {
@@ -511,16 +511,12 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
             (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
         }
         
-        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
-        glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, screenWidth_, screenHeight_);
-        glBindTexture(GL_TEXTURE_2D, 0);
-        
         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, originid); // unbind
 	}
@@ -559,7 +555,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
     
-    if(renderBackgroundCamera && !onlineBlending)
+    if(renderBackgroundCamera && (!onlineBlending || !meshRendering_))
     {
         background_renderer_->Draw(uvsTransformed, 0, screenWidth_, screenHeight_, false);
 
@@ -594,6 +590,10 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+        else
+        {
+            trace_->ClearVertexArray();
+        }
 	}
 
 	if(gridVisible_ && !renderBackgroundCamera)
@@ -629,14 +629,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 				cloud->getPose().z() - openglCamera.z());
 		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
 
-		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTextures_[0]:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}
 
 	if(onlineBlending)
 	{
-        if(renderBackgroundCamera)
+        if(renderBackgroundCamera && meshRendering_)
         {
-            background_renderer_->Draw(uvsTransformed, depthTextures_[1], screenWidth_, screenHeight_, meshRendering_?mapping:false);
+            background_renderer_->Draw(uvsTransformed, depthTexture_, screenWidth_, screenHeight_, mapping);
         }
         
 		glDisable (GL_BLEND);

app/android/jni/scene.h

@@ -208,7 +208,7 @@ class Scene {
   float b_;
   GLuint fboId_;
   GLuint rboId_;
-  GLuint depthTextures_[2]; // 0=objects+occlusion 1=objects only
+  GLuint depthTexture_; // 0=objects+occlusion
   GLsizei screenWidth_;
   GLsizei screenHeight_;
   bool doubleTapOn_;

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

@@ -183,7 +183,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
     case kFirstPerson:
       SetOrthoMode(false);
       SetFieldOfView(kLowestFov);
-      SetNearFarClipPlanes(0.1, 50);
+      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;

app/android/libs/.gitignore

@@ -1 +1,4 @@
-*.jar
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore

app/android/res/values/strings.xml

@@ -19,7 +19,7 @@
     <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>
@@ -80,11 +80,11 @@
     <string name="pref_default_fisheye">false</string>
     
     <string name="pref_key_camera_driver">pref_key_camera_driver</string>
-    <string name="pref_default_camera_driver">0</string>
+    <string name="pref_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_default_arcore_localization_filtering_speed">0</string>
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
     <string name="pref_key_max_speed">pref_key_max_speed</string>
@@ -108,7 +108,7 @@
     <string name="pref_key_features_type">pref_key_features_type</string>
     <string name="pref_default_features_type">6</string>
     <string name="pref_key_optimizer">pref_key_optimizer</string>
-    <string name="pref_default_optimizer">1</string>
+    <string name="pref_default_optimizer">2</string>
     <string name="pref_key_optimize_end">pref_key_optimize_end</string>
     <string name="pref_default_optimize_end">true</string>
     <string name="pref_key_marker_detection">pref_key_marker_detection</string>
@@ -381,12 +381,14 @@
     <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>

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

@@ -115,6 +115,9 @@ public class ARCoreSharedCamera {
 	// Image reader that continuously processes CPU images.
 	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
 	private boolean mTOFAvailable = false;
+
+	ByteBuffer mPreviousDepth = null;
+	double mPreviousDepthStamp = 0.0;
 	
 	public boolean isDepthSupported() {return mTOFAvailable;}
 
@@ -688,7 +691,7 @@ public class ARCoreSharedCamera {
 					if(mToast!=null && previousAnchorPose != null)
 					{
 						String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
-						if(!mToast.getView().isShown())
+						if(mToast.getView() == null || !mToast.getView().isShown())
 						{
 							mToast.makeText(mActivity.getApplicationContext(), 
 									msg, Toast.LENGTH_LONG).show();
@@ -698,6 +701,7 @@ public class ARCoreSharedCamera {
 							mToast.setText(msg);
 						}
 						previousAnchorPose = null;
+						arCoreCorrection = Pose.IDENTITY;
 					}
 				}
 			});
@@ -718,7 +722,8 @@ public class ARCoreSharedCamera {
 				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());
+					// Only correct the translation to not lose rotation aligned with gravity
+					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose.compose(pose.inverse()).extractTranslation());
 					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]));
 					
@@ -736,7 +741,7 @@ public class ARCoreSharedCamera {
 										+ "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().isShown())
+								if(mToast.getView() == null || !mToast.getView().isShown())
 								{
 									mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
 								}
@@ -744,7 +749,6 @@ public class ARCoreSharedCamera {
 								{
 									mToast.setText(msg);
 								}
-								previousAnchorPose = null;
 							}
 						}
 					});
@@ -755,9 +759,8 @@ public class ARCoreSharedCamera {
 			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));
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f arcore %f %f %f cor= %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), pose.tx(), pose.ty(), pose.tz(), arCoreCorrection.tx(), arCoreCorrection.ty(), arCoreCorrection.tz(), 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();
@@ -813,6 +816,12 @@ public class ARCoreSharedCamera {
 						depth = mTOFImageReader.depth16_raw;
 						depthStamp = (double)mTOFImageReader.timestamp/10e8;
 					}
+
+					if(mPreviousDepth == null)
+					{
+						mPreviousDepth = depth;
+						mPreviousDepthStamp = depthStamp;
+					}
 					
 					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));
@@ -825,13 +834,17 @@ public class ARCoreSharedCamera {
 							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,
+							depthStamp>stamp?mPreviousDepthStamp:depthStamp,
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():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]);
+					
+					
+					mPreviousDepthStamp = depthStamp;
+					mPreviousDepth = depth;
 				}
 				else
 				{

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

@@ -2,6 +2,7 @@ package com.introlab.rtabmap;
 
 import java.io.File;
 import java.io.FileInputStream;
+import java.io.FileNotFoundException;
 import java.io.FileOutputStream;
 import java.io.IOException;
 import java.io.InputStream;
@@ -25,6 +26,8 @@ import android.app.NotificationManager;
 import android.app.PendingIntent;
 import android.app.ProgressDialog;
 import android.content.ComponentName;
+import android.content.ContentResolver;
+import android.content.ContentValues;
 import android.content.Context;
 import android.content.DialogInterface;
 import android.content.DialogInterface.OnShowListener;
@@ -35,6 +38,7 @@ import android.content.pm.ApplicationInfo;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
+import android.database.Cursor;
 import android.hardware.Camera;
 import android.hardware.Sensor;
 import android.hardware.SensorEvent;
@@ -46,6 +50,7 @@ import android.hardware.display.DisplayManager;
 import android.location.Location;
 import android.location.LocationListener;
 import android.location.LocationManager;
+import android.media.MediaScannerConnection;
 import android.net.Uri;
 import android.net.wifi.WifiInfo;
 import android.net.wifi.WifiManager;
@@ -56,6 +61,8 @@ import android.os.Handler;
 import android.os.IBinder;
 import android.os.Message;
 import android.preference.PreferenceManager;
+import android.provider.MediaStore;
+import android.provider.OpenableColumns;
 import android.support.v4.app.FragmentActivity;
 import android.support.v4.content.FileProvider;
 import android.text.InputType;
@@ -118,7 +125,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 	public static final String RTABMAP_TMP_DB = "rtabmap.tmp.db";
 	public static final String RTABMAP_TMP_DIR = "tmp";
 	public static final String RTABMAP_TMP_FILENAME = "map";
-	public static final String RTABMAP_SDCARD_PATH = "/sdcard/";
+	public static final String RTABMAP_SDCARD_PATH = "/Internal storage/";
 	public static final String RTABMAP_EXPORT_DIR = "Export/";
 
 	public static final String RTABMAP_AUTH_TOKEN_KEY = "com.introlab.rtabmap.AUTH_TOKEN";
@@ -256,6 +263,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 	ARCoreSharedCamera mArCoreCamera = null;
 	int mCameraDriver = 0;
+	
+	private String mIntentDbToOpen = null;
 
 	//Tango Service connection.
 	boolean mCameraServiceConnectionUsed = false; 
@@ -494,10 +503,29 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mWorkingDirectoryHuman = "";
 		mTotalLoopClosures = 0;
 		mLastFastMovementNotificationStamp = System.currentTimeMillis()/1000;
-
-		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0)
+		
+		
+		int targetSdkVersion= 0;
+		try {
+	        ApplicationInfo app = this.getPackageManager().getApplicationInfo("com.introlab.rtabmap", 0);        
+	        targetSdkVersion = app.targetSdkVersion;
+	    } catch (NameNotFoundException e) {
+	        e.printStackTrace();
+	    }
+	
+		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0 && 
+			(targetSdkVersion < 30 || getActivity().getExternalFilesDirs(null).length >=1))
 		{
-			File extStore = Environment.getExternalStorageDirectory();
+			File extStore;
+			if(targetSdkVersion < 30)
+			{
+				extStore = Environment.getExternalStorageDirectory();
+			}
+			else // >= android30
+			{
+				extStore = getActivity().getExternalFilesDirs(null)[0];
+			}
+			
 			mWorkingDirectory = extStore.getAbsolutePath() + "/" + getString(R.string.app_name) + "/";
 			extStore = new File(mWorkingDirectory);
 			extStore.mkdirs();
@@ -507,7 +535,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		{
 			// show warning that data cannot be saved!
 			mToast.makeText(getApplicationContext(), 
-					String.format("Failed to get external storage path (SD-CARD, state=%s). Saving disabled.", 
+					String.format("Failed to get external storage path (state=%s). Saving disabled.", 
 							Environment.getExternalStorageState()), mToast.LENGTH_LONG).show();
 		}
 
@@ -574,33 +602,141 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		DISABLE_LOG =  !( 0 != ( getApplicationInfo().flags & ApplicationInfo.FLAG_DEBUGGABLE ) );
 
-		if (!PermissionHelper.hasPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
-			PermissionHelper.requestPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE);
-		}
-		else
-		{
-			postCreate();
-		}
-
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 		
 		isArCoreAvailable();
 		isArEngineAvailable();
+		
+		if (!PermissionHelper.hasPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
+			PermissionHelper.requestPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE);
+		}
+		else
+		{
+			// Get intent, action and MIME type
+		    Intent intent = getIntent();
+		    String action = intent.getAction();
+		    String type = intent.getType();
+
+		    if (Intent.ACTION_SEND.equals(action) && type != null) {
+		        if ("application/octet-stream".equals(type)) {
+		        	Uri imageUri = (Uri) intent.getParcelableExtra(Intent.EXTRA_STREAM);
+		            if (imageUri != null) {
+		               String fileName = getFileName(imageUri);
+		               Log.i(TAG, "Intent received: " + imageUri.getPath() + " Name:" + fileName);
+		               if(fileName.endsWith(".db"))
+		               {
+			               File file = new File(mWorkingDirectory+fileName);
+			               if(file.exists())
+			               {
+			            	   mToast.makeText(this, fileName + " already exists in RTAB-Map's library! Cannot be copied.", mToast.LENGTH_LONG).show();
+			               }
+			               else
+			               {
+				               copy(imageUri, file);
+				               mIntentDbToOpen = fileName;
+			               }
+		               }
+		            }
+		        }
+		    } else if (Intent.ACTION_SEND_MULTIPLE.equals(action) && type != null) {
+		    	if (type.startsWith("application/")) {
+		    		ArrayList<Uri> imageUris = intent.getParcelableArrayListExtra(Intent.EXTRA_STREAM);
+		    		if (imageUris != null) {
+		    			boolean added = false;
+		    			for(Uri imageUri: imageUris)
+		    			{
+		    				String fileName = getFileName(imageUri);
+		    				Log.i(TAG, "Intent received: " + imageUri.getPath() + " Name:" + fileName);
+		    				if(fileName.endsWith(".db"))
+		    				{
+		    					File file = new File(mWorkingDirectory+"/"+getFileName(imageUri));
+		    					if(!file.exists())
+		    					{
+			    					copy(imageUri, file);
+			    					added = true;
+		    					}
+		    					else
+		    					{
+		    						Log.e(TAG,  fileName + " already exists in RTAB-Map's library! Cannot be copied.");
+		    					}
+		    				}
+		    			}
+		    			if(added)
+		    			{
+		    				openDatabase();
+		    			}
+		    		}
+
+		    	}
+		    }
+
+			postCreate();
+		}
 	}
 	
+	public void copy(File src, File dst) throws IOException {
+		InputStream in = new FileInputStream(src);
+		OutputStream out = new FileOutputStream(dst);
+
+		// Transfer bytes from in to out
+		byte[] buf = new byte[1024];
+		int len;
+		while ((len = in.read(buf)) > 0) {
+			out.write(buf, 0, len);
+		}
+		in.close();
+		out.close();
+	}
+	
+	public void copy(Uri uri, File file)
+	{
+		InputStream in;
+		try {
+			in = getApplicationContext().getContentResolver().openInputStream(uri);
+			
+			OutputStream out = new FileOutputStream(file);
+    		byte[] buf = new byte[1024];
+    		int len;
+    		while ((len = in.read(buf)) > 0) {
+    			out.write(buf, 0, len);
+    		}
+    		in.close();
+    		out.close();
+		} catch (IOException e) {
+			Log.e(TAG, e.getMessage());
+		}
+	}
+	
+	public String getFileName(Uri uri) {
+		  String result = null;
+		  if (uri.getScheme().equals("content")) {
+		    Cursor cursor = getContentResolver().query(uri, null, null, null, null);
+		    try {
+		      if (cursor != null && cursor.moveToFirst()) {
+		        result = cursor.getString(cursor.getColumnIndex(OpenableColumns.DISPLAY_NAME));
+		      }
+		    } finally {
+		      cursor.close();
+		    }
+		  }
+		  if (result == null) {
+		    result = uri.getPath();
+		    int cut = result.lastIndexOf('/');
+		    if (cut != -1) {
+		      result = result.substring(cut + 1);
+		    }
+		  }
+		  return result;
+		}
+
+	
 	// Should be called only if read/write permissions are granted!
 	private void postCreate()
 	{
 		Log.i(TAG, "postCreate()");
 		
-		String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-		(new File(tmpDatabase)).delete();
-		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
-		boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
-		RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
-		
 		final String[] files = Util.loadFileList(mWorkingDirectory, true);
 		if(files.length == 0)
 		{
@@ -613,6 +749,17 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Log.i(TAG, String.format("updateCameraDriverSettings() mCameraDriver=%d RTABMapLib.isBuiltWith(%d)=%d", mCameraDriver, mCameraDriver, RTABMapLib.isBuiltWith(nativeApplication, mCameraDriver)?1:0));
 		
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
+		mCameraDriver = Integer.parseInt(cameraDriverStr);
+		
+		if(mCameraDriver == -1)
+		{
+			// Prioritize tango if available
+			mCameraDriver = 0;
+			SharedPreferences.Editor editor = sharedPref.edit();
+			editor.putString(getString(R.string.pref_key_camera_driver), "0");
+			editor.commit();
+		}
 		
 		if(mCameraDriver == 0 && (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION) || !RTABMapLib.isBuiltWith(nativeApplication, 0)))
 		{
@@ -1167,7 +1314,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		final boolean depthFromMotion = sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion)));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 
-		if(!DISABLE_LOG) Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
+		Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
 		if(mCameraDriver == 0) // Tango
 		{
 			// Check if the Tango Core is out dated.
@@ -1490,7 +1637,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}
 
 		updateState(mState);
-
+		
 		return true;
 	}
 
@@ -1992,6 +2139,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				final int loopDetected = RTABMapLib.postProcessing(nativeApplication, -1);
 				runOnUiThread(new Runnable() {
 					public void run() {
+						updateState(State.STATE_IDLE);
 						if(mExportProgressDialog.isShowing())
 						{
 							mExportProgressDialog.dismiss();
@@ -2024,8 +2172,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							mProgressDialog.dismiss();
 							mToast.makeText(getActivity(), String.format("Optimization canceled"), mToast.LENGTH_LONG).show();
 						}
-
-						updateState(State.STATE_IDLE);
 					}
 				});
 			} 
@@ -2144,7 +2290,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			mButtonCloseVisualization.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
 			mButtonCloseVisualization.setEnabled(true);
 			mButtonSaveOnDevice.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
-			mButtonShareOnSketchfab.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
+			//mButtonShareOnSketchfab.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
 			mButtonLibrary.setVisibility(View.INVISIBLE);
 			mButtonNewScan.setVisibility(View.INVISIBLE);
 			mItemSave.setEnabled(mState != State.STATE_VISUALIZING_CAMERA);
@@ -2217,6 +2363,12 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		{
 			mGLView.setRenderMode(GLSurfaceView.RENDERMODE_CONTINUOUSLY);
 		}
+		
+		if(mState == State.STATE_WELCOME && mIntentDbToOpen != null)
+		{
+			openDatabase(mIntentDbToOpen, false);
+			mIntentDbToOpen = null;
+		}
 	}
 
 	private void startMapping() {
@@ -2626,7 +2778,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
 							boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
 							String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-							RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
+							RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);
 
 							mItemLocalizationMode.setEnabled(!mItemDataRecorderMode.isChecked());		   
 
@@ -2780,16 +2932,110 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mOpenedDatabasePath = "";
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
-		String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-		RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
-
-		if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))
+		final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
+		
+		File newFile = new File(tmpDatabase);
+		final int fileSizeMB = (int)newFile.length()/(1024 * 1024);
+		if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING) &&
+			newFile.exists() && 
+			fileSizeMB>1) // >1MB
 		{
-			setCamera(1);
-			startCamera(String.format("Hold Tight! Initializing Camera Service...\n"
-					+ "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\"."));
+			AlertDialog d2 = new AlertDialog.Builder(getActivity())
+					.setCancelable(false)
+					.setTitle("Recovery")
+					.setMessage(String.format("The previous session (%d MB) was not correctly saved, do you want to recover it?", fileSizeMB))
+					.setNegativeButton("Ignore", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							(new File(tmpDatabase)).delete();
+							newScan();
+						}
+					})
+					.setNeutralButton("Cancel", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							// do nothing
+						}
+					})
+					.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							final String fileName = new SimpleDateFormat("yyMMdd-HHmmss").format(new Date()) + ".db";
+							final String outputDbPath = mWorkingDirectory + fileName;
+							
+							mExportProgressDialog.setTitle("Recovering");
+							mExportProgressDialog.setMessage(String.format("Please wait while recovering data..."));
+							mExportProgressDialog.setProgress(0);
+
+							final State previousState = mState;
+
+							mExportProgressDialog.show();
+							updateState(State.STATE_PROCESSING);
+
+							Thread exportThread = new Thread(new Runnable() {
+								public void run() {
+
+									final long startTime = System.currentTimeMillis()/1000;
+
+									final boolean success = RTABMapLib.recover(
+											nativeApplication, 
+											tmpDatabase,
+											outputDbPath);
+									runOnUiThread(new Runnable() {
+										public void run() {
+											if(mExportProgressDialog.isShowing())
+											{							
+												if(success)
+												{								
+													AlertDialog d2 = new AlertDialog.Builder(getActivity())
+															.setCancelable(false)
+															.setTitle("Database saved!")
+															.setMessage(String.format("Database \"%s\" (%d MB) successfully saved!", fileName, fileSizeMB))
+															.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+																public void onClick(DialogInterface dialog, int which) {
+																	openDatabase(fileName, false);
+																}
+															})
+															.create();
+													d2.setCanceledOnTouchOutside(true);
+													d2.show();
+												}
+												else
+												{
+													updateState(previousState);
+													mToast.makeText(getActivity(), String.format("Recovery failed!"), mToast.LENGTH_LONG).show();
+												}
+												mExportProgressDialog.dismiss();
+											}
+											else
+											{
+												mToast.makeText(getActivity(), String.format("Recovery canceled"), mToast.LENGTH_LONG).show();
+												updateState(previousState);
+											}
+										}
+									});
+								} 
+							});
+							exportThread.start();
+							
+							refreshSystemMediaScanDataBase(getActivity(), outputDbPath);
+						}
+					})
+					.create();
+			d2.setCanceledOnTouchOutside(false);
+			d2.show();
+		}
+		else
+		{
+		
+			RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);
+	
+			if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))
+			{
+				setCamera(0);
+				startCamera(String.format("Hold Tight! Initializing Camera Service...\n"
+						+ "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\"."));
+			}
 		}
 	}
+		
 
 	private void openDatabase()
 	{
@@ -3215,7 +3461,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		context.sendBroadcast(mediaScanIntent);
 	}
 
-	private void saveDatabase(String fileName)
+	private void saveDatabase(final String fileName)
 	{
 		final String newDatabasePath = mWorkingDirectory + fileName + ".db";
 		final String newDatabasePathHuman = mWorkingDirectoryHuman + fileName + ".db";
@@ -3247,7 +3493,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							mSavedStamp = System.currentTimeMillis();
 							msg = String.format("Database saved to \"%s\".", newDatabasePathHuman);	
 						}
-
+									            
 						// build notification
 						Intent intent = new Intent(getActivity(), RTABMapActivity.class);
 						// use System.currentTimeMillis() to have a unique ID for the pending intent
@@ -3280,7 +3526,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 						AlertDialog d2 = new AlertDialog.Builder(getActivity())
 								.setCancelable(false)
 								.setTitle("Database saved!")
-								.setMessage(String.format("Database \"%s\" (%d MB) successfully saved on the SD-CARD!", newDatabasePathHuman, fileSizeMB))
+								.setMessage(String.format("Database \"%s\" (%d MB) successfully saved!", newDatabasePathHuman, fileSizeMB))
 								.setPositiveButton("OK", new DialogInterface.OnClickListener() {
 									public void onClick(DialogInterface dialog, int which) {
 										resetNoTouchTimer(true);
@@ -3334,30 +3580,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				dialog.dismiss();
 				if(!fileName.isEmpty())
 				{
-					File newFile = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + fileName + ".zip");
-					if(newFile.exists())
-					{
-						AlertDialog ad = new AlertDialog.Builder(getActivity())
-								.setCancelable(false)
-								.setTitle("File Already Exists")
-								.setMessage("Do you want to overwrite the existing file?")
-								.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-									public void onClick(DialogInterface dialog, int which) {
-										writeExportedFiles(fileName);
-									}
-								})
-								.setNegativeButton("No", new DialogInterface.OnClickListener() {
-									public void onClick(DialogInterface dialog, int which) {
-										saveOnDevice();
-									}
-								}).create();
-						ad.setCanceledOnTouchOutside(false);
-						ad.show();
-					}
-					else
-					{
-						writeExportedFiles(fileName);
-					}
+					writeExportedFiles(fileName);
 				}
 			}
 		});
@@ -3371,7 +3594,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 	{		
 		Log.i(TAG, String.format("Write exported mesh to \"%s\"", fileName));
 
-		mProgressDialog.setTitle("Saving to sd-card");
+		mProgressDialog.setTitle("Exporting");
 		mProgressDialog.setMessage(String.format("Compressing the files..."));
 		mProgressDialog.show();
 
@@ -3397,7 +3620,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				}
 				File exportDir = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR);
 				exportDir.mkdirs();
-
+				
 				final String pathHuman = mWorkingDirectoryHuman + RTABMAP_EXPORT_DIR + fileName + ".zip";
 				final String zipOutput = mWorkingDirectory+RTABMAP_EXPORT_DIR+fileName+".zip";
 				if(RTABMapLib.writeExportedMesh(nativeApplication, mWorkingDirectory + RTABMAP_TMP_DIR, RTABMAP_TMP_FILENAME))
@@ -3424,7 +3647,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							final String msg = e.getMessage();
 							runOnUiThread(new Runnable() {
 								public void run() {
-									mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! Error=%s", pathHuman, msg), mToast.LENGTH_LONG).show();
+									mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! Error=%s", fileName, msg), mToast.LENGTH_LONG).show();
 								}
 							});
 						}
@@ -3443,13 +3666,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							AlertDialog d = new AlertDialog.Builder(getActivity())
 									.setCancelable(false)
 									.setTitle("Mesh Saved!")
-									.setMessage(String.format("Mesh \"%s\" (%d MB) successfully exported on the SD-CARD! Share it?", pathHuman, fileSizeMB))
+									.setMessage(String.format("Mesh \"%s\" (%d MB) successfully exported! Share it?", pathHuman, fileSizeMB))
 									.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 										public void onClick(DialogInterface dialog, int which) {
 											// Send to...
 											Intent shareIntent = new Intent();
 											shareIntent.setAction(Intent.ACTION_SEND);
-											shareIntent.putExtra(Intent.EXTRA_STREAM, Uri.fromFile(f));
+											shareIntent.putExtra(Intent.EXTRA_STREAM, FileProvider.getUriForFile(getActivity(), getActivity().getApplicationContext().getPackageName() + ".provider", f));
+											shareIntent.addFlags(Intent.FLAG_GRANT_READ_URI_PERMISSION);
 											shareIntent.setType("application/zip");
 											startActivity(Intent.createChooser(shareIntent, "Sharing..."));
 
@@ -3471,7 +3695,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 					runOnUiThread(new Runnable() {
 						public void run() {
 							mProgressDialog.dismiss();
-							mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! No files found in tmp directory!? Last export may have failed or have been canceled.", pathHuman), mToast.LENGTH_LONG).show();
+							mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! No files found in tmp directory!? Last export may have failed or have been canceled.", fileName), mToast.LENGTH_LONG).show();
 							resetNoTouchTimer(true);
 						}
 					});
@@ -3498,8 +3722,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Thread openThread = new Thread(new Runnable() {
 			public void run() {
 
-				final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;				
-				final int status = RTABMapLib.openDatabase2(nativeApplication, mOpenedDatabasePath, tmpDatabase, databaseInMemory, optimize);
+				final int status = RTABMapLib.openDatabase(nativeApplication, mOpenedDatabasePath, databaseInMemory, optimize, false);
 
 				runOnUiThread(new Runnable() {
 					public void run() {
@@ -3578,20 +3801,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		openThread.start();	
 	}
 
-	public void copy(File src, File dst) throws IOException {
-		InputStream in = new FileInputStream(src);
-		OutputStream out = new FileOutputStream(dst);
-
-		// Transfer bytes from in to out
-		byte[] buf = new byte[1024];
-		int len;
-		while ((len = in.read(buf)) > 0) {
-			out.write(buf, 0, len);
-		}
-		in.close();
-		out.close();
-	}
-
 	private void shareToSketchfab()
 	{
 		if (!PermissionHelper.hasPermission(this, Manifest.permission.INTERNET)) {

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

@@ -34,9 +34,10 @@ public class RTABMapLib
     
     public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
     
-    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize);
-    public static native int openDatabase2(long nativeApplication, String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize, boolean clearDatabase);
     
+    public static native boolean recover(long nativeApplication, String from, String to);
+
     public static native boolean isBuiltWith(long nativeApplication, int cameraDriver);
     public static native boolean startCamera(long nativeApplication, IBinder binder, Context context, Activity activity, int driver);
     public static native void stopCamera(long nativeApplication);

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/RTABMapApp.xcodeproj/project.pbxproj

@@ -979,9 +979,10 @@
 				ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
 				ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
 				CLANG_CXX_LIBRARY = "libc++";
+				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 7;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				EXCLUDED_ARCHS = "";
@@ -1006,7 +1007,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.21.0;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1019,7 +1020,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1038,7 +1039,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 7;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				FRAMEWORK_SEARCH_PATHS = (
@@ -1063,7 +1064,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.21.0;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -1077,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",

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

@@ -39,6 +39,7 @@
       ignoresPersistentStateOnLaunch = "NO"
       debugDocumentVersioning = "YES"
       debugServiceExtension = "internal"
+      enableGPUFrameCaptureMode = "2"
       allowLocationSimulation = "YES">
       <BuildableProductRunnable
          runnableDebuggingMode = "0">

app/ios/RTABMapApp/.gitignore

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

app/ios/RTABMapApp/Base.lproj/LaunchScreen.storyboard

@@ -1,8 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="19455" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
     <device id="retina6_1" orientation="portrait" appearance="light"/>
     <dependencies>
-        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <deployment identifier="iOS"/>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="19454"/>
         <capability name="Safe area layout guides" minToolsVersion="9.0"/>
         <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
     </dependencies>

app/ios/RTABMapApp/Base.lproj/Main.storyboard

@@ -1,8 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="19455" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
     <device id="retina3_5" orientation="portrait" appearance="light"/>
     <dependencies>
-        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <deployment identifier="iOS"/>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="19454"/>
         <capability name="Image references" minToolsVersion="12.0"/>
         <capability name="Safe area layout guides" minToolsVersion="9.0"/>
         <capability name="System colors in document resources" minToolsVersion="11.0"/>

app/ios/RTABMapApp/Libraries/.gitignore

@@ -1,6 +0,0 @@
-# Ignore everything
-*
-
-# But not these files...
-!.gitignore
-!install_deps.sh

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -69,24 +69,11 @@ void setScreenRotationNative(const void *object, int displayRotation)
     }
 }
 
-int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize)
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
     if(object)
     {
-        return native(object)->openDatabase(databasePath, databaseInMemory, optimize);
-    }
-    else
-    {
-        UERROR("object is null!");
-        return -1;
-    }
-}
-
-int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize)
-{
-    if(object)
-    {
-        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, databaseSource);
+        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, clearDatabase);
     }
     else
     {
@@ -107,6 +94,31 @@ void saveNative(const void *object, const char * databasePath)
     }
 }
 
+bool recoverNative(const void *object, const char * from, const char * to)
+{
+    if(object)
+    {
+        return native(object)->recover(from, to);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+void cancelProcessingNative(const void *object)
+{
+    if(object)
+    {
+        native(object)->cancelProcessing();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
 int postProcessingNative(const void *object, int approach)
 {
     if(object)
@@ -272,11 +284,11 @@ void setCameraNative(const void *object, int type) {
 }
 
 void postCameraPoseEventNative(const void *object,
-        float x, float y, float z, float qx, float qy, float qz, float qw)
+        float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
 {
     if(object)
     {
-        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,0.0);
+        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp);
     }
     else
     {
@@ -477,13 +489,6 @@ void setGridVisibleNative(const void *object, bool visible)
     else
         UERROR("object is null!");
 }
-void setRawScanSavedNative(const void *object, bool enabled)
-{
-    if(object)
-        native(object)->setRawScanSaved(enabled);
-    else
-        UERROR("object is null!");
-}
 void setFullResolutionNative(const void *object, bool enabled)
 {
     if(object)

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -32,9 +32,10 @@ void setupCallbacksNative(const void *object, void * classPtr,
                                                    float, float, float, float, float, float));
 void destroyNativeApplication(const void *object);
 void setScreenRotationNative(const void *object, int displayRotation);
-int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize);
-int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize);
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
 void saveNative(const void *object, const char * databasePath);
+bool recoverNative(const void *object, const char * from, const char * to);
+void cancelProcessingNative(const void * object);
 int postProcessingNative(const void *object, int approach);
 bool exportMeshNative(
             const void *object,
@@ -66,7 +67,7 @@ bool startCameraNative(const void *object);
 void stopCameraNative(const void *object);
 void setCameraNative(const void *object, int type);
 void postCameraPoseEventNative(const void *object,
-                         float x, float y, float z, float qx, float qy, float qz, float qw);
+                         float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
 void postOdometryEventNative(const void *object,
                        float x, float y, float z, float qx, float qy, float qz, float qw,
                        float fx, float fy, float cx, float cy,
@@ -96,7 +97,6 @@ void setGraphOptimizationNative(const void *object, bool enabled);
 void setNodesFilteringNative(const void *object, bool enabled);
 void setGraphVisibleNative(const void *object, bool visible);
 void setGridVisibleNative(const void *object, bool visible);
-void setRawScanSavedNative(const void *object, bool enabled);
 void setFullResolutionNative(const void *object, bool enabled);
 void setSmoothingNative(const void *object, bool enabled);
 void setAppendModeNative(const void *object, bool enabled);

app/ios/RTABMapApp/RTABMap.swift

@@ -113,17 +113,9 @@ class RTABMap {
         setupGraphicNative(native_rtabmap, Int32(size.width), Int32(size.height));
     }
     
-    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
+    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool, clearDatabase: Bool) -> Int {
         databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
-            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize))
-        }
-    }
-    
-    func openDatabase(databaseSource:String, databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
-        databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
-            databaseSource.utf8CString.withUnsafeBufferPointer { bufferSource -> Int in
-                return Int(openDatabase2Native(native_rtabmap, bufferSource.baseAddress, buffer.baseAddress, databaseInMemory, optimize))
-            }
+            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize, clearDatabase))
         }
     }
     
@@ -133,6 +125,18 @@ class RTABMap {
         }
     }
     
+    func recover(from: String, to: String) -> Bool {
+        from.utf8CString.withUnsafeBufferPointer { bufferFrom -> Bool in
+            to.utf8CString.withUnsafeBufferPointer { bufferTo -> Bool in
+                return recoverNative(native_rtabmap, bufferFrom.baseAddress, bufferTo.baseAddress)
+            }
+        }
+    }
+    
+    func cancelProcessing() {
+        cancelProcessingNative(native_rtabmap);
+    }
+    
     func postProcessing(approach: Int) -> Int {
         return Int(postProcessingNative(native_rtabmap, Int32(approach)))
     }
@@ -212,18 +216,18 @@ class RTABMap {
         setCameraNative(native_rtabmap, Int32(type))
     }
     
-    func postCameraPoseEvent(pose: simd_float4x4) {
+    func postCameraPoseEvent(pose: simd_float4x4, stamp: TimeInterval) {
         let rotation = GLKMatrix3(
             m: (pose[0,0], pose[0,1], pose[0,2],
             pose[1,0], pose[1,1], pose[1,2],
             pose[2,0], pose[2,1], pose[2,2]))
         let quat = GLKQuaternionMakeWithMatrix3(rotation)
-        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, stamp)
     }
     
     func notifyLost() {
         // a null transform will make rtabmap creating a new session
-        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0)
+        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0)
     }
     
     func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) {
@@ -235,7 +239,7 @@ class RTABMap {
         
         let quat = GLKQuaternionMakeWithMatrix3(rotation)
                 
-        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, frame.timestamp)
                 
         let confMap = frame.sceneDepth?.confidenceMap
         let depthMap = frame.sceneDepth?.depthMap
@@ -401,9 +405,6 @@ class RTABMap {
     func setGridVisible(visible: Bool) {
         setGridVisibleNative(native_rtabmap, visible)
     }
-    func setRawScanSaved(enabled: Bool) {
-        setRawScanSavedNative(native_rtabmap, enabled)
-    }
     func setFullResolution(enabled: Bool) {
         setFullResolutionNative(native_rtabmap, enabled)
     }
@@ -486,7 +487,7 @@ func getPreviewImage(databasePath: String) -> UIImage?
             let bitmap = CIImage(bitmapData: data, bytesPerRow: Int(image.width*image.channels), size: CGSize(width: Int(image.width), height: Int(image.height)), format: CIFormat.BGRA8, colorSpace: nil)
             return UIImage(ciImage: bitmap)
         }
-        return nil
+        return UIImage(named: "RTAB-Map1024")
     }
     return imageOut
 }

app/ios/RTABMapApp/ViewController.swift

@@ -8,6 +8,7 @@
 import GLKit
 import ARKit
 import Zip
+import StoreKit
 
 extension Array {
     func size() -> Int {
@@ -41,6 +42,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     private var mMaxFeatures: Int = 0
     private var mLoopThr = 0.11
     
+    private var mReviewRequested = false
+    
     // UI states
     private enum State {
         case STATE_WELCOME,    // Camera/Motion off - showing only buttons open and start new scan
@@ -112,6 +115,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     @IBOutlet weak var toastLabel: UILabel!
     
     let RTABMAP_TMP_DB = "rtabmap.tmp.db"
+    let RTABMAP_RECOVERY_DB = "rtabmap.tmp.recovery.db"
     let RTABMAP_EXPORT_DIR = "Export"
 
     func getDocumentDirectory() -> URL {
@@ -1315,7 +1319,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         // update preference
         rtabmap!.setOnlineBlending(enabled: defaults.bool(forKey: "Blending"));
         rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering"));
-        rtabmap!.setRawScanSaved(enabled: defaults.bool(forKey: "SaveRawScan"));
         rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode"));
         rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing"));
         rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode"));
@@ -1340,6 +1343,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         rtabmap!.setMappingParameter(key: "Vis/FeatureType", value: defaults.string(forKey: "FeatureType")!);
         rtabmap!.setMappingParameter(key: "Mem/NotLinkedNodesKept", value: defaults.bool(forKey: "SaveAllFramesInDatabase") ? "true" : "false");
         rtabmap!.setMappingParameter(key: "RGBD/OptimizeFromGraphEnd", value: defaults.bool(forKey: "OptimizationfromGraphEnd") ? "true" : "false");
+        rtabmap!.setMappingParameter(key: "RGBD/MaxOdomCacheSize", value: defaults.string(forKey: "MaximumOdometryCacheSize")!);
         rtabmap!.setMappingParameter(key: "Optimizer/Strategy", value: defaults.string(forKey: "GraphOptimizer")!);
         rtabmap!.setMappingParameter(key: "RGBD/ProximityBySpace", value: defaults.string(forKey: "ProximityDetection")!);
 
@@ -1413,12 +1417,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         
         present(alertController, animated: true)
         
-        setGLCamera(type: 1);
+        setGLCamera(type: 0);
         startCamera();
     }
     
     func newScan()
     {
+        print("databases.size() = \(databases.size())")
+        if(databases.count >= 5 && !mReviewRequested && self.depthSupported)
+        {
+            SKStoreReviewController.requestReviewInCurrentScene()
+            mReviewRequested = true
+        }
+        
         if(mState == State.STATE_VISUALIZING)
         {
             closeVisualization()
@@ -1426,15 +1437,121 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         
         mMapNodes = 0;
         self.openedDatabasePath = nil
-        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
-        
+        let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
         let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory")
-        self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false)
-
-        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) &&
+           FileManager.default.fileExists(atPath: tmpDatabase.path) &&
+           tmpDatabase.fileSize > 1024*1024) // > 1MB
         {
-            self.setGLCamera(type: 1);
-            self.startCamera();
+            dismiss(animated: true, completion: {
+                let msg = "The previous session (\(tmpDatabase.fileSizeString)) was not correctly saved, do you want to recover it?"
+                let alert = UIAlertController(title: "Recovery", message: msg, preferredStyle: .alert)
+                let alertActionNo = UIAlertAction(title: "Ignore", style: .destructive) {
+                    (UIAlertAction) -> Void in
+                    do {
+                        try FileManager.default.removeItem(at: tmpDatabase)
+                    }
+                    catch {
+                        print("Could not clear tmp database: \(error)")
+                    }
+                    self.newScan()
+                }
+                alert.addAction(alertActionNo)
+                let alertActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                    (UIAlertAction) -> Void in
+                    // do nothing
+                }
+                alert.addAction(alertActionCancel)
+                let alertActionYes = UIAlertAction(title: "Yes", style: .default) {
+                    (UIAlertAction2) -> Void in
+
+                    let fileName = Date().getFormattedDate(format: "yyMMdd-HHmmss") + ".db"
+                    let outputDbPath = self.getDocumentDirectory().appendingPathComponent(fileName).path
+                    
+                    var indicator: UIActivityIndicatorView?
+                    
+                    let alertView = UIAlertController(title: "Recovering", message: "Please wait while recovering data...", preferredStyle: .alert)
+                    let alertViewActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                        (UIAlertAction) -> Void in
+                        self.dismiss(animated: true, completion: {
+                            self.progressView = nil
+                            
+                            indicator = UIActivityIndicatorView(style: .large)
+                            indicator?.frame = CGRect(x: 0.0, y: 0.0, width: 60.0, height: 60.0)
+                            indicator?.center = self.view.center
+                            self.view.addSubview(indicator!)
+                            indicator?.bringSubviewToFront(self.view)
+                            
+                            indicator?.startAnimating()
+                            self.rtabmap!.cancelProcessing();
+                        })
+                    }
+                    alertView.addAction(alertViewActionCancel)
+                    
+                    let previousState = self.mState
+                    self.updateState(state: .STATE_PROCESSING);
+                    
+                    self.present(alertView, animated: true, completion: {
+                        //  Add your progressbar after alert is shown (and measured)
+                        let margin:CGFloat = 8.0
+                        let rect = CGRect(x: margin, y: 84.0, width: alertView.view.frame.width - margin * 2.0 , height: 2.0)
+                        self.progressView = UIProgressView(frame: rect)
+                        self.progressView!.progress = 0
+                        self.progressView!.tintColor = self.view.tintColor
+                        alertView.view.addSubview(self.progressView!)
+                        
+                        var success : Bool = false
+                        DispatchQueue.background(background: {
+                            
+                            success = self.rtabmap!.recover(from: tmpDatabase.path, to: outputDbPath)
+                            
+                        }, completion:{
+                            if(indicator != nil)
+                            {
+                                indicator!.stopAnimating()
+                                indicator!.removeFromSuperview()
+                            }
+                            if self.progressView != nil
+                            {
+                                self.dismiss(animated: self.openedDatabasePath == nil, completion: {
+                                    if(success)
+                                    {
+                                        let alertSaved = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully recovered!", fileName), preferredStyle: .alert)
+                                        let yes = UIAlertAction(title: "OK", style: .default) {
+                                            (UIAlertAction) -> Void in
+                                            self.openDatabase(fileUrl: URL(fileURLWithPath: outputDbPath))
+                                        }
+                                        alertSaved.addAction(yes)
+                                        self.present(alertSaved, animated: true, completion: nil)
+                                    }
+                                    else
+                                    {
+                                        self.updateState(state: previousState);
+                                        self.showToast(message: "Recovery failed!", seconds: 4)
+                                    }
+                                })
+                            }
+                            else
+                            {
+                                self.showToast(message: "Recovery canceled", seconds: 2)
+                                self.updateState(state: previousState);
+                            }
+                        })
+                    })
+                }
+                alert.addAction(alertActionYes)
+                self.present(alert, animated: true, completion: nil)
+            })
+        }
+        else
+        {
+            self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false, clearDatabase: true)
+
+            if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+            {
+                self.setGLCamera(type: 0);
+                self.startCamera();
+            }
         }
     }
     
@@ -1518,12 +1635,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             
             self.openedDatabasePath = URL(fileURLWithPath: filePath)
             
-            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved on the SD-CARD!", fileName), preferredStyle: .alert)
+            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved!", fileName), preferredStyle: .alert)
             let yes = UIAlertAction(title: "OK", style: .default) {
                 (UIAlertAction) -> Void in
             }
             alert.addAction(yes)
             self.present(alert, animated: true, completion: nil)
+            do {
+                let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
+                try FileManager.default.removeItem(at: tmpDatabase)
+            }
+            catch {
+                print("Could not clear tmp database: \(error)")
+            }
             self.updateDatabases()
             self.updateState(state: previousState)
         })
@@ -1559,6 +1683,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 indicator?.bringSubviewToFront(self.view)
                 
                 indicator?.startAnimating()
+                
+                self.rtabmap!.cancelProcessing()
             })
             
         }))
@@ -1663,6 +1789,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
             self.dismiss(animated: true)
             self.progressView = nil
+            self.rtabmap!.cancelProcessing()
         }))
 
         let previousState = mState
@@ -1799,7 +1926,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             stopMapping(ignoreSaving: true)
         }
         
-        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
         openedDatabasePath = fileUrl;
         let fileName: String = self.openedDatabasePath!.lastPathComponent
         
@@ -1811,7 +1937,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         updateState(state: .STATE_PROCESSING);
         var status = 0
         DispatchQueue.background(background: {
-            status = self.rtabmap!.openDatabase(databaseSource: self.openedDatabasePath!.path, databasePath: tmpDatabase.path, databaseInMemory: true, optimize: false)
+            status = self.rtabmap!.openDatabase(databasePath: self.openedDatabasePath!.path, databaseInMemory: true, optimize: false, clearDatabase: false)
         }, completion:{
             // main thread
             if(status == -1) {
@@ -1973,6 +2099,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
             self.dismiss(animated: true)
             self.progressView = nil
+            self.rtabmap!.cancelProcessing()
         }))
         
         let previousState = mState;
@@ -2072,7 +2199,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     }
                     .sorted(by: { $0.1 > $1.1 }) // sort descending modification dates
                     .map { $0.0 } // extract file names
-            databases = data.filter{ $0.pathExtension == "db" }
+            databases = data.filter{ $0.pathExtension == "db" && $0.lastPathComponent != RTABMAP_TMP_DB && $0.lastPathComponent != RTABMAP_RECOVERY_DB }
             
         } catch {
             print("Error while enumerating files : \(error.localizedDescription)")
@@ -2210,6 +2337,14 @@ extension Date {
         dateformat.dateFormat = format
         return dateformat.string(from: self)
     }
+    
+    var millisecondsSince1970:Int64 {
+        Int64((self.timeIntervalSince1970 * 1000.0).rounded())
+    }
+    
+    init(milliseconds:Int64) {
+        self = Date(timeIntervalSince1970: TimeInterval(milliseconds) / 1000)
+    }
 }
 
 extension DispatchQueue {
@@ -2322,3 +2457,11 @@ extension UserDefaults {
         setDefaultsFromSettingsBundle()
     }
 }
+
+extension SKStoreReviewController {
+    public static func requestReviewInCurrentScene() {
+        if let scene = UIApplication.shared.connectedScenes.first(where: { $0.activationState == .foregroundActive }) as? UIWindowScene {
+            requestReview(in: scene)
+        }
+    }
+}

app/ios/RTABMapApp/install_deps.sh

@@ -1,7 +1,11 @@
 #!/bin/bash
 
+set -euxo pipefail
+
 # Tested on Apple Silicon Mac, with cmake 3.19.2.
 
+mkdir Libraries
+cd Libraries
 pwd=$(pwd)
 prefix=$pwd
 sysroot=iphoneos
@@ -55,9 +59,11 @@ cd $pwd
 
 # FLANN
 echo "wget flann..."
-curl -L http://www.cs.ubc.ca/research/flann/uploads/FLANN/flann-1.8.4-src.zip -o flann-1.8.4-src.zip
-unzip -qq flann-1.8.4-src.zip
-cd flann-1.8.4-src
+git clone https://github.com/flann-lib/flann.git
+cd flann
+git checkout 1.8.4
+curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
+git apply flann_ios.patch
 mkdir build
 cd build
 # comment "add_subdirectory( test )" in top CMakeLists.txt
@@ -66,7 +72,7 @@ cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_O
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
-#rm -r flann-1.8.4-src.zip flann-1.8.4-src
+#rm -r flann
 
 # GTSAM
 git clone https://bitbucket.org/gtborg/gtsam.git
@@ -105,7 +111,7 @@ git checkout tags/v8.2.0
 git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
 mkdir build
 cd build
-cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DIOS_SIMULATOR_ARCHITECTURES=arm64 -DModule_vtkFiltersModeling=ON ..
+cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
 # For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES=""
 cmake --build . --config Release
 cd $pwd
@@ -116,7 +122,7 @@ git clone https://github.com/PointCloudLibrary/pcl.git
 cd pcl
 git checkout tags/pcl-1.11.1
 # patch
-curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/4a66ebb9faa1dfe997a0860d733bc5473cff20ee/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
+curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
 git apply pcl_1_11_1_vtk_ios_support.patch
 mkdir build
 cd build
@@ -134,6 +140,8 @@ cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.4.2
+curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
+git apply opencv_ios.patch
 mkdir build
 cd build
 # add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
@@ -145,6 +153,10 @@ cd $pwd
 
 mkdir rtabmap
 cd rtabmap
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../..
+cmake -DANDROID_PREBUILD=ON ../../../../..
+cmake --build . --config Release
+mkdir ios
+cd ios
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install

app/ios/Settings.bundle/License.plist

@@ -13,7 +13,7 @@
 			<string>
 ======= RTAB-Map =======
 RTAB-Map - https://github.com/introlab/rtabmap
-Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) 2010-2023, 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

app/ios/Settings.bundle/Mapping.plist

@@ -552,6 +552,50 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don&apos;t teleport to a location very similar to one we previously localized on.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Maximum odometry cache size</string>
+			<key>Key</key>
+			<string>MaximumOdometryCacheSize</string>
+			<key>DefaultValue</key>
+			<string>10</string>
+			<key>Titles</key>
+			<array>
+				<string>500</string>
+				<string>200</string>
+				<string>100</string>
+				<string>75</string>
+				<string>50</string>
+				<string>40</string>
+				<string>30</string>
+				<string>20</string>
+				<string>10</string>
+				<string>5</string>
+				<string>Disabled</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>500</string>
+				<string>200</string>
+				<string>100</string>
+				<string>75</string>
+				<string>50</string>
+				<string>40</string>
+				<string>30</string>
+				<string>20</string>
+				<string>10</string>
+				<string>5</string>
+				<string>0</string>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -708,22 +752,6 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
-		<dict>
-			<key>Type</key>
-			<string>PSGroupSpecifier</string>
-			<key>FooterText</key>
-			<string>Save raw point clouds in database.</string>
-		</dict>
-		<dict>
-			<key>Type</key>
-			<string>PSToggleSwitchSpecifier</string>
-			<key>Title</key>
-			<string>Save Raw Scan</string>
-			<key>Key</key>
-			<string>SaveRawScan</string>
-			<key>DefaultValue</key>
-			<true/>
-		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>

app/ios/Settings.bundle/Root.plist

@@ -14,7 +14,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<integer>2</integer>
+			<integer>1</integer>
 			<key>Key</key>
 			<string>PointCloudDensity</string>
 			<key>Title</key>
@@ -454,7 +454,7 @@
 		</dict>
 		<dict>
 			<key>FooterText</key>
-			<string>Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
+			<string>Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
 			<key>Title</key>
 			<string>About</string>
 			<key>Type</key>
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.12</string>
+			<string>0.21.0</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

app/src/CMakeLists.txt

@@ -3,31 +3,6 @@ SET(SRC_FILES
     main.cpp
 )
 
-SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/utilite/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/guilib/include
-    ${CMAKE_CURRENT_SOURCE_DIR}
-    ${OpenCV_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-)
-
-IF(QT4_FOUND)
-    INCLUDE(${QT_USE_FILE})
-ENDIF(QT4_FOUND)
-
-SET(LIBRARIES
-    ${QT_LIBRARIES} 
-    ${OpenCV_LIBS}
-    ${PCL_LIBRARIES}
-)
-
-# rc.exe has problems with these defintions... commented!
-#add_definitions(${PCL_DEFINITIONS})
-
-# Make sure the compiler can find include files from our library.
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
 # For Apple set the icns file containing icons
 IF(APPLE AND BUILD_AS_BUNDLE)
   # set how it shows up in the Info.plist file
@@ -49,40 +24,37 @@ IF(WIN32)
   ELSE( MINGW )
     SET(SRC_FILES ${SRC_FILES} ${PROJECT_NAME}.rc)
   ENDIF( MINGW )
-ENDIF(WIN32)
+ENDIF(WIN32)
+
+IF(BUILD_AS_BUNDLE)
+  ADD_DEFINITIONS("-DBUILD_AS_BUNDLE")
+ENDIF()
 
 # Add binary
 IF(APPLE AND BUILD_AS_BUNDLE)
-  ADD_EXECUTABLE(rtabmap MACOSX_BUNDLE ${SRC_FILES})
-ELSEIF(MINGW)
-  ADD_EXECUTABLE(rtabmap WIN32 ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES})
+ELSEIF(WIN32 AND BUILD_AS_BUNDLE)
+  ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES})
 ELSE()
-  ADD_EXECUTABLE(rtabmap ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
 ENDIF()
-TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
-IF(Qt5_FOUND)
-    IF(Qt5Svg_FOUND)
-        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
-    ELSE()
-        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
-    ENDIF()
-ENDIF(Qt5_FOUND)
+TARGET_LINK_LIBRARIES(rtabmap_app rtabmap_gui)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
     OUTPUT_NAME ${CMAKE_BUNDLE_NAME})
 ELSEIF(WIN32)
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
     OUTPUT_NAME ${PROJECT_NAME})
 ELSE()
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
     OUTPUT_NAME ${PROJECT_PREFIX})
 ENDIF()
 
 #---------------------------
 # Installation stuff
 #--------------------------- 
-INSTALL(TARGETS rtabmap
+INSTALL(TARGETS rtabmap_app
         RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
         BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
       
@@ -93,15 +65,18 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
        
 IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
-  SET(plugin_dest_dir bin)
+  SET(plugin_dest_dir bin/plugins)
   SET(qtconf_dest_dir bin)
-  SET(openni2_dest_dir bin)
+  SET(thirdparty_dest_dir bin)
  
   IF(APPLE)
-    SET(plugin_dest_dir MacOS)
+    SET(plugin_dest_dir MacOS/plugins)
+    IF(Qt6_FOUND)
+       SET(plugin_dest_dir PlugIns)
+    ENDIF()
     SET(qtconf_dest_dir Resources)
-    SET(openni2_dest_dir MacOS)
-    SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/MacOS/${CMAKE_BUNDLE_NAME}")
+    SET(thirdparty_dest_dir MacOS)
+    SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/MacOS/${CMAKE_BUNDLE_NAME}")
   ENDIF(APPLE)
 
   IF(OpenNI2_FOUND)
@@ -117,11 +92,11 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
     ENDIF()
     
     INSTALL(DIRECTORY "${OpenNI2_BIN_DIR}/OpenNI2" 
-          DESTINATION ${openni2_dest_dir} 
+          DESTINATION ${thirdparty_dest_dir} 
           COMPONENT runtime 
           REGEX ".*pdb" EXCLUDE)
     INSTALL(FILES "${OpenNI2_BIN_DIR}/OpenNI.ini" 
-          DESTINATION ${openni2_dest_dir} 
+          DESTINATION ${thirdparty_dest_dir} 
           COMPONENT runtime)
   ENDIF(OpenNI2_FOUND)
   
@@ -130,69 +105,124 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
     IF(WIN32)
 		file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR)
 		INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" 
-          DESTINATION ${plugin_dest_dir} 
+          DESTINATION ${thirdparty_dest_dir} 
           COMPONENT runtime)
 	ENDIF(WIN32)
   ENDIF(k4a_FOUND)
-
-  # Install needed Qt plugins by copying directories from the qt installation
-  # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
-  # Exclude debug libraries
-  IF(QT_PLUGINS_DIR)
-    INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
-          DESTINATION ${plugin_dest_dir}/plugins 
-          COMPONENT runtime 
-          REGEX ".*d4.dll" EXCLUDE
-          REGEX ".*d4.a" EXCLUDE)
-  ELSE()
-	#Qt5
-	foreach(plugin ${Qt5Gui_PLUGINS})
-		get_target_property(plugin_loc ${plugin} LOCATION)
-		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
-		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
-		list(GET loc_list 1 plugin_type)
-		IF(NOT plugin_root)
-			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
-		ENDIF(NOT plugin_root)
-		#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
-		INSTALL(FILES ${plugin_loc}
-          DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
-          COMPONENT runtime)
-	endforeach()
-	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
-		IF(WIN32)
-			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
-		ELSEIF(APPLE)
-			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
-		ENDIF()
-		IF(EXISTS ${plugin_loc})
-			get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
-			string(REPLACE "plugins" ";" loc_list ${plugin_dir})
-			list(GET loc_list 1 plugin_type)
-			INSTALL(FILES ${plugin_loc}
-				DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
-				COMPONENT runtime)
-			#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
-		ENDIF(EXISTS ${plugin_loc})
-	ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
-  ENDIF()
   
-  # install a qt.conf file
-  # this inserts some cmake code into the install script to write the file
-  SET(QT_CONF_FILE [Paths]\nPlugins=plugins)
-  IF(APPLE)
-    SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
-  ENDIF(APPLE)
-  INSTALL(CODE "
-    file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
-    " COMPONENT runtime)
+  IF(Torch_FOUND)
+	# Install needed cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll
+	# TODO: should be a more general way to include them if version is different
+    IF(WIN32 AND CUDA_FOUND)
+		find_file(CUDNN_OPS_DLL NAMES cudnn_ops_infer64_8.dll)
+		find_file(CUDNN_CNN_DLL NAMES cudnn_cnn_infer64_8.dll)
+		IF(CUDNN_OPS_DLL AND CUDNN_CNN_DLL)
+			MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}")
+			MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}")
+			INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL}
+			  DESTINATION ${thirdparty_dest_dir} 
+			  COMPONENT runtime)
+		ELSE()
+			MESSAGE(AUTHOR_WARNING "Using Torch with CUDA, but cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll are not found on the PATH, so it won't be added to package.")
+		ENDIF()
+	ENDIF(WIN32 AND CUDA_FOUND)
+  ENDIF(Torch_FOUND)
+
+  IF(Qt6_FOUND)
+	# Reference: https://doc-snapshots.qt.io/qt6-6.4/qt-deploy-runtime-dependencies.html
+	# The following script must only be executed at install time
+	set(deploy_script "${CMAKE_CURRENT_BINARY_DIR}/deploy_app$<CONFIG>.cmake")
+	file(GENERATE OUTPUT ${deploy_script} CONTENT "
+	# Including the file pointed to by QT_DEPLOY_SUPPORT ensures the generated
+	# deployment script has access to qt_deploy_runtime_dependencies()
+	include(\"${QT_DEPLOY_SUPPORT}\")
+	qt_deploy_runtime_dependencies(
+		EXECUTABLE \"${APPS}\"
+		PLUGINS_DIR ${plugin_dest_dir}
+		GENERATE_QT_CONF
+		NO_TRANSLATIONS
+		VERBOSE
+	)")
+    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "Debug")
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appDebug.cmake"
+            CONFIGURATIONS Debug
+            COMPONENT runtime)
+    	ENDIF()
+    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "Release")
+        install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake"
+            CONFIGURATIONS Release
+            COMPONENT runtime)
+    	ENDIF()
+    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
+        install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake"
+            CONFIGURATIONS RelWithDebInfo
+            COMPONENT runtime)
+    	ENDIF()
+    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
+        install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake"
+            CONFIGURATIONS MinSizeRel
+            COMPONENT runtime)
+    	ENDIF()
+  ELSEIF(Qt5_FOUND)
+    #Qt5
+    foreach(plugin ${Qt5Gui_PLUGINS})
+        get_target_property(plugin_loc ${plugin} LOCATION)
+        get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+        string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+        list(GET loc_list 1 plugin_type)
+        IF(NOT plugin_root)
+            get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
+        ENDIF(NOT plugin_root)
+        #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}${plugin_type}\"")
+        INSTALL(FILES ${plugin_loc}
+          DESTINATION ${plugin_dest_dir}${plugin_type}
+          COMPONENT runtime)
+    endforeach()
+    IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+        IF(WIN32)
+            SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+        ELSEIF(APPLE)
+            SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+        ENDIF()
+        IF(EXISTS ${plugin_loc})
+            get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+            string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+            list(GET loc_list 1 plugin_type)
+            INSTALL(FILES ${plugin_loc}
+                DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+                COMPONENT runtime)
+            #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}${plugin_type}\"")
+        ENDIF(EXISTS ${plugin_loc})
+    ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+  ELSEIF(QT_PLUGINS_DIR) # Qt4
+    # Install needed Qt plugins by copying directories from the qt installation
+    # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
+    # Exclude debug libraries
+    INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
+        DESTINATION ${plugin_dest_dir}
+        COMPONENT runtime 
+        REGEX ".*d4.dll" EXCLUDE
+        REGEX ".*d4.a" EXCLUDE)
+  ENDIF()
+ 
+  IF(Qt5_FOUND OR QT4_FOUND)
+      # install a qt.conf file
+      # this inserts some cmake code into the install script to write the file
+      SET(QT_CONF_FILE [Paths]\nPlugins=plugins)
+      IF(APPLE)
+        SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
+      ENDIF(APPLE)
+      INSTALL(CODE "
+        file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
+        " COMPONENT runtime)
+  ENDIF()
 
   # directories to look for dependencies
-  SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_SOURCE_DIR}/bin)
+  SET(DIRS "${QT_LIBRARY_DIRS}" "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/lib")
   IF(APPLE)
-    SET(DIRS ${DIRS} /usr/local /usr/local/lib)
-  ENDIF(APPLE)
-
+    SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current)
+  ENDIF(APPLE)
+ 
   # Now the work of copying dependencies into the bundle/package
   # The quotes are escaped and variables to use at install time have their $ escaped
   # An alternative is the do a configure_file() on a script and use install(SCRIPT  ...).
@@ -200,10 +230,11 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   # over.
   # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them).
   install(CODE "
-   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
     set(BU_CHMOD_BUNDLE_ITEMS ON)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
-  " COMPONENT runtime)
+  " COMPONENT runtime)
+
 ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

app/src/main.cpp

@@ -36,10 +36,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UConversion.h"
 
+#include <vtkVersionMacros.h>
 #include <vtkObject.h>
 
-#ifdef RTABMAP_PYTHON
-#include "rtabmap/core/PythonInterface.h"
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+#include <QVTKRenderWidget.h>
 #endif
 
 using namespace rtabmap;
@@ -54,12 +55,9 @@ int main(int argc, char* argv[])
 	CoInitialize(nullptr);
 #endif
 
-#ifdef RTABMAP_PYTHON
-	PythonInterface python; // Make sure we initialize python in main thread
-#endif
-
-#if VTK_MAJOR_VERSION >= 8
-	vtkObject::GlobalWarningDisplayOff();
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+    // needed to ensure appropriate OpenGL context is created for VTK rendering.
+    QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());
 #endif
 
 	/* Create tasks */

archive/2010-LoopClosure/README.md

@@ -0,0 +1,25 @@
+
+ To reproduce results (based on parameters of this [paper](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/b/bc/TRO2013.pdf)):
+ 
+ ```
+ rtabmap-console \
+       --Rtabmap/StatisticLogged true\
+       --Rtabmap/StatisticLoggedHeaders false\
+       --Kp/DetectorStrategy 0\
+       --Rtabmap/MemoryThr 300\
+       --Rtabmap/LoopRatio 0.9\
+       --SURF/HessianThreshold 150\
+       --Mem/STMSize 30\
+       --Vis/MaxFeatures 400\
+       --Kp/TfIdfLikelihoodUsed false\
+       --Kp/MaxFeatures 400\
+       --Kp/BadSignRatio 0.25\
+       --Mem/BadSignaturesIgnored true\
+       --Mem/RehearsalSimilarity 0.20\
+       --Mem/RecentWmRatio 0.2\
+       -gt "~/Downloads/UdeS_1Hz.png"\
+       ~/Downloads/UdeS_1Hz
+ ```
+Adding the ground truth file here is optional to show recall at 100% precision at the end of the process directly without using the octave/MATLAB script below. For NewCollege and CityCentre datasets, `rtabmap-imagesJoiner` can be used to assemble the left and right images together.
+ 
+ To analyze with Octave/MATLAB, drop `LogF.txt` and `LogI.txt` generated files from command above in ShowLogs directly, then execute `showLogs.m`.

archive/2010-LoopClosure/README.txt

@@ -1,4 +0,0 @@
-This directory contains some basic concepts on Bayes filtering.
-Main scripts :
- RecursivesBayes.m
- RecursivesBayesAvpd.m

archive/2010-LoopClosure/ShowLogs/getPrecisionRecall.m

@@ -32,8 +32,7 @@ if ~isempty(GroundTruth)
     if size(GroundTruth, 1) ~=  length(LogF(:,1)) ||  size(GroundTruth, 1) ~=  length(LogI(:,1))
         error(['The ground truth size doesn''t match the log files (LogI=' num2str(length(LogI(:,1))) ', LogF=' num2str(length(LogF(:,1))) ', GT=' num2str(size(GroundTruth, 1)) ')'])
     end
-    
-    
+
     %[highestHypot, CorrespondingID, GT, Accepted, Good, Index, UnderLoopRatio] descending order
     if(sum(LogI(:,8) == 10) > 0)
         %OLD
@@ -111,14 +110,14 @@ if ~isempty(GroundTruth)
     index = find(PR(:,1) == 1);
     if ~isempty(index)
         maxRecall = PR(index(end),2) * 100;
-        display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2)))])
+        display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2))) '/' num2str(GT_total_positives)])
     else
         display('Recall max (Precision=100%) = 0')
     end
     indexAccepted = find(PR(:,3) == 1);
     if ~isempty(indexAccepted)
         maxRecall = PR(indexAccepted(end),2) * 100;
-        display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2)))])
+        display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2))) '/' num2str(GT_total_positives)])
     else
         display('Recall max accepted (Precision=100%) = 0')
     end

archive/2010-LoopClosure/ShowLogs/showlogs.m

@@ -22,6 +22,8 @@ function [LogF LogI] = showlogs(PathPrefix, GT_file)
 set(0,'defaultAxesFontName', 'Times')
 set(0,'defaultTextFontName', 'Times')
 
+close all
+
 if nargin < 2, GT_file = ''; end
 if nargin < 1, PathPrefix = '.'; end
 
@@ -325,7 +327,7 @@ y(LogI(:, 1) == 0) = [];
 x(LogI(:, 1) == 0) = [];
 plot(x,y, 'g.')
 
-set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))})
+%set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))})
 % %matched sign words
 % y = LogI(:,2);
 % x = 1:length(y);

archive/2010-LoopClosure/run_all.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/NewCollege ~/loop_closure_detection_datasets/NewCollege.png
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/CityCentre ~/loop_closure_detection_datasets/CityCentre.png
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/UdeS_1Hz ~/loop_closure_detection_datasets/UdeS_1Hz.png
+ 

archive/2010-LoopClosure/run_bow.sh

@@ -0,0 +1,32 @@
+#!/bin/bash
+
+DATASET_FOLDER=""
+GT_FILE=""
+if [ $# -eq 2 ] 
+then
+    DATASET_FOLDER=$1
+    GT_FILE=$2
+else
+echo "Usage: run_bow.sh \"dataset folder\" \"ground truth file\""
+exit
+fi
+
+rtabmap-console \
+       -quiet \
+       --Rtabmap/StatisticLogged true\
+       --Rtabmap/StatisticLoggedHeaders false\
+       --Kp/DetectorStrategy 0\
+       --SURF/HessianThreshold 150\
+       --Rtabmap/MemoryThr 300\
+       --Rtabmap/LoopRatio 0.9\
+       --Mem/STMSize 30\
+       --Vis/MaxFeatures 400\
+       --Kp/TfIdfLikelihoodUsed false\
+       --Kp/MaxFeatures 400\
+       --Kp/BadSignRatio 0.25\
+       --Mem/BadSignaturesIgnored true\
+       --Mem/RehearsalSimilarity 0.20\
+       --Mem/RecentWmRatio 0.20\
+       -gt "$GT_FILE"\
+       "$DATASET_FOLDER"
+

archive/2020-IlluminationInvariant/README.md

@@ -1,22 +0,0 @@
-
-## Multi-Session Visual SLAM for Illumination Invariant Localization in Indoor Environments
-
-* Paper: https://arxiv.org/abs/2103.03827
-
-* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
-![Overview](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_overview.jpg)]
-
-
-## Description
-
-This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using Multi-Session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg)]
-
-The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg)]
-
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_map_merged_999.jpg)]
-

archive/2020-IlluminationInvariant/scripts/export_stats.sh

@@ -1,19 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14) #0 1 6 7 8 9 11 12 13 14
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPORT_TOOL="${PREFIX}rtabmap-report"
-
-for d in "${DETECTOR[@]}"
-do
-  $REPORT_TOOL --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m "$SKIP/$d/loc"
-  $REPORT_TOOL --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ "$SKIP/$d/consecutive_loc"
-done
-

archive/2020-IlluminationInvariant/scripts/reprocess_maps.sh

@@ -1,42 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
-
-[ ! -d "$SKIP" ] && mkdir $SKIP
-[ ! -d "$SKIP/$TYPE" ] && mkdir $SKIP/$TYPE
-# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
-DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
-
-PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
-
-if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
-then
-  # binary descriptors
-  PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
-else
-  # float descriptors
-  PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
-if
-
-echo $PARAMS
-for db in "${DATABASES[@]}"
-do
-  $REPROCESS_TOOL --skip $SKIP --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 4 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --uerror $PARAMS $db $SKIP/$TYPE/$db
-  $DETECT_MORE_LOOP_CLOSURE_TOOL --uwarn $SKIP/$TYPE/$db
-done
-

archive/2020-IlluminationInvariant/scripts/reprocess_maps_all.sh

@@ -1,16 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./reprocess_maps.sh $d $SKIP
-  ./run_merge.sh $d $SKIP
-done
-

archive/2020-IlluminationInvariant/scripts/run_all.sh

@@ -1,13 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-./reprocess_maps_all.sh $SKIP
-./run_merge.sh $SKIP
-./run_localization_single_all.sh $SKIP
-./run_consecutive_localization_all.sh $SKIP
-

archive/2020-IlluminationInvariant/scripts/run_consecutive_localization.sh

@@ -1,31 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
-TARGETS=($SKIP/$TYPE'/map_190321-172717.db;'$SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-182709.db;' $SKIP/$TYPE'/map_190321-182709.db;'$SKIP/$TYPE'/map_190321-185608.db' $SKIP/$TYPE'/map_190321-185608.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-193556.db' )
-
-
-[ ! -d "$SKIP/$TYPE/consecutive_loc" ] && mkdir $SKIP/$TYPE/consecutive_loc
-
-for i in ${!SOURCE[@]}
-do
-    db=${SOURCE[$i]}
-    loc_dbs=${TARGETS[$i]}
-    $REPROCESS_TOOL --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --uwarn "$SKIP/$TYPE/$db;$loc_dbs" $SKIP/$TYPE/consecutive_loc/loc_$db
-done
-

archive/2020-IlluminationInvariant/scripts/run_consecutive_localization_all.sh

@@ -1,15 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./run_consecutive_localization.sh $d $SKIP
-done
-

archive/2020-IlluminationInvariant/scripts/run_localization_accuracy.sh

@@ -1,38 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
-LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
-
-[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/accuracy
-
-db=merged_9999.db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
-
-

archive/2020-IlluminationInvariant/scripts/run_localization_single.sh

@@ -1,32 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_9999_reduced.db'
-DATABASES=(  'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db'  )
-# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
-LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
-
-[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/loc
-
-echo $PARAMS
-for db in "${DATABASES[@]}"
-do
-  $REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/loc/loc_$db
-done
-
-

archive/2020-IlluminationInvariant/scripts/run_localization_single_all.sh

@@ -1,15 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./run_localization_single.sh $d $SKIP
-done
-

archive/2020-IlluminationInvariant/scripts/run_merge.sh

@@ -1,38 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
-
-DATABASES="$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-185608.db;$SKIP/$TYPE/map_190321-193556.db"
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$DATABASES" $SKIP/$TYPE/merged_9999.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999.db
-
-#$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 --Mem/ReduceGraph true --Vis/MinInliers 60 "$DATABASES" $SKIP/$TYPE/merged_9999_reduced.db
-#$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999_reduced.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_16.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_16.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-185608.db" $SKIP/$TYPE/merged_135.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_135.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_246.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_246.db
-
-
-

archive/2020-IlluminationInvariant/scripts/valgrind.sh

@@ -1,19 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 8 9 11 12 14)
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPORT_TOOL="${PREFIX}rtabmap-report"
-
-for d in "${DETECTOR[@]}"
-do
-  valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 ${PREFIX}rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false  "${SKIP}/${d}/merged_9999.db;map_190321-164651.db" output.db
-  rm output.db
-done
-

archive/2022-IlluminationInvariant/README.md

@@ -0,0 +1,106 @@
+
+## Multi-Session Visual SLAM for Illumination Invariant Re-Localization in Indoor Environments
+
+* Paper: https://doi.org/10.3389/frobt.2022.801886
+
+* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
+![Overview](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_overview.jpg)
+
+
+## Description
+
+This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using multi-session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_single_percentage.jpg)
+
+The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_merged_percentage.jpg)
+
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_map_merged_999.jpg)
+
+
+## Dataset
+
+We provide two formats: the first one is more general and the second one is used to produce the results in this paper with RTAB-Map. Please open issue if the links are outdated.
+
+* [Images](https://drive.google.com/file/d/1fUm1m8oW6q8qlThx7BjrBH2vrVbNQ9bQ/view?usp=drive_link):
+  * `rgb`: folder containing *.jpg color camera images
+  * `depth`: folder containing *.png 16bits mm depth images
+  * `calib`: folder containing calibration for each color image. Each calibration contains also the transform between `device` and `camera` frames as `local_transform`.
+  * `device_poses.txt`: VIO poses of each image in `device` frame
+  * `camera_poses.txt`: VIO poses of each image in `camera` frame
+* [RTAB-Map Databases](https://drive.google.com/file/d/1TklUcTKFSrcg8b0t0U80G_IpFRMVRlY5/view?usp=drive_link)
+* Dataset now also available on [Federated Research Data Repository (FRDR)](https://doi.org/10.20383/103.0931) (if links above don't work)
+
+
+## How reproduce results shown in the paper
+
+1. RTAB-Map should be built from source with those dependencies (don't need to "install" it, we will launch it from build directory in the scripts below to avoid conflicting with another rtabmap already installed): 
+    * Use Ubuntu 20.04+ to avoid any python2/python3 conflicts.
+    * OpenCV built with **xfeatures2d** and **nonfree** modules
+    * [torchlib c++](https://pytorch.org/get-started/locally/) (tested on v1.10.2) to enable [SuperPoint](https://github.com/magicleap/SuperPointPretrainedNetwork)
+    * Git clone [SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork) into scripts directory.
+    * Generate `superpoint_v1.pt` in the scripts directory (can also be downloaded from [here](https://github.com/KinglittleQ/SuperPoint_SLAM/blob/master/superpoint.pt) but may not be compatible with more recent pytorch versions):
+      ```bash
+      cd rtabmap/archive/2022-IlluminationInvariant/scripts
+      wget https://github.com/magicleap/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth
+      wget https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py
+      python trace.py
+      ```
+
+2. Download databases of the dataset and extract them.
+3. Adjust the path inside `rtabmap_latest.sh` script to match where you just built rtabmap with right dependencies.
+4. Run `run_all.sh DATABASES_PATH OUTPUT_PATH`, this script will do the following steps (warning, this could take hours to do...):
+    * Recreate the map databases for each feature type
+    * Create the merged databases
+    * Run localization databases over all map/merged databases
+    * Run consecutive localization experiment
+  
+5. Export statistics with `export_stats.sh` script.
+
+6. Use the MatLab/Octave scripts in this folder to show results you want. Set `dataDir` to directory containing the exported statistics. 
+```
+sudo apt install install octave liboctave-dev
+
+# In octave:
+pkg install -forge control signal
+```
+
+### Docker
+
+1. Create the docker image:
+```
+cd rtabmap
+docker build -t rtabmap_frontiers -f docker/frontiers2022/Dockerfile .
+```
+
+2. Assuming you extracted the databases of the dataset in `~/Downloads/Illumination_invariant_databases`, create an output directory for results:
+```
+mkdir ~/Downloads/Illumination_invariant_databases/results
+```
+
+3. Run script:
+```
+docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
+    --user $(id -u):$(id -g) \
+    -w=/workspace/scripts \
+    -v ~/Downloads/Illumination_invariant_databases:/workspace/databases \
+    -v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
+    rtabmap_frontiers /workspace/scripts/run_all.sh /workspace/databases /workspace/results
+```
+
+4. Export statistics:
+```
+docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
+    --env="DISPLAY=$DISPLAY" \
+    --env="QT_X11_NO_MITSHM=1" \
+    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+    --env="XAUTHORITY=$XAUTH" \
+    --volume="$XAUTH:$XAUTH" \
+    --user $(id -u):$(id -g) \
+    -w=/workspace/results \
+    -v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
+    rtabmap_frontiers /workspace/scripts/export_stats.sh /workspace/results
+```

archive/2022-IlluminationInvariant/results_common.m

@@ -2,24 +2,35 @@
 clear all
 close all
 
+# sudo apt install octave-signal
 pkg load signal
 
-# rtabmap-report --loc 32 Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms . Keypoint/Current_frame/words
-# Right-click on thr legend of the figure, copy all data to clipboard
-# Paste in correction#.txt, inliers#.txt and time#.txt where # is the 
-# number of the descriptor used
+# Use with files generated by export_stats.sh
 
-skipFrameDir = '0';
-prefix = 'Stat';
-RAMaddOverhead = 1;
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
+resultsToShow = 1; % 1=single 2=Consecutive
+
+prefix = 'Stat'; 
+RAMaddOverhead = 0;
 % Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words'
 % Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1)
-statNames = {'Loop/Odom_correction_norm/m', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/'}; % 'Odometry_average'
 
-datasets = [ 0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/'}; % 'Odometry_average'
+
+ 
+datasets = [ 0 1 6 7 9 14 11 111 ]; % 0 1 6 7 9 12 14 11
 sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
 sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
 
+if resultsToShow == 2
+  sep = [0, 1000, 3000, 5000, 7000, 9000];
+  sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
+  prefix = 'Consecutive';
+  statNames = {'Loop/Distance_since_last_loc/', 'Distance_since_last_loc_under_50cm'};
+endif
+
+MapsN = length(sepName);
+
 allCumResults = {};
 allMaxResults = {};
 
@@ -35,13 +46,15 @@ statName = strrep(statNames{s},'/','-');
 for d=1:length(datasets)
 
 if strcmp(statName,'Inliers_ratio_%')
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
-  dataWords = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  dataWords = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
   data(:, 2:end) = data(:, 2:end) ./ dataWords(:, 2:end) * 100;
 elseif strcmp(statName, 'Odometry_average')
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+elseif strcmp(statName, 'Distance_since_last_loc_under_50cm')
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
 else
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
 endif
 sessions = size(data,2)-1;
 
@@ -69,7 +82,7 @@ for i = 1:sessions
       if datasets(d) == 7
         %% 135 MB overhead for BRISK kernel
         y = y + 135;
-      elseif datasets(d) == 11
+      elseif datasets(d) == 11 || datasets(d) == 111
         %% 645 MB (library cuda) + 800 MB (network) for SuperPoint
         y = y + 645+800;
       elseif datasets(d) == 13 || datasets(d) == 14
@@ -81,6 +94,12 @@ for i = 1:sessions
     if strcmp(statName, 'Loop-Map_id-')
       nonzeros = y;
     end
+    if strcmp(statName, 'Distance_since_last_loc_under_50cm')
+      y(y>0.55) = 0;
+      y(isnan(y)) = 0;
+      y(y>0) = 1;
+      nonzeros = y;
+    end
     if length(nonzeros) > 0
       avgValue = sum(nonzeros)/length(nonzeros);
       avgResultsTmp(i,j) = avgValue;
@@ -103,7 +122,7 @@ for d=1:length(datasets)
   if sum(totalResults{1,d}, 2)
     cumResults(2:end-1,d+1) = sum(absResults{1,d}, 2) ./ sum(totalResults{1,d}, 2);
   endif
-  cumResults(end,d+1) = sum(sum(absResults{1,d}(1:6,1:6).*eye(6,6))) / sum(sum(totalResults{1,d}(1:6,1:6).*eye(6,6)));
+  cumResults(end,d+1) = sum(sum(absResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN))) / sum(sum(totalResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
 end
 cumResults(2:end-1,1) = 1:sessions;
 
@@ -123,7 +142,7 @@ for d=1:length(datasets)
   if sum(totalResults{1,d}, 2)
     cumMaxResults(2:end-1,d+1) = max(maxResults{1,d}, [], 2);
   endif
-  cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:6,1:6).*eye(6,6)));
+  cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
 end
 cumMaxResults(2:end-1,1) = 1:sessions;
 
@@ -133,3 +152,18 @@ allMaxResults{2,s} = cumMaxResults;
 endfor % statNames
 
 
+if resultsToShow == 2
+  disp('30min') 
+  for d=1:length(datasets)
+    round(sum(absResults{1,d} .* eye(5,5)) / sum(totalResults{1,d} .*eye(5,5)) * 100)
+  endfor
+  disp('60min') 
+  for d=1:length(datasets)
+    round(sum(absResults{1,d} .* [[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) / sum(totalResults{1,d} .*[[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) * 100)
+  endfor
+  disp('120min') 
+  for d=1:length(datasets)
+    round(avgResults{1,d}(1,5) * 100)
+  endfor
+endif
+

archive/2022-IlluminationInvariant/results_loc_over_time.m

@@ -4,16 +4,15 @@ clear all
 
 pkg load signal
 
-# rtabmap-report --loc 32 Loop/Map_id/ loc
-# Right-click on thr legend of the figure, copy all data to clipboard
-# Paste in data#.txt where # is the number of the descriptor used
+# Use with files generated by export_stats.sh
 
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
 resultsToShow = 1; % 1=single loc, 2=merged loc, 3=consecutive
-skipFrameDir = '0';
 
 datasetPrefix = 'Stat';
-datasets = [0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+datasets = [0 1 6 7 9 14 11 111]; % 0 1 6 7 8 9 11 12
 datasetsName = {'SURF' 'SIFT' 'ORB' 'FAST/FREAK' 'FAST/BRIEF' 'GFTT/FREAK' 'GFTT/BRIEF' 'BRISK' 'GFTT/ORB' 'KAZE' 'ORB-OCTREE' 'SuperPoint' 'SURF/FREAK' 'GFTT/DAISY' 'SURF/DAISY'};
+datasetsName{112} = 'SuperGlue'
 sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
 sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
 
@@ -41,13 +40,13 @@ globalc = [];
 
 for d=1:length(datasets)
 
-data = dlmread([skipFrameDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);
+data = dlmread([dataDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);
 
 curvesBeg = 2;
-curvesEnd = size(data,2)-4;
+curvesEnd = size(data,2)-5;  % -4 for '0', -5 for '1'
 
 if resultsToShow == 2
-  curvesBeg = 8;
+  curvesBeg = 8; % 2 if only 4 merged_reduced in stats, 8 to skip first 6
   curvesEnd = size(data,2);
 elseif resultsToShow == 3
   curvesEnd = size(data,2);
@@ -198,7 +197,24 @@ for d=1:length(datasets)
   data=percentResults{1,d}*100;
   data(isnan(data)) = 0;
   hAxes = gca;
-  imagesc( hAxes, data, [0, 100])
+  % Upscaling the image to reduce anti-aliasing effect in pfd viewers
+  scale = 50;
+  tickXStep = zeros(1, size(data, 2));
+  tickYStep = zeros(1, size(data, 1));
+  dataUp = upsample(upsample(data',scale)',scale);
+  for x=0:size(data, 2)-1
+    for y=1:scale-1
+       dataUp(:,(x*scale+1)+y) = dataUp(:,x*scale+1);
+    endfor
+    tickXStep(1,x+1) = scale/2 + scale*x;
+  endfor
+  for x=0:size(data, 1)-1
+    for y=1:scale-1
+       dataUp((x*scale+1)+y,:) = dataUp(x*scale+1,:);
+    endfor
+    tickYStep(1,x+1) = scale/2 + scale*x;
+  endfor
+  imagesc( hAxes, dataUp, [0, 100])
   %title({"",datasetsName{datasets(d)+1}})
   colors = [ones(100,1) [1:100]'*0.01 [1:100]'*0];
   colors(1,:) = 1;
@@ -211,12 +227,14 @@ for d=1:length(datasets)
     ylabel("Map")
   endif
   xlabel([datasetsName{datasets(d)+1} " Localization"])
-  set (gca, "xaxislocation", "top");
+  set(gca, "xaxislocation", "top");
+  set(gca, 'XTick', tickXStep)
   set(gca, 'XTickLabel', sepName, 'fontsize',7)
+  set(gca, 'YTick', tickYStep)
   if resultsToShow == 3
     set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56'}, 'fontsize',7)
   elseif resultsToShow == 2
-    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+6', 'bundle', 'reduced'}, 'fontsize',7)
+    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+5+6', '1-2-3-4-5-6', 'not set'}, 'fontsize',7)
   else
     set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56', '19:35'}, 'fontsize',7)
   endif
@@ -232,4 +250,4 @@ for d=1:length(datasets)
   endif
 end
 cumResults(2:end-1,1) = 1:curves;
-cumResults
+cumResults

archive/2022-IlluminationInvariant/results_loc_prob_by_dist.m

@@ -0,0 +1,109 @@
+
+clear all
+close all
+
+# sudo apt install octave-signal
+pkg load signal
+
+# Use with files generated by export_stats.sh
+
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
+prefix = 'Stat';
+
+dataset = 6
+sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
+sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
+
+statName = strrep('Loop/Distance_since_last_loc/','/','-');
+
+data = dlmread([dataDir '/' prefix num2str(dataset) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
+
+sessions = size(data,2)-1
+
+j = 1 % session #
+x3 = data(:,1);
+y3 = data(:,11);
+y3 = y3(x3>=sep(j) & x3<=sep(j+1), :);
+x3 = x3(x3>=sep(j) & x3<=sep(j+1), :);
+
+max = 0;
+for i=2:length(y3)
+  if isfinite(y3(i)) && isfinite(y3(i-1)) && (x3(i) - x3(i-1) < 1.5)
+    v = y3(i);
+    if v>max
+      max = v
+    endif
+  endif
+endfor
+xb = max/2:max:5;
+[nn3, xx3] = hist (y3, xb);
+nn3 = nn3/length(x3);
+nn3(nn3==0) = NaN;
+
+for j=1:6
+
+  x{j} = data(:,1);
+  y{j} = data(:,2);
+  y{j} = y{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
+  x{j} = x{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
+  [nn{j}, xx{j}] = hist (y{j}, xb);
+  %nn{j} = nn{j}/length(x{j});
+  %nn{j} = nn{j}/sum(nn{j});
+
+  for k=length(nn{j}):-1:1
+    if nn{j}(k) != 0
+      break;
+    else
+      nn{j}(k) = NaN;
+    endif
+  endfor
+
+endfor
+
+j = 6 % session #
+x4 = data(:,1);
+y4 = data(:,12);
+y4 = y4(x4>=sep(j) & x4<=sep(j+1), :);
+x4 = x4(x4>=sep(j) & x4<=sep(j+1), :);
+[nn4, xx4] = hist (y4, xb);
+nn4 = nn4/length(x4);
+nn4(nn4==0) = NaN;
+
+figure
+hold on
+%plot(x1,y1, '.-');
+%plot(x2-x2(1),y2, '.-');
+plot(x3-x3(1),y3, '.-');
+%plot(x4-x4(1),y4, '.-');
+%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+legend('Map1+2+3+4+5+6 -> LocA')
+xlabel('Time')
+ylabel('m')
+title('Distance since last loc')
+
+figure
+hold on
+%plot(x1,y1, '.-');
+%plot(x2-x2(1),y2, '.-');
+plot(x{6}-x{6}(1),y{6}, '.-');
+%plot(x4-x4(1),y4, '.-');
+%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+legend('Map1 -> LocF')
+xlabel('Time')
+ylabel('m')
+title('Distance since last loc')
+
+figure
+hold on
+for j=1:6
+plot(xx{j},nn{j}, '-', 'linewidth', 3)
+endfor
+%plot(xx3,nn3, '.-', 'linewidth', 3)
+%plot(xx4,nn4, '.-', 'linewidth', 3)
+%legend('Loc-F', 'Loc-E', 'Loc-D', 'Loc-C', 'Loc-B', 'Loc-A')
+legend('A-16:51', 'B-17:31', 'C-17:58', 'D-18:30', 'E-18:59', 'F-19:42', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+%h = get(gca,'Children');
+%set(gca,'Children',[h(6) h(5) h(4) h(3) h(2) h(1)])
+%set(gca, 'YScale', 'log')
+xlabel(['Distance not localized (m) Step '  num2str(max) ' m'])
+ylabel('Re-Localization Probability on Map 1 (16:46)')

archive/2022-IlluminationInvariant/scripts/export_stats.sh

@@ -0,0 +1,19 @@
+#!/bin/bash
+
+if [ $# -lt 1 ]
+then
+  echo "No arguments supplied. It should be the data directory (where the reprocessed map databases are saved)."
+  exit
+fi
+DATA=$1
+
+DETECTOR=(0 1 6 7 9 14 11 111) 
+
+source rtabmap_latest.bash
+
+for d in "${DETECTOR[@]}"
+do
+  rtabmap-report --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/ Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
+  rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/ "$DATA/$d/consecutive_loc"
+done
+

archive/2022-IlluminationInvariant/scripts/reprocess_maps.sh

@@ -0,0 +1,41 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+source rtabmap_latest.bash
+
+[ ! -d "$OUTPUT" ] && mkdir $OUTPUT
+[ ! -d "$OUTPUT/$TYPE" ] && mkdir $OUTPUT/$TYPE
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
+DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
+
+PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
+
+if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
+then
+  # binary descriptors
+  PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
+else
+  # float descriptors
+  PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
+fi
+
+if [ $TYPE -eq 111 ] 
+then
+  PARAMS="--Vis/CorNNType 6 --SuperGlue/Path SuperGluePretrainedNetwork/rtabmap_superglue.py --Reg/RepeatOnce false --Vis/CorGuessWinSize 0 $PARAMS --Kp/DetectorStrategy 11 --Vis/FeatureType 11"
+fi
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  rtabmap-reprocess --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 0 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint_v1.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --Db/TargetVersion "" --Icp/CorrespondenceRatio 0.1 --RGBD/MaxOdomCacheSize 0 --uwarn $PARAMS $INPUT/$db $OUTPUT/$TYPE/$db
+  rtabmap-detectMoreLoopClosures --uwarn $OUTPUT/$TYPE/$db
+done
+

archive/2022-IlluminationInvariant/scripts/reprocess_maps_all.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./reprocess_maps.sh $d $INPUT $OUTPUT
+  ./run_merge.sh $d $OUTPUT
+done
+

archive/2022-IlluminationInvariant/scripts/rtabmap_latest.bash

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+export PATH=~/workspace/rtabmap/build/bin:$PATH
+export LD_LIBRARY_PATH=~/workspace/rtabmap/build/lib:$LD_LIBRARY_PATH
+

archive/2022-IlluminationInvariant/scripts/run_all.sh

@@ -0,0 +1,14 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+./reprocess_maps_all.sh $INPUT $OUTPUT
+./run_localization_single_all.sh $INPUT $OUTPUT
+./run_consecutive_localization_all.sh $OUTPUT
+

archive/2022-IlluminationInvariant/scripts/run_consecutive_localization.sh

@@ -0,0 +1,25 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
+  exit
+fi
+TYPE=$1
+DATA=$2
+
+source rtabmap_latest.bash
+
+SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
+TARGETS=($DATA/$TYPE'/map_190321-172717.db;'$DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-182709.db;' $DATA/$TYPE'/map_190321-182709.db;'$DATA/$TYPE'/map_190321-185608.db' $DATA/$TYPE'/map_190321-185608.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-193556.db' )
+
+
+[ ! -d "$DATA/$TYPE/consecutive_loc" ] && mkdir $DATA/$TYPE/consecutive_loc
+
+for i in ${!SOURCE[@]}
+do
+    db=${SOURCE[$i]}
+    loc_dbs=${TARGETS[$i]}
+    rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$DATA/$TYPE/$db;$loc_dbs" $DATA/$TYPE/consecutive_loc/loc_$db
+done
+

archive/2022-IlluminationInvariant/scripts/run_consecutive_localization_all.sh

@@ -0,0 +1,16 @@
+#!/bin/bash
+
+if [ $# -lt 1 ]
+then
+  echo "No arguments supplied. It should be the data directory (where the reprocessed map databases will be saved)."
+  exit
+fi
+DATA=$1
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_consecutive_localization.sh $d $DATA
+done
+

archive/2022-IlluminationInvariant/scripts/run_localization_accuracy.sh

@@ -0,0 +1,33 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+source rtabmap_latest.bash
+
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
+
+[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/accuracy
+
+db=merged_123456.db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
+
+

archive/2022-IlluminationInvariant/scripts/run_localization_single.sh

@@ -0,0 +1,28 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+
+source rtabmap_latest.bash
+
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db'
+DATABASES=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db')
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
+
+[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/loc
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  rtabmap-reprocess -loc_null --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/loc/loc_$db
+done
+
+

archive/2022-IlluminationInvariant/scripts/run_localization_single_all.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_localization_single.sh $d $INPUT $OUTPUT
+done
+

archive/2022-IlluminationInvariant/scripts/run_merge.sh

@@ -0,0 +1,34 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
+  exit
+fi
+TYPE=$1
+DATA=$2
+
+MIN_INLIERS=20 #20 40 60 80
+
+source rtabmap_latest.bash
+
+DATABASES="$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-185608.db;$DATA/$TYPE/map_190321-193556.db"
+
+# To compute "Ground truth"
+rtabmap-reprocess --uwarn "$DATABASES" $DATA/$TYPE/merged_123456.db
+
+cp $DATA/$TYPE/merged_123456.db $DATA/$TYPE/merged_123456_gt.db
+rtabmap-detectMoreLoopClosures -r 0.5 -i 5 $DATA/$TYPE/merged_123456_gt.db
+
+rtabmap-reprocess --uwarn -gt $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_16.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-185608.db" $DATA/$TYPE/merged_135.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_246.db
+
+# Reduced graph
+rtabmap-reprocess --uwarn -gt --Mem/ReduceGraph true --Vis/MinInliers $MIN_INLIERS $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456_reduced.db
+
+

archive/2022-IlluminationInvariant/scripts/trace.py

@@ -0,0 +1,9 @@
+import torch
+import torchvision
+from demo_superpoint  import SuperPointNet
+model = SuperPointNet()
+model.load_state_dict(torch.load("superpoint_v1.pth"))
+model.eval()
+example = torch.rand(1, 1, 640, 480)
+traced_script_module = torch.jit.trace(model, example)
+traced_script_module.save("superpoint_v1.pt")