bump 0.20.23

DBViewer: ignore marker constraints in refine, add marker priors when optimizing the graph
fix bigobj under MSVC, fix yaml-cpp link. (#957 )
2026-09-02 01:20:25 +08:00 · 2023-01-21 16:09:41 -08:00 · 2023-01-21 15:52:01 -08:00 · 2023-01-21 15:50:02 -08:00 · 2023-01-16 16:41:48 -08:00 · 2023-01-16 14:14:25 -08:00
235 changed files with 12179 additions and 5459 deletions
--- a/.dockerignore
+++ b/.dockerignore
@@ -0,0 +1 @@
+build/*
--- a/.github/workflows/cmake-ros.yml
+++ b/.github/workflows/cmake-ros.yml
@@ -0,0 +1,64 @@
+name: CMake-ROS
+
+on:
+  push:
+    branches:
+      - '**'
+  pull_request:
+    branches:
+      - '**'
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        ros_distribution: [melodic, noetic, foxy, humble, rolling]
+        include:
+        - ros_distribution: 'melodic' 
+          os: ubuntu-18.04
+        - ros_distribution: 'noetic'
+          os: ubuntu-20.04
+        - ros_distribution: 'foxy'
+          os: ubuntu-20.04
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-22.04
+
+    steps:
+    - uses: ros-tooling/setup-ros@v0.4
+      with:
+        required-ros-distributions: ${{ matrix.ros_distribution }}
+    
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install ros-${{ matrix.ros_distribution }}-rtabmap
+        sudo apt-get -y remove ros-${{ matrix.ros_distribution }}-rtabmap
+
+    - uses: actions/checkout@v2
+
+    - 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
+      
--- a/.github/workflows/cmake.yml
+++ b/.github/workflows/cmake.yml
@@ -2,59 +2,42 @@ name: CMake

 on:
  push:
-    branches: [ master ]
+    branches: 
+      - '**'
  pull_request:
-    branches: [ master ]
+    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_distro }} and ${{ matrix.os }}
+    name: ${{ matrix.os }}
    runs-on: ${{ matrix.os }}
    strategy:
      matrix:
-        os: [ubuntu-20.04, ubuntu-18.04]
-        include:
-        - os: ubuntu-20.04
-          ros_distro: 'noetic'
-        - os: ubuntu-18.04
-          ros_distro: 'melodic'
+        os: [ubuntu-22.04, ubuntu-20.04, ubuntu-18.04]

-    steps:
-    - uses: ros-tooling/setup-ros@v0.2
-      with:
-        required-ros-distributions: ${{ matrix.ros_distro }}
-    
+    steps:    
    - name: Install dependencies
      run: |
+        DEBIAN_FRONTEND=noninteractive
        sudo apt-get update
-        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
-        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev

    - uses: actions/checkout@v2

    - name: Configure CMake
-      # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
-      # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
      run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}

    - name: Build
-      # Build your program with the given configuration
      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}

    - name: Info
      working-directory: ${{github.workspace}}/build/bin
      run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
        ./rtabmap-console --version

 #    - name: Test
--- a/.github/workflows/docker.yml
+++ b/.github/workflows/docker.yml
@@ -11,12 +11,14 @@ jobs:
    
    strategy:
      matrix:
-        docker_tag: [xenial, bionic, focal, android23, android24, android26]
+        docker_tag: [xenial, bionic, focal, android23, android24, android26, android30]
        include:
        - docker_tag: xenial
          docker_tags: |
            introlab3it/rtabmap:xenial
            introlab3it/rtabmap:16.04
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
          docker_path: 'xenial'
@@ -24,6 +26,8 @@ jobs:
          docker_tags: |
            introlab3it/rtabmap:bionic
            introlab3it/rtabmap:18.04
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
            linux/arm64
@@ -33,6 +37,8 @@ jobs:
            introlab3it/rtabmap:focal
            introlab3it/rtabmap:20.04
            introlab3it/rtabmap:latest
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
            linux/arm64
@@ -41,21 +47,35 @@ jobs:
          docker_tags: |
            introlab3it/rtabmap:android23
            introlab3it/rtabmap:tango
+          docker_args: |
+            API_VERSION=23
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_api23'
+          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_api24'
+          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_api26'
+          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:
      -
@@ -79,11 +99,12 @@ jobs:
        name: Build and push
        uses: docker/build-push-action@v2
        with:
-          context: ./docker/${{ matrix.docker_path }}
+          context: .
          push: true
          platforms: ${{ matrix.docker_platforms }}
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
          build-args: |
-            CACHE_DATE=${{ github.head_ref }}.${{ github.sha }}
+            ${{ matrix.docker_args }}
          tags: ${{ matrix.docker_tags }}
          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
          cache-to: type=inline
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 16)
+SET(RTABMAP_PATCH_VERSION 23)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -106,6 +106,7 @@ if(MSVC)
      SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N}")
    endif()
  endif()
+  add_compile_options("/bigobj")
 endif()

 # [Eclipse] Automatic Discovery of Include directories (Optional, but handy) 
@@ -212,6 +213,7 @@ option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
+option(WITH_OPENGV        "Include OpenGV support"               OFF)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -221,7 +223,7 @@ ENDIF()
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
 set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5)

-FIND_PACKAGE(OpenCV REQUIRED QUIET)
+FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)

 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
@@ -314,7 +316,11 @@ IF(WITH_QT)
 			IF(value EQUAL -1)
                list(FIND PCL_LIBRARIES vtkGUISupportQt value)
                IF(value EQUAL -1)
-                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    IF("${VTK_MAJOR_VERSION}" GREATER 8)
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::GUISupportQt")
+                    ELSE()
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    ENDIF()
                    SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
                ENDIF(value EQUAL -1)
 		    ENDIF(value EQUAL -1)
@@ -349,6 +355,13 @@ IF(WITH_QT)
    ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)

+IF(NOT VTK_FOUND)
+    # Newest PCL versions won't set -DDISABLE_VTK
+    IF(NOT "${PCL_DEFINITIONS}" MATCHES "-DDISABLE_VTK")
+        SET(PCL_DEFINITIONS "${PCL_DEFINITIONS};-DDISABLE_VTK")
+    ENDIF()
+ENDIF(NOT VTK_FOUND)
+
 IF(WITH_TORCH)
    FIND_PACKAGE(Torch QUIET)
    IF(TORCH_FOUND)
@@ -357,7 +370,7 @@ IF(WITH_TORCH)
 ENDIF(WITH_TORCH)

 IF(WITH_PYTHON)
-    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy)
    IF(Python3_FOUND)
        MESSAGE(STATUS "Found Python3")
    ENDIF(Python3_FOUND)
@@ -433,16 +446,29 @@ IF(WITH_DC1394)
 ENDIF(WITH_DC1394)

 IF(WITH_G2O)
-	FIND_PACKAGE(g2o QUIET NO_MODULE)
+	  FIND_PACKAGE(g2o QUIET NO_MODULE)
    IF(g2o_FOUND)
       MESSAGE(STATUS "Found g2o (targets)")
-	   SET(G2O_FOUND ${g2o_FOUND})
-	   SET(G2O_CPP11 1)
+	     SET(G2O_FOUND ${g2o_FOUND})
+	     get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
+	     MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
+	     FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+         PATHS ${G2O_INCLUDES}
+         NO_DEFAULT_PATH)
+       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+       IF(${matchres} EQUAL -1)
+           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 2)
+       ELSE()
+           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 1)
+       ENDIF()
    ELSE()
-       FIND_PACKAGE(G2O QUIET)
-       IF(G2O_FOUND)
-		 MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
-       ENDIF(G2O_FOUND)
+        FIND_PACKAGE(G2O QUIET)
+        IF(G2O_FOUND)
+            MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+        ENDIF(G2O_FOUND)
    ENDIF()
 ENDIF(WITH_G2O)

@@ -475,7 +501,7 @@ ENDIF(WITH_POINTMATCHER)
 IF(libpointmatcher_FOUND OR GTSAM_FOUND)
    find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
    IF(Boost_MINOR_VERSION GREATER 47)
-        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer REQUIRED)
+        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED)
    ENDIF(Boost_MINOR_VERSION GREATER 47)
    IF(WIN32)
 	    MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
@@ -701,6 +727,13 @@ IF(WITH_FASTCV)
    ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)

+IF(WITH_OPENGV)
+    FIND_PACKAGE(opengv QUIET)
+    IF(opengv_FOUND)
+       MESSAGE(STATUS "Found OpenGV: ${opengv_INCLUDE_DIRS}")
+    ENDIF(opengv_FOUND)
+ENDIF(WITH_OPENGV)
+
 IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
    FIND_PACKAGE(ORB_SLAM QUIET)
    IF(ORB_SLAM_FOUND)
@@ -709,8 +742,8 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)

 IF(NOT MSVC)
-	IF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)
-	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
+	IF((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND))
+	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
 	 include(CheckCXXCompilerFlag)
 	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
 	  IF(COMPILER_SUPPORTS_CXX14)
@@ -719,7 +752,7 @@ IF(NOT MSVC)
 	  ELSE()
 	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
 	  ENDIF()
-	ENDIF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)
+	ENDIF()

 	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
 	       G2O_FOUND OR
@@ -804,6 +837,7 @@ ENDIF()

 IF(NOT G2O_FOUND)
   SET(G2O "//")
+   SET(G2O_CPP_CONF "//")
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
   IF(NOT G2O_CPP11)
@@ -815,9 +849,9 @@ IF(NOT GTSAM_FOUND)
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
 ENDIF()
-IF(NOT WITH_CERES OR NOT CERES_FOUND)
+IF(NOT CERES_FOUND)
   SET(CERES "//")
-ENDIF(NOT WITH_CERES OR NOT CERES_FOUND)
+ENDIF(NOT CERES_FOUND)
 IF(NOT WITH_TORO)
   SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -841,6 +875,9 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
   SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
+IF(NOT opengv_FOUND)
+   SET(OPENGV "//")
+ENDIF(NOT opengv_FOUND)
 IF(NOT PDAL_FOUND)
   SET(PDAL "//")
 ENDIF(NOT PDAL_FOUND)
@@ -987,7 +1024,11 @@ IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
   SET(CONF_VTK_QT true)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+   IF("${VTK_MAJOR_VERSION}" GREATER 8)
+      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} VTK::GUISupportQt)
+   ELSE()
+      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+   ENDIF()
 ELSE()
   SET(CONF_VTK_QT false)
 ENDIF()
@@ -1308,8 +1349,12 @@ ELSE()
 MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()

-IF(WITH_CERES AND CERES_FOUND)
+IF(CERES_FOUND)
+IF(WITH_CERES)
 MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}          = YES (License: BSD)")
+ELSE()
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}          = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)")
+ENDIF()
 ELSEIF(NOT WITH_CERES)
 MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
 ELSE()
@@ -1360,6 +1405,14 @@ ELSE()
 MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()

+IF(opengv_FOUND)
+MESSAGE(STATUS "  With OpenGV               = YES (License: BSD)")
+ELSEIF(NOT WITH_OPENGV)
+MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenGV               = NO (OpenGV not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)
--- a/README.md
+++ b/README.md
@@ -5,9 +5,8 @@ rtabmap

 [![Release][release-image]][releases]
 [![License][license-image]][license]
-Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)

-[release-image]: https://img.shields.io/badge/release-0.20.8-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases

 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
@@ -15,7 +14,8 @@ Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cm

 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 +25,52 @@ 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="2">ROS 1</td>
+           <td>Melodic</td>
+            <td><a href="http://build.ros.org/job/Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Noetic</td>
+            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td rowspan="3">ROS 2</td>
+            <td>Foxy</td>
+            <td><a href="http://build.ros2.org/job/Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Humble</td>
+            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Rolling</td>
+            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+    </tbody>
+</table>
+ 
--- a/Version.h.in
+++ b/Version.h.in
@@ -40,7 +40,7 @@ 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
@VERTIGO@#define RTABMAP_VERTIGO
@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@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
--- a/app/android/AndroidManifest.xml.in
+++ b/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"/>
--- a/app/android/jni/CameraARCore.cpp
+++ b/app/android/jni/CameraARCore.cpp
@@ -520,11 +520,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;
--- a/app/android/jni/CameraAREngine.cpp
+++ b/app/android/jni/CameraAREngine.cpp
@@ -244,7 +244,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
--- a/app/android/jni/CameraMobile.cpp
+++ b/app/android/jni/CameraMobile.cpp
@@ -171,7 +171,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
    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);
--- a/app/android/jni/CameraTango.cpp
+++ b/app/android/jni/CameraTango.cpp
@@ -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)
 				{
--- a/app/android/jni/RTABMapApp.cpp
+++ b/app/android/jni/RTABMapApp.cpp
@@ -64,6 +64,7 @@ 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 <pcl/common/common.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
@@ -82,6 +83,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;
@@ -243,6 +281,12 @@ RTABMapApp::RTABMapApp() :

 	this->registerToEventsManager();
 	LOGI("RTABMapApp::RTABMapApp() end");
+
+#ifdef __ANDROID__
+#ifndef DISABLE_LOG
+	start_logger();
+#endif
+#endif
 }

 #ifndef __ANDROID__ // __APPLE__
@@ -305,9 +349,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 +404,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 +460,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();
@@ -593,7 +635,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
                                    {
@@ -1199,7 +1241,12 @@ int RTABMapApp::Render()
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
 	try
 	{
-		UTimer fpsTime;
+        if(camera_ == 0)
+        {
+            // We are not doing continous drawing, just measure single draw
+            fpsTime_.restart();
+        }
+        
 #ifdef DEBUG_RENDERING_PERFORMANCE
 		UTimer time;
 #endif
@@ -1446,12 +1493,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
@@ -1578,7 +1625,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
 									{
@@ -1863,7 +1910,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
@@ -2053,13 +2100,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())
 			{
@@ -2083,7 +2130,7 @@ int RTABMapApp::Render()
 				if(camera_!=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)));
 				}
 			}
 		}
@@ -2096,7 +2143,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)
 			{
@@ -2115,16 +2162,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_;
@@ -2557,6 +2594,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!");
@@ -3711,14 +3768,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
 				}

--- a/app/android/jni/RTABMapApp.h
+++ b/app/android/jni/RTABMapApp.h
@@ -75,7 +75,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 +153,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,
@@ -265,6 +266,8 @@ 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_;
--- a/app/android/jni/jni_interface.cpp
+++ b/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
 	{
--- a/app/android/jni/point_cloud_drawable.cpp
+++ b/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");
--- a/app/android/jni/point_cloud_drawable.h
+++ b/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_;
--- a/app/android/jni/scene.cpp
+++ b/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);

@@ -633,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);
--- a/app/android/jni/scene.h
+++ b/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_;
--- a/app/android/jni/tango-gl/gesture_camera.cpp
+++ b/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;
--- a/app/android/res/values/strings.xml
+++ b/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,7 +80,7 @@
    <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>
@@ -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>
--- a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java
+++ b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java
@@ -688,7 +688,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();
@@ -736,7 +736,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();
 								}
--- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+++ b/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)) {
--- a/app/android/src/com/introlab/rtabmap/RTABMapLib.java
+++ b/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);
--- a/app/android/src/com/introlab/rtabmap/Util.java
+++ b/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];
--- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj
+++ b/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 = 8;
+				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.16;
+				MARKETING_VERSION = 0.20.19;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1038,7 +1039,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 8;
+				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.16;
+				MARKETING_VERSION = 0.20.19;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
--- a/app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme
+++ b/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">
--- a/app/ios/RTABMapApp/.gitignore
+++ b/app/ios/RTABMapApp/.gitignore
@@ -0,0 +1 @@
+Libraries
--- a/app/ios/RTABMapApp/Base.lproj/LaunchScreen.storyboard
+++ b/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>
--- a/app/ios/RTABMapApp/Base.lproj/Main.storyboard
+++ b/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"/>
--- a/app/ios/RTABMapApp/Libraries/.gitignore
+++ b/app/ios/RTABMapApp/Libraries/.gitignore
@@ -1,6 +0,0 @@
-# Ignore everything
-*
-
-# But not these files...
-!.gitignore
-!install_deps.sh
--- a/app/ios/RTABMapApp/NativeWrapper.cpp
+++ b/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)
--- a/app/ios/RTABMapApp/NativeWrapper.hpp
+++ b/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,
--- a/app/ios/RTABMapApp/RTABMap.swift
+++ b/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)))
    }
@@ -486,7 +490,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
 }
--- a/app/ios/RTABMapApp/ViewController.swift
+++ b/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 {
@@ -1414,12 +1418,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()
@@ -1427,15 +1438,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();
+            }
        }
    }
    
@@ -1519,12 +1636,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)
        })
@@ -1560,6 +1684,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                indicator?.bringSubviewToFront(self.view)
                
                indicator?.startAnimating()
+                
+                self.rtabmap!.cancelProcessing()
            })
            
        }))
@@ -1664,6 +1790,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
@@ -1800,7 +1927,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
        
@@ -1812,7 +1938,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) {
@@ -1974,6 +2100,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;
@@ -2073,7 +2200,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)")
@@ -2211,6 +2338,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 {
@@ -2323,3 +2458,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)
+        }
+    }
+}
--- a/app/ios/RTABMapApp/Libraries/install_deps.sh
+++ b/app/ios/RTABMapApp/Libraries/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
@@ -107,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
--- a/app/ios/Settings.bundle/License.plist
+++ b/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-2022, 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
--- a/app/ios/Settings.bundle/Root.plist
+++ b/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-2022, 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.16</string>
+			<string>0.20.19</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>
--- a/app/src/CMakeLists.txt
+++ b/app/src/CMakeLists.txt
@@ -135,6 +135,24 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
          COMPONENT runtime)
 	ENDIF(WIN32)
  ENDIF(k4a_FOUND)
+  
+  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 ${plugin_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)

  # Install needed Qt plugins by copying directories from the qt installation
  # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
@@ -154,17 +172,17 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 		list(GET loc_list 1 plugin_type)
 		IF(NOT plugin_root)
 			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
-		ENDIF(NOT plugin_root)
+		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()
+	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")
+			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)
--- a/app/src/main.cpp
+++ b/app/src/main.cpp
@@ -38,10 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <vtkObject.h>

-#ifdef RTABMAP_PYTHON
-#include "rtabmap/core/PythonInterface.h"
-#endif
-
 using namespace rtabmap;

 int main(int argc, char* argv[])
@@ -54,10 +50,6 @@ 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();
 #endif
--- a/archive/2020-IlluminationInvariant/README.md
+++ b/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)]
-
--- a/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg
+++ b/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg
--- a/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg
+++ b/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg
--- a/archive/2020-IlluminationInvariant/scripts/export_stats.sh
+++ b/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
-
--- a/archive/2020-IlluminationInvariant/scripts/reprocess_maps.sh
+++ b/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
-
--- a/archive/2020-IlluminationInvariant/scripts/reprocess_maps_all.sh
+++ b/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
-
--- a/archive/2020-IlluminationInvariant/scripts/run_all.sh
+++ b/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
-
--- a/archive/2020-IlluminationInvariant/scripts/run_consecutive_localization.sh
+++ b/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
-
--- a/archive/2020-IlluminationInvariant/scripts/run_consecutive_localization_all.sh
+++ b/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
-
--- a/archive/2020-IlluminationInvariant/scripts/run_localization_accuracy.sh
+++ b/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
-
-
--- a/archive/2020-IlluminationInvariant/scripts/run_localization_single.sh
+++ b/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
-
-
--- a/archive/2020-IlluminationInvariant/scripts/run_localization_single_all.sh
+++ b/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
-
--- a/archive/2020-IlluminationInvariant/scripts/run_merge.sh
+++ b/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
-
-
-
--- a/archive/2020-IlluminationInvariant/scripts/valgrind.sh
+++ b/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
-
--- a/archive/2022-IlluminationInvariant/README.md
+++ b/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://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EV8F4PZUxOxLhwAyEehlzKwBjF-9xNuxR32Q4mUjx5u-rA?e=eCJ3TW):
+  * `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://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EU5fb0jEKzlGhPK3OWjMGLUBnDo1BRAoZwtB2czyeVLE_A?e=Y0JyXY)
+
+
+
+## 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
+```
--- a/archive/2022-IlluminationInvariant/images/fig_map_merged_999.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_map_merged_999.jpg
--- a/archive/2022-IlluminationInvariant/images/fig_merged_percentage.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_merged_percentage.jpg
--- a/archive/2022-IlluminationInvariant/images/fig_overview.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_overview.jpg
--- a/archive/2022-IlluminationInvariant/images/fig_single_percentage.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_single_percentage.jpg
--- a/archive/2022-IlluminationInvariant/results_common.m
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/results_loc_over_time.m
+++ b/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
--- a/archive/2022-IlluminationInvariant/results_loc_prob_by_dist.m
+++ b/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)')
--- a/archive/2022-IlluminationInvariant/scripts/export_stats.sh
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/reprocess_maps.sh
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/reprocess_maps_all.sh
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/rtabmap_latest.bash
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/run_all.sh
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/run_consecutive_localization.sh
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/run_consecutive_localization_all.sh
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/run_localization_accuracy.sh
+++ b/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
+
+
--- a/archive/2022-IlluminationInvariant/scripts/run_localization_single.sh
+++ b/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
+
+
--- a/archive/2022-IlluminationInvariant/scripts/run_localization_single_all.sh
+++ b/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
+
--- a/archive/2022-IlluminationInvariant/scripts/run_merge.sh
+++ b/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
+
+
--- a/archive/2022-IlluminationInvariant/scripts/trace.py
+++ b/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")
--- a/archive/2022-IlluminationInvariant/scripts/valgrind.sh
+++ b/archive/2022-IlluminationInvariant/scripts/valgrind.sh
@@ -0,0 +1,20 @@
+#!/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)
+
+source rtabmap_latest.bash
+
+for d in "${DETECTOR[@]}"
+do
+  valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false  "${OUTPUT}/${d}/map_190321-164651.db;${INPUT}/loc_190321-165128.db" output.db
+  rm output.db
+done
+
--- a/archive/2022-IlluminationInvariant/showLoopOverTime.m
+++ b/archive/2022-IlluminationInvariant/showLoopOverTime.m
@@ -0,0 +1,108 @@
+##close all
+##clear all
+
+%% Use Export Poses in TORO format, then copy columns
+load vertexes.txt;
+load edges.txt;
+
+set(0,'defaultAxesFontName', 'Times')
+set(0,'defaultTextFontName', 'Times')
+
+%matlab indexes  % rtabmap indexes
+endMap1 = 201; % ID=206 
+endMap2 = 401; % ID=411 
+endMap3 = 604; % ID=621  
+endMap4 = 794; % ID=814
+endMap5 = 968; % ID=990
+endMap6 = 1201; % ID=1230
+
+%% 3D
+
+t = vertexes(:,1);
+
+##figure
+##plot3(vertexes(1:endMap1,2), vertexes(1:endMap1,3), vertexes(1:endMap1,1))
+##hold on
+##plot3(vertexes(endMap1+1:endMap2,2), vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1))
+##plot3(vertexes(endMap2+1:endMap3,2), vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1))
+##plot3(vertexes(endMap3+1:endMap4,2), vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1))
+##plot3(vertexes(endMap4+1:endMap5,2), vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1))
+##plot3(vertexes(endMap5+1:end,2), vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1))
+
+mapIds = zeros(vertexes(end,1), 2); % matlab index to vertexes, map id
+for i=1:size(vertexes,1)
+    mapIds(vertexes(i,1),1) = i;
+    if i <= endMap1
+        mapIds(vertexes(i,1),2) = 1;
+    elseif i<=endMap2
+        mapIds(vertexes(i,1),2) = 2;
+    elseif i<=endMap3
+        mapIds(vertexes(i,1),2) = 3;
+    elseif i<=endMap4
+        mapIds(vertexes(i,1),2) = 4;
+    elseif i<=endMap5
+        mapIds(vertexes(i,1),2) = 5;
+    else
+        mapIds(vertexes(i,1),2) = 6;
+    end
+        
+end
+
+##interLoopClosures = 0;
+##intraLoopClosures = 0;
+##
+##for i=1:size(edges, 1)
+##    if edges(i,2) > edges(i,1)+1
+##        x = [vertexes(mapIds(edges(i,1),1), 2) vertexes(mapIds(edges(i,2),1), 2)];
+##        y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
+##        t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
+##        if mapIds(edges(i,1),2) ~= mapIds(edges(i,2),2)
+##            plot3(x,y,t, 'g')
+##            interLoopClosures = interLoopClosures+1;
+##        else
+##            plot3(x,y,t, 'r')
+##            intraLoopClosures = intraLoopClosures + 1;
+##        end
+##    end
+##end
+##xlabel('x')
+##ylabel('y')
+##zlabel('Node indexes')
+##
+##interLoopClosures
+##intraLoopClosures
+
+%% 2D
+figure
+hold on
+plot([-8 6], [vertexes(endMap1,1) vertexes(endMap1,1)], 'k:')
+plot([-8 6], [vertexes(endMap2,1) vertexes(endMap2,1)], 'k:')
+plot([-8 6], [vertexes(endMap3,1) vertexes(endMap3,1)], 'k:')
+plot([-8 6], [vertexes(endMap4,1) vertexes(endMap4,1)], 'k:')
+plot([-8 6], [vertexes(endMap5,1) vertexes(endMap5,1)], 'k:')
+
+colors = {'r:', 'g:', 'c:', 'y:', 'm:', 'c'};
+
+for i=1:size(edges, 1)
+    if edges(i,2) > edges(i,1)+1
+        y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
+        t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
+        mapId = mapIds(edges(i,1),2);
+        if mapId ~= mapIds(edges(i,2),2) && (mapId == 1 || mapIds(edges(i,2),2) == 1)
+            plot(y,t, 'r')
+        else
+            %plot(y,t, 'r')
+        end
+    end
+end
+
+curveColor = 'b'
+plot(vertexes(1:endMap1,3), vertexes(1:endMap1,1), curveColor)
+plot(vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1), curveColor)
+plot(vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1), curveColor)
+plot(vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1), curveColor)
+plot(vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1), curveColor)
+plot(vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1), 'k')
+
+xlabel('y')
+ylabel('Node indexes')
--- a/cmake_modules/FindG2O.cmake
+++ b/cmake_modules/FindG2O.cmake
@@ -1,4 +1,5 @@
 #Pre-requisites: Look for csparse
+MESSAGE(STATUS "RTAB-Map's cmake g2o find module used for convenience (for older g2o versions)")
 FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse csparse EXTERNAL/suitesparse EXTERNAL/csparse g2o/EXTERNAL/suitesparse g2o/EXTERNAL/csparse
  PATHS "C:\\Program Files\\g2o\\include\\EXTERNAL")
 FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse g2o_ext_csparse 
@@ -21,6 +22,10 @@ FIND_FILE(G2O_CONFIG_FILE g2o/config.h
  PATHS ${G2O_INCLUDE_DIR}
  NO_DEFAULT_PATH)

+FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
 #ifdef G2O_NUMBER_FORMAT_STR
 #define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
 #endif
@@ -83,7 +88,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER

 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_FACTORY_FILE AND G2O_SOLVERS_FOUND)
  SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
  SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -120,8 +125,16 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL
    SET(G2O_CPP11 0)
  ELSE()
    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
-    SET(G2O_CPP11 1)
+    FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+    STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+    IF(${matchres} EQUAL -1)
+      MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 2)
+    ELSE()
+      MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 1)
+    ENDIF()
  ENDIF()

  SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_FACTORY_FILE AND G2O_SOLVERS_FOUND)
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -167,7 +167,7 @@ public:
 	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
-	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
+	bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -272,7 +272,7 @@ protected:
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;

 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
-	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
--- a/corelib/include/rtabmap/core/DBDriverSqlite3.h
+++ b/corelib/include/rtabmap/core/DBDriverSqlite3.h
@@ -137,7 +137,7 @@ protected:
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;

 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
-	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
--- a/corelib/include/rtabmap/core/DBReader.h
+++ b/corelib/include/rtabmap/core/DBReader.h
@@ -54,7 +54,10 @@ public:
 			 int cameraIndex = -1,
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
-			 bool landmarksIgnored = false);
+			 bool landmarksIgnored = false,
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
@@ -64,7 +67,10 @@ public:
 			 int cameraIndex = -1,
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
-			 bool landmarksIgnored = false);
+			 bool landmarksIgnored = false,
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1);
 	virtual ~DBReader();

 	virtual bool init(
@@ -75,6 +81,8 @@ public:
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const {return !_odometryIgnored;}

+	const DBDriver * driver() const {return _dbDriver;}
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);

@@ -91,6 +99,9 @@ private:
 	int _cameraIndex;
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
+	bool _featuresIgnored;
+	int _startMapId;
+	int _stopMapId;

 	DBDriver * _dbDriver;
 	UTimer _timer;
--- a/corelib/include/rtabmap/core/Graph.h
+++ b/corelib/include/rtabmap/core/Graph.h
@@ -125,7 +125,8 @@ void RTABMAP_EXP computeMaxGraphErrors(
 		float & maxLinearError,
 		float & maxAngularError,
 		const Link ** maxLinearErrorLink = 0,
-		const Link ** maxAngularErrorLink = 0);
+		const Link ** maxAngularErrorLink = 0,
+		bool for3DoF = false);

 std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links);

@@ -158,14 +159,14 @@ std::list<Link> RTABMAP_EXP findLinks(
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
 /**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
 */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
 /**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
 */
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,
@@ -266,54 +267,58 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float angularVelocity = 0.0f); // rad/sec

 /**
- * Get the nearest node of the target pose
+ * Find the nearest node of the target pose
 * @param nodes the nodes to search for
 * @param targetPose the target pose to search around
 * @param distance squared distance of the nearest node found (optional)
 * @return the node id.
 */
 int RTABMAP_EXP findNearestNode(
-		const std::map<int, rtabmap::Transform> & nodes,
+		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance = 0);

 /**
- * Get K nearest nodes of the target pose
- * @param nodes the nodes to search for
- * @param targetPose the target pose to search around
- * @param k number of nearest neighbors to search for
- * @return the node ids with squared distance to target pose.
- */
-std::map<int, float> RTABMAP_EXP findNearestNodes(
-		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
-		int k);
-
-/**
- * Get nodes near the query
+ * Find the nearest nodes of the query pose or node
 * @param nodeId the query id
 * @param nodes the nodes to search for
- * @param radius radius to search for (m)
+ * @param radius radius to search for (m), if 0, k should be > 0.
+ * @param k max nearest neighbors (0=all inside the radius)
 * @return the nodes with squared distance to query node.
 */
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius);
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius);
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
+		float angle = 0.0f,
+		int k=0);
+
+// typedef hack to avoid error with RTABMAP_DEPRECATED
+typedef std::map<int, float> _mapIntFloat;
+typedef std::map<int, Transform> _mapIntTransform;
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");

 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
--- a/corelib/include/rtabmap/core/MarkerDetector.h
+++ b/corelib/include/rtabmap/core/MarkerDetector.h
@@ -65,16 +65,22 @@ public:

    RTABMAP_DEPRECATED(
    MapIdPose detect(const cv::Mat & image,
-                                    const CameraModel & model,
-                                    const cv::Mat & depth = cv::Mat(),
-                                    float * estimatedMarkerLength = 0,
-                                    cv::Mat * imageWithDetections = 0), "Use the other constructor, in which the returned map contains the length of each marker detected.");
+			const CameraModel & model,
+			const cv::Mat & depth = cv::Mat(),
+			float * estimatedMarkerLength = 0,
+			cv::Mat * imageWithDetections = 0), "Use the other detect(), in which the returned map contains the length of each marker detected.");

    std::map<int, MarkerInfo> detect(const cv::Mat & image,
-                               const CameraModel & model,
-                               const cv::Mat & depth = cv::Mat(),
-                               const std::map<int, float> & markerLengths = std::map<int, float>(),
-                               cv::Mat * imageWithDetections = 0);
+		    const std::vector<CameraModel> & models,
+		    const cv::Mat & depth = cv::Mat(),
+		    const std::map<int, float> & markerLengths = std::map<int, float>(),
+		    cv::Mat * imageWithDetections = 0);
+
+    std::map<int, MarkerInfo> detect(const cv::Mat & image,
+		    const CameraModel & model,
+		    const cv::Mat & depth = cv::Mat(),
+		    const std::map<int, float> & markerLengths = std::map<int, float>(),
+		    cv::Mat * imageWithDetections = 0);

 private:
 #ifdef HAVE_OPENCV_ARUCO
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -55,6 +55,7 @@ class Statistics;
 class Registration;
 class RegistrationInfo;
 class RegistrationIcp;
+class RegistrationVis;
 class Stereo;
 class OccupancyGrid;
 class MarkerDetector;
@@ -205,7 +206,7 @@ public:
 			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
-			StereoCameraModel & stereoModel) const;
+			std::vector<StereoCameraModel> & stereoModels) const;
 	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -320,8 +321,10 @@ private:
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
 	float _laserScanNormalRadius;
+	float _laserScanGroundNormalsUp;
 	bool _reextractLoopClosureFeatures;
 	bool _localBundleOnLoopClosure;
+	bool _invertedReg;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
@@ -346,7 +349,7 @@ private:
 	bool _allNodesInWM;
 	GPS _gpsOrigin;
 	std::vector<CameraModel> _rectCameraModels;
-	StereoCameraModel _rectStereoCameraModel;
+	std::vector<StereoCameraModel> _rectStereoCameraModels;
 	std::vector<double> _odomMaxInf;

 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
@@ -366,6 +369,7 @@ private:

 	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcpMulti;
+	RegistrationVis * _registrationVis;

 	OccupancyGrid * _occupancy;

--- a/corelib/include/rtabmap/core/OccupancyGrid.h
+++ b/corelib/include/rtabmap/core/OccupancyGrid.h
@@ -122,6 +122,7 @@ private:
 	bool projMapFrame_;
 	float maxObstacleHeight_;
 	int normalKSearch_;
+	float groundNormalsUp_;
 	float maxGroundAngle_;
 	float clusterRadius_;
 	int minClusterSize_;
--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/corelib/include/rtabmap/core/Odometry.h
@@ -122,7 +122,7 @@ private:

 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
-	StereoCameraModel stereoModel_;
+	std::vector<StereoCameraModel> stereoModels_;
 	std::vector<CameraModel> models_;
 	std::map<double, Transform> imus_;

--- a/corelib/include/rtabmap/core/OdometryInfo.h
+++ b/corelib/include/rtabmap/core/OdometryInfo.h
@@ -103,7 +103,7 @@ public:
 	int localBundleConstraints;
 	float localBundleTime;
 	std::map<int, Transform> localBundlePoses;
-	std::map<int, CameraModel> localBundleModels;
+	std::map<int, std::vector<CameraModel> > localBundleModels;
 	bool keyFrameAdded;
 	float timeEstimation;
 	float timeParticleFiltering;
--- a/corelib/include/rtabmap/core/Optimizer.h
+++ b/corelib/include/rtabmap/core/Optimizer.h
@@ -41,14 +41,18 @@ namespace rtabmap {
 class FeatureBA
 {
 public:
-	FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat()):
+	FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat(), int cameraIndexIn = 0):
 		kpt(kptIn),
 		depth(depthIn),
-		descriptor(descriptorIn)
-	{}
+		descriptor(descriptorIn),
+		cameraIndex(cameraIndexIn)
+	{
+		//UDEBUG("kpt=(%f,%f) depth=%f, camIndex=%d", kpt.pt.x, kpt.pt.y, depth, cameraIndex);
+	}
 	cv::KeyPoint kpt;
 	float depth;
 	cv::Mat descriptor;
+	int cameraIndex;
 };

 ////////////////////////////////////////////
@@ -75,7 +79,8 @@ public:
 			const std::map<int, Transform> & posesIn,
 			const std::multimap<int, Link> & linksIn,
 			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut) const;
+			std::multimap<int, Link> & linksOut,
+			bool adjustPosesWithConstraints = true) const;

 public:
 	virtual ~Optimizer() {}
@@ -133,7 +138,7 @@ public:
 			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
+			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
 			std::set<int> * outliers = 0);
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -183,7 +183,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
    RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
    RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "",          "Working directory.");
-    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum nodes retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRepublished,           unsigned int, 2, uFormat("Maximum nodes republished when requesting missing data. When %s=false, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if %s=false.", kRGBDEnabled().c_str(), kRtabmapPublishLastSignature().c_str()));
    RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true,  "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
    RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
    RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
@@ -370,6 +371,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess,     bool, false,  uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
    RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes. Raw features are not saved in database.");
    RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
+    RTABMAP_PARAM(RGBD, InvertedReg,                  bool, false,  "On loop closure, do registration from the target to reference instead of reference to target.");
    RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
    RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
    RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
@@ -385,7 +387,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
-    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str(), kRGBDProximityPathRawPosesUsed().c_str()));
+    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str()));
+    RTABMAP_PARAM(RGBD, ProximityMergedScanCovFactor, double, 100.0, uFormat("Covariance factor for one-to-many proximity detection (when %s>0 and scans are used).", kRGBDProximityPathMaxNeighbors().c_str()));

    // Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -593,6 +596,7 @@ class RTABMAP_EXP Parameters
 #else
    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #endif
+    RTABMAP_PARAM(Vis, PnPMaxVariance,           float, 0.0,    uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));

    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.1,    uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
@@ -669,7 +673,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
 #endif
    RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 0.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
    RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
@@ -779,8 +783,11 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
-    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,    "Maximum range in which markers will be detected. <=0 for unlimited range.");
-    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,    "Miniminum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
+    RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
+    RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");

    RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
    RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
--- a/corelib/include/rtabmap/core/Registration.h
+++ b/corelib/include/rtabmap/core/Registration.h
@@ -45,7 +45,8 @@ public:
 		kTypeIcp = 1,
 		kTypeVisIcp = 2
 	};
-	static double COVARIANCE_EPSILON;
+	static double COVARIANCE_LINEAR_EPSILON;
+	static double COVARIANCE_ANGULAR_EPSILON;

 public:
 	static Registration * create(const ParametersMap & parameters);
--- a/corelib/include/rtabmap/core/RegistrationVis.h
+++ b/corelib/include/rtabmap/core/RegistrationVis.h
@@ -82,6 +82,7 @@ private:
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;
+	float _PnPMaxVar;
 	int _correspondencesApproach;
 	int _flowWinSize;
 	int _flowIterations;
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -49,6 +49,7 @@ class Memory;
 class BayesFilter;
 class Signature;
 class Optimizer;
+class PythonInterface;

 class RTABMAP_EXP Rtabmap
 {
@@ -196,8 +197,8 @@ public:
 			bool withGrid = false,
 			bool withWords = true,
 			bool withGlobalDescriptors = true) const;
-	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
-	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
 			float clusterRadiusMax = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -222,6 +223,7 @@ public:
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
+	void addNodesToRepublish(const std::vector<int> & ids);

 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -283,6 +285,7 @@ private:
 	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
+	unsigned int _maxRepublished;
 	bool _rawDataKept;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
@@ -307,6 +310,7 @@ private:
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
 	bool _proximityOdomGuess;
+	double _proximityMergedScanCovFactor;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;
@@ -322,6 +326,8 @@ private:
 	bool _loopGPS;
 	int _maxOdomCacheSize;
 	bool _createGlobalScanMap;
+	float _markerPriorsLinearVariance;
+	float _markerPriorsAngularVariance;

 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -362,6 +368,9 @@ private:
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
 	std::vector<float> _odomCorrectionAcc;
+	std::map<int, Transform> _markerPriors;
+
+	std::set<int> _nodesToRepublish;

 	// Planning stuff
 	int _pathStatus;
@@ -373,6 +382,10 @@ private:
 	int _pathStuckCount;
 	float _pathStuckDistance;

+#ifdef RTABMAP_PYTHON
+	PythonInterface * _python;
+#endif
+
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/RtabmapEvent.h
+++ b/corelib/include/rtabmap/core/RtabmapEvent.h
@@ -59,21 +59,25 @@ class RtabmapEventCmd : public UEvent
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
 	enum Cmd {
+			kCmdUndef,
 			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
 			kCmdClose,            // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
+			kCmdUpdateParams,     // params: ParametersMap
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
 			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
+			kCmdRepublishData,    // params: [vector<int>] ids
 			kCmdTriggerNewMap,
 			kCmdPause,
 			kCmdResume,
 			kCmdGoal,             // params: [string] label or [int] location ID
 			kCmdCancelGoal,
-			kCmdLabel             // params: [string] label, [int] location ID
+			kCmdLabel,            // params: [string] label, [int] location ID
+			kCmdRemoveLabel       // params: [string] label
 	};
 public:
 	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :
--- a/corelib/include/rtabmap/core/RtabmapThread.h
+++ b/corelib/include/rtabmap/core/RtabmapThread.h
@@ -54,21 +54,8 @@ class RTABMAP_EXP RtabmapThread :
 {
 public:
 	enum State {
-		kStateInit,
 		kStateDetecting,
-		kStateReseting,
-		kStateClose,
-		kStateChangingParameters,
-		kStateDumpingMemory,
-		kStateDumpingPrediction,
-		kStateExportingDOTGraph,
-		kStateExportingPoses,
-		kStateCleanDataBuffer,
-		kStatePublishingMap,
-		kStateTriggeringMap,
-		kStateSettingGoal,
-		kStateCancellingGoal,
-		kStateLabelling
+		kStateProcessCommand
 	};

 public:
@@ -104,13 +91,13 @@ private:
 	void process();
 	void addData(const OdometryEvent & odomEvent);
 	bool getData(OdometryEvent & data);
-	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
+	void pushNewState(State newState, const RtabmapEventCmd & cmdEvent = RtabmapEventCmd(RtabmapEventCmd::kCmdUndef));
 	void publishMap(bool optimized, bool full, bool graphOnly) const;

 private:
 	UMutex _stateMutex;
 	std::queue<State> _state;
-	std::queue<ParametersMap> _stateParam;
+	std::queue<RtabmapEventCmd> _stateParam;

 	std::list<OdometryEvent> _dataBuffer;
 	std::list<double> _newMapEvents;
--- a/corelib/include/rtabmap/core/SensorData.h
+++ b/corelib/include/rtabmap/core/SensorData.h
@@ -126,6 +126,25 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());

+	// Multi-cameras stereo constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<StereoCameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras stereo constructor + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<StereoCameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
 	// IMU constructor
 	SensorData(
 			const IMU & imu,
@@ -143,8 +162,8 @@ public:
 			_depthOrRightCompressed.empty() &&
 			_laserScanRaw.isEmpty() &&
 			_laserScanCompressed.isEmpty() &&
-			_cameraModels.size() == 0 &&
-			!_stereoCameraModel.isValidForProjection() &&
+			_cameraModels.empty() &&
+			_stereoCameraModels.empty() &&
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
@@ -173,6 +192,7 @@ public:
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const std::vector<CameraModel> & models, bool clearPreviousData = true);
 	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & stereoCameraModel, bool clearPreviousData = true);
+	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const std::vector<StereoCameraModel> & stereoCameraModels, bool clearPreviousData = true);

 	/**
 	 * Set laser scan data. Detect automatically if raw or compressed.
@@ -183,7 +203,8 @@ public:

 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
-	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
+	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModels.clear(); _stereoCameraModels.push_back(stereoCameraModel);}
+	void setStereoCameraModels(const std::vector<StereoCameraModel> & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;}

 	//for convenience
 	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
@@ -213,7 +234,7 @@ public:
 			cv::Mat * emptyCellsRaw = 0) const;

 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
-	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
+	const std::vector<StereoCameraModel> & stereoCameraModels() const {return _stereoCameraModels;}

 	/**
 	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
@@ -302,7 +323,7 @@ private:
 	LaserScan _laserScanRaw;

 	std::vector<CameraModel> _cameraModels;
-	StereoCameraModel _stereoCameraModel;
+	std::vector<StereoCameraModel> _stereoCameraModels;

 	// user data
 	cv::Mat _userDataCompressed;      // compressed data
--- a/corelib/include/rtabmap/core/Statistics.h
+++ b/corelib/include/rtabmap/core/Statistics.h
@@ -151,6 +151,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Odom_cache_links,);
 	RTABMAP_STATS(Memory, Small_movement,);
 	RTABMAP_STATS(Memory, Fast_movement,);
+	RTABMAP_STATS(Memory, New_landmark,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
@@ -240,7 +241,9 @@ public:
 	void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
 	void setStamp(double stamp) {_stamp = stamp;}

-	void setLastSignatureData(const Signature & data) {_lastSignatureData = data;}
+	RTABMAP_DEPRECATED(void setLastSignatureData(const Signature & data), "Use addSignatureData() instead.");
+	void addSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}
+	void setSignaturesData(const std::map<int, Signature> & data) {_signaturesData = data;}

 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -269,7 +272,8 @@ public:
 	int proximityDetectionMapId() const {return _proximiyDetectionMapId;}
 	double stamp() const {return _stamp;}

-	const Signature & getLastSignatureData() const {return _lastSignatureData;}
+	const Signature & getLastSignatureData() const {return _signaturesData.empty()?_dummyEmptyData:_signaturesData.rbegin()->second;}
+	const std::map<int, Signature> & getSignaturesData() const {return _signaturesData;}

 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -301,7 +305,8 @@ private:
 	int _proximiyDetectionMapId;
 	double _stamp;

-	Signature _lastSignatureData;
+	std::map<int, Signature> _signaturesData;
+	Signature _dummyEmptyData;

 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
--- a/corelib/include/rtabmap/core/Transform.h
+++ b/corelib/include/rtabmap/core/Transform.h
@@ -104,6 +104,8 @@ public:
 	Transform translation() const;
 	Transform to3DoF() const;
 	Transform to4DoF() const;
+	bool is3DoF() const;
+	bool is4DoF() const;

 	cv::Mat rotationMatrix() const;
 	cv::Mat translationMatrix() const;
@@ -141,6 +143,8 @@ public:
 	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
+	static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
+	static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);

 	static Transform opengl_T_rtabmap() {return Transform(
 			 0.0f, -1.0f, 0.0f, 0.0f,
--- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h
+++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h
@@ -58,6 +58,8 @@ public:
 	virtual ~CameraDepthAI();

 	void setOutputDepth(bool enabled, int confidence = 200);
+	void setIMUFirmwareUpdate(bool enabled);
+	void setIMUPublished(bool published);

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -74,6 +76,8 @@ private:
 	bool outputDepth_;
 	int depthConfidence_;
 	int resolution_;
+	bool imuFirmwareUpdate_;
+	bool imuPublished_;
 	std::shared_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
--- a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
+++ b/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
@@ -126,6 +126,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			UDEBUG("Cluster radius=%f", clusterRadius_);
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
 			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
+			UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -138,7 +139,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 					flatObstaclesDetected_,
 					maxGroundHeight_,
 					flatObstacles,
-					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
+					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
+					groundNormalsUp_);
 			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
@@ -166,7 +168,11 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
 		{
-			UDEBUG("");
+			UDEBUG("Radius filtering (%ld ground %ld obstacles, radius=%f k=%d)",
+					groundIndices->size(),
+					obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0),
+					noiseFilteringRadius_,
+					noiseFilteringMinNeighbors_);
 			if(groundIndices->size())
 			{
 				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
@@ -179,6 +185,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			{
 				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
 			}
+			UDEBUG("Radius filtering end (%ld ground %ld obstacles)",
+					groundIndices->size(),
+					obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0));

 			if(groundIndices->empty() && obstaclesIndices->empty())
 			{
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -63,7 +63,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
-		const Eigen::Vector4f & viewPoint)
+		const Eigen::Vector4f & viewPoint,
+		float groundNormalsUp)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -81,7 +82,8 @@ void segmentObstaclesFromGround(
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
 				normalKSearch,
-				viewPoint);
+				viewPoint,
+				groundNormalsUp);

 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
@@ -107,7 +109,7 @@ void segmentObstaclesFromGround(
 					{
 						Eigen::Vector4f min,max;
 						pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
-						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
+						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.at(i)->size() > points)
 						{
 							points = clusteredFlatSurfaces.at(i)->size();
 							biggestFlatSurfaceIndex = i;
@@ -205,7 +207,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
-		const Eigen::Vector4f & viewPoint)
+		const Eigen::Vector4f & viewPoint,
+		float groundNormalsUp)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -220,7 +223,8 @@ void segmentObstaclesFromGround(
 			segmentFlatObstacles,
 			maxGroundHeight,
 			flatObstacles,
-			viewPoint);
+			viewPoint,
+			groundNormalsUp);
 }

 template<typename PointT>
--- a/corelib/include/rtabmap/core/odometry/OdometryF2M.h
+++ b/corelib/include/rtabmap/core/odometry/OdometryF2M.h
@@ -83,7 +83,7 @@ private:
 	std::map<int, Transform> bundlePoses_;
 	std::multimap<int, Link> bundleLinks_;
 	std::multimap<int, Link> bundleIMUOrientations_;
-	std::map<int, CameraModel> bundleModels_;
+	std::map<int, std::vector<CameraModel> > bundleModels_;
 	std::map<int, int> bundlePoseReferences_;
 	int bundleSeq_;
 	Optimizer * sba_;
--- a/Show More
+++ b/Show More