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

* explicitly set deps to install instead of relying on buildfarm rtabmap (in case buildfarm build is failing) * use rosdep instead * missing rosdep update * updated rosdep command * removed rosdep init * fixed qt_gui_cpp not found * revert last commit, should be able to use qt_gui_cpp in rosdep * removed deprecated 18.04
bump 0.21.1
2026-09-01 17:10:26 +08:00 · 2023-04-09 14:40:57 -07:00 · 2023-04-09 12:00:53 -07:00 · 2023-04-06 20:57:35 -07:00 · 2023-04-05 22:19:54 -07:00 · 2023-03-25 22:41:28 -07:00
508 changed files with 18652 additions and 10297 deletions
--- a/.dockerignore
+++ b/.dockerignore
@@ -0,0 +1 @@
+build/*
--- a/.github/workflows/cmake-ros.yml
+++ b/.github/workflows/cmake-ros.yml
@@ -0,0 +1,66 @@
+name: CMake-ROS
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        ros_distribution: [ noetic, foxy, humble, rolling]
+        include:
+        - ros_distribution: 'noetic'
+          os: ubuntu-20.04
+        - ros_distribution: 'foxy'
+          os: ubuntu-20.04
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-22.04
+
+    steps:
+    - name: Workaround dpkg grub-efi-amd64-signed error
+      run: |
+        sudo apt-mark hold grub-efi-amd64-signed
+    
+    - uses: ros-tooling/setup-ros@v0.5
+      with:
+        required-ros-distributions: ${{ matrix.ros_distribution }}
+    
+    - uses: actions/checkout@v2
+
+    - name: Install dependencies
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        rosdep update
+        rosdep install --from-paths ${{github.workspace}} -y
+
+    - name: Configure CMake
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        ./rtabmap-console --version
+      
--- a/.github/workflows/cmake.yml
+++ b/.github/workflows/cmake.yml
@@ -2,59 +2,42 @@ name: CMake

 on:
  push:
-    branches: [ master ]
+    branches: 
+      - master
  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]

-    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}}/bin
+      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, focal-foxy, jammy, android23, android24, android26, android30]
        include:
        - docker_tag: xenial
          docker_tags: |
            introlab3it/rtabmap:xenial
            introlab3it/rtabmap:16.04
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
          docker_path: 'xenial'
@@ -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,29 +37,65 @@ jobs:
            introlab3it/rtabmap:focal
            introlab3it/rtabmap:20.04
            introlab3it/rtabmap:latest
+          docker_args: |
+            NOT_USED=0
          docker_platforms: |
            linux/amd64
            linux/arm64
          docker_path: 'focal'
+        - docker_tag: focal-foxy
+          docker_tags: |
+            introlab3it/rtabmap:focal-foxy
+            introlab3it/rtabmap:20.04-foxy
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal-foxy'
+        - docker_tag: jammy
+          docker_tags: |
+            introlab3it/rtabmap:jammy
+            introlab3it/rtabmap:22.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy'
        - docker_tag: android23
          docker_tags: |
            introlab3it/rtabmap:android23
            introlab3it/rtabmap:tango
+          docker_args: |
+            API_VERSION=23
          docker_platforms: |
            linux/amd64
-          docker_path: 'bionic/android/rtabmap_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 +119,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/.gitignore
+++ b/.gitignore
@@ -10,3 +10,5 @@ app/android/AndroidManifest.xml
 app/android/res/raw/
 compile_flags.txt
 tags
+build_*
+*.bak
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 15)
+SET(RTABMAP_MINOR_VERSION 21)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -69,7 +69,11 @@ if(POLICY CMP0020)
    cmake_policy(SET CMP0020 NEW)
 endif()
 if(POLICY CMP0043)
-    cmake_policy(SET CMP0043 OLD)
+    cmake_policy(SET CMP0043 NEW)
+endif()
+# To suppress g2o related opengl warning
+if(POLICY CMP0072)
+    cmake_policy(SET CMP0072 NEW)
 endif()

 IF(MINGW)
@@ -106,6 +110,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) 
@@ -123,9 +128,9 @@ OPTION( BUILD_SHARED_LIBS "Set to OFF to build static libraries" ON )


 ####### OUTPUT DIR #######
-SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
-SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
-SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin)
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin)
+SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib)

 # Avoid Visual Studio bin/Release and bin/Debug sub directories
 SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
@@ -145,7 +150,7 @@ endif()
 set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR})
  
 ####### BUILD OPTIONS #######
-
+include (GenerateExportHeader)
 # ANDROID_PREBUILD (early exit if true)
 OPTION( ANDROID_PREBUILD "Set to ON to build rtabmap resource build tool (required for android build)" OFF )
 IF(ANDROID_PREBUILD)
@@ -181,12 +186,14 @@ option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
 option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_TORO          "Include TORO support"                 ON)
-option(WITH_CERES         "Include Ceres support"                ON)
+option(WITH_CERES         "Include Ceres support"                OFF)
+option(WITH_MRPT          "Include MRPT support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
-option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_CVSBA         "Include cvsba support"                OFF)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
-option(WITH_CCCORELIB     "Include CCCoreLib support"            ON)
-option(WITH_LOAM          "Include LOAM support"                 ON)
+option(WITH_CCCORELIB     "Include CCCoreLib support"            OFF)
+option(WITH_OPEN3D        "Include Open3D support"               OFF)
+option(WITH_LOAM          "Include LOAM support"                 OFF)
 option(WITH_FLOAM         "Include FLOAM support"                OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
@@ -195,22 +202,23 @@ option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
-option(WITH_DEPTHAI       "Include depthai-core support"         ON)
+option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
-option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
-option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
+option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
+option(WITH_OPENCHISEL    "Include open_chisel support"          OFF)
 option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)
-option(WITH_FOVIS         "Include FOVIS support"                ON)
-option(WITH_VISO2         "Include VISO2 support"                ON)
-option(WITH_DVO           "Include DVO support"                  ON)
-option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" ON)
-option(WITH_OKVIS         "Include OKVIS support"                ON)
-option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"           OFF)
-option(WITH_VINS          "Include VINS-Fusion support"          ON)
-option(WITH_OPENVINS      "Include OpenVINS support"             ON)
+option(WITH_FOVIS         "Include FOVIS support"                OFF)
+option(WITH_VISO2         "Include VISO2 support"                OFF)
+option(WITH_DVO           "Include DVO support"                  OFF)
+option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" OFF)
+option(WITH_OKVIS         "Include OKVIS support"                OFF)
+option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
+option(WITH_VINS          "Include VINS-Fusion support"          OFF)
+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()
@@ -218,9 +226,9 @@ option(PCL_OMP            "With PCL OMP implementations"         ON)
 ENDIF()

 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
-set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5)
+set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)

-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)
@@ -289,35 +297,50 @@ IF(WITH_QT)
    ENDIF(NOT VTK_FOUND)

    # If Qt is here, the GUI will be built
-    # look for Qt5 (if vtk>5 is installed) before Qt4
-    IF("${VTK_MAJOR_VERSION}" GREATER 5)
-        if(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
-            FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
-            IF(Qt5_FOUND)
-                FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
-            ENDIF(Qt5_FOUND)
+    IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
+	  IF(NOT VTK_QT_VERSION)
+	    MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.")
+	  ENDIF()
+      MESSAGE(STATUS "VTK>=9 detected, will use VTK_QT_VERSION=${VTK_QT_VERSION} for Qt version.")
+      IF(${VTK_QT_VERSION} EQUAL 6)
+        FIND_PACKAGE(Qt6 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
+      ELSEIF(${VTK_QT_VERSION} EQUAL 5)
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
+      ELSE()
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
+      ENDIF()
+    ELSE()
+      # look for Qt5 (if vtk>5 is installed) before Qt4
+      IF("${VTK_MAJOR_VERSION}" GREATER 5)
+        IF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
+          FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
        ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
-    ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
+      ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)

-    IF(NOT Qt5_FOUND)
+      IF(NOT Qt5_FOUND)
        IF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4")
-            FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
+          FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
        ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4")
-    ENDIF(NOT Qt5_FOUND)
+      ENDIF(NOT Qt5_FOUND)
+    ENDIF()
        
-    IF(QT4_FOUND OR Qt5_FOUND)
+    IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
        IF("${VTK_MAJOR_VERSION}" EQUAL 5)
            FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
        ELSE()
-		    list(FIND PCL_LIBRARIES VTK::GUISupportQt value)
-			IF(value EQUAL -1)
+            list(FIND PCL_LIBRARIES VTK::GUISupportQt value)
+            IF(value EQUAL -1)
                list(FIND PCL_LIBRARIES vtkGUISupportQt value)
                IF(value EQUAL -1)
-                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
+                        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)
-		    
+            ENDIF(value EQUAL -1)
+
            MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}")

            IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
@@ -335,19 +358,26 @@ IF(WITH_QT)
                ENDIF(value EQUAL -1)
            ELSEIF("${VTK_MAJOR_VERSION}" EQUAL 9)
                list(FIND PCL_LIBRARIES VTK::RenderingOpenGL2 value)
-			    IF(NOT value EQUAL -1)
-			        list(FIND PCL_LIBRARIES VTK::RenderingVolumeOpenGL2 value)
+                IF(NOT value EQUAL -1)
+                    list(FIND PCL_LIBRARIES VTK::RenderingVolumeOpenGL2 value)
                    IF(value EQUAL -1)
                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::RenderingVolumeOpenGL2")
                    ENDIF(value EQUAL -1)
-			    ENDIF(NOT value EQUAL -1)
+                ENDIF(NOT value EQUAL -1)
            ENDIF()

        ENDIF()
        ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals/foreach and Qt macros
-    ENDIF(QT4_FOUND OR Qt5_FOUND)
+    ENDIF(QT4_FOUND OR Qt5_FOUND OR Qt6_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)
@@ -356,7 +386,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)
@@ -396,7 +426,7 @@ ENDIF(WITH_FREENECT2)

 IF(WITH_K4W2 AND WIN32)
    FIND_PACKAGE(KinectSDK2 QUIET)
-	IF(KinectSDK2_FOUND)
+    IF(KinectSDK2_FOUND)
       MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}")
    ENDIF(KinectSDK2_FOUND)
 ENDIF(WITH_K4W2 AND WIN32)
@@ -411,7 +441,7 @@ IF(WITH_K4A)
            SET(k4a_FOUND FALSE)
        ENDIF(NOT (k4a_FOUND AND k4arecord_FOUND))
    ENDIF()
-	IF(k4a_FOUND)
+    IF(k4a_FOUND)
       MESSAGE(STATUS "Found Kinect for Azure: ${k4a_INCLUDE_DIRS}")
    ENDIF(k4a_FOUND)
 ENDIF(WITH_K4A)
@@ -432,16 +462,29 @@ IF(WITH_DC1394)
 ENDIF(WITH_DC1394)

 IF(WITH_G2O)
-	FIND_PACKAGE(g2o QUIET NO_MODULE)
+    FIND_PACKAGE(g2o 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)

@@ -450,6 +493,14 @@ IF(WITH_GTSAM)
    FIND_PACKAGE(GTSAM CONFIG QUIET)
 ENDIF(WITH_GTSAM)

+IF(WITH_MRPT)
+    FIND_PACKAGE(MRPT COMPONENTS poses QUIET)
+    IF(MRPT_FOUND)
+        message(STATUS "MRPT_VERSION: ${MRPT_VERSION}")
+        message(STATUS "MRPT_LIBRARIES: ${MRPT_LIBRARIES}")
+    ENDIF(MRPT_FOUND)
+ENDIF(WITH_MRPT)
+
 IF(WITH_FLYCAPTURE2)
    FIND_PACKAGE(FlyCapture2 QUIET)
    IF(FlyCapture2_FOUND)
@@ -474,11 +525,11 @@ 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}")
-	    link_directories(${Boost_LIBRARY_DIRS})
+        MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
+        link_directories(${Boost_LIBRARY_DIRS})
    ENDIF(WIN32)
 ENDIF(libpointmatcher_FOUND OR GTSAM_FOUND)

@@ -489,6 +540,19 @@ IF(WITH_CCCORELIB)
    ENDIF(CCCoreLib_FOUND)
 ENDIF(WITH_CCCORELIB)

+IF(WITH_OPEN3D)
+    IF(${CMAKE_VERSION} VERSION_LESS "3.19.0") 
+       MESSAGE(WARNING "Open3D requires CMake version >=3.19 (current is ${CMAKE_VERSION})")
+    ELSE()
+       # Build Open3D like this to avoid linker errors in rtabmap:
+       # cmake -DBUILD_SHARED_LIBS=ON -DGLIBCXX_USE_CXX11_ABI=ON -DCMAKE_BUILD_TYPE=Release ..
+       find_package(Open3D QUIET)
+       IF(Open3D_FOUND)
+          MESSAGE(STATUS "Found Open3D: ${Open3DINCLUDE_DIRS}")
+       ENDIF(Open3D_FOUND)
+    ENDIF()
+ENDIF(WITH_OPEN3D)
+
 IF(WITH_LOAM)
    find_package(loam_velodyne QUIET)
    IF(loam_velodyne_FOUND)
@@ -687,6 +751,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)
@@ -695,51 +766,62 @@ 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)
-	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
-	 include(CheckCXXCompilerFlag)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-	  IF(COMPILER_SUPPORTS_CXX14)
-	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-	    set(CMAKE_CXX_STANDARD 14)
-	  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)
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1))
+      # Qt6 requires c++17
+      include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
+      IF(COMPILER_SUPPORTS_CXX17)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
+        set(CMAKE_CXX_STANDARD 17)
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler if you want to use Qt6.")
+      ENDIF()
+    ENDIF()
+    IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND ((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)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+        set(CMAKE_CXX_STANDARD 14)
+      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()

-	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
-	       G2O_FOUND OR
-	       GTSAM_FOUND OR
-	       CERES_FOUND OR
-	       ZED_FOUND OR
-	       ZEDOC_FOUND OR
-	       ANDROID OR
-	       RealSense_FOUND OR
-	       realsense2_FOUND OR
-	       ORB_SLAM_FOUND OR
-	       okvis_FOUND OR
-	       open_chisel_FOUND OR
-	       msckf_vio_FOUND OR
-	       vins_FOUND OR
-	       ov_msckf_FOUND OR
-	       libpointmatcher_FOUND))
-	 #Newest versions require std11
-	 include(CheckCXXCompilerFlag)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-	  IF(COMPILER_SUPPORTS_CXX11)
-	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-	  ELSEIF(COMPILER_SUPPORTS_CXX0X)
-	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-	  ELSE()
-	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-	  ENDIF()
-	ENDIF()
+    IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "17") AND NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
+           G2O_FOUND OR
+           GTSAM_FOUND OR
+           CERES_FOUND OR
+           ZED_FOUND OR
+           ZEDOC_FOUND OR
+           ANDROID OR
+           RealSense_FOUND OR
+           realsense2_FOUND OR
+           ORB_SLAM_FOUND OR
+           okvis_FOUND OR
+           open_chisel_FOUND OR
+           msckf_vio_FOUND OR
+           vins_FOUND OR
+           ov_msckf_FOUND OR
+           libpointmatcher_FOUND))
+     #Newest versions require std11
+     include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+      IF(COMPILER_SUPPORTS_CXX11)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+      ELSEIF(COMPILER_SUPPORTS_CXX0X)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+      ENDIF()
+    ENDIF()
 ENDIF()

 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
-  IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
+  IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
       
    # Required when packaging, and set CMAKE_INSTALL_PREFIX to "/".
    SET(CMAKE_INSTALL_PREFIX "/")
@@ -767,11 +849,6 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)

 ####### SOURCES (Projects) #######

-# CONF_DEPENDENCIES contains only dependencies not required by the headers
-SET(CONF_DEPENDENCIES 
-    ${ZLIB_LIBRARIES}
-)
-
 # OpenCV2 has nonfree if OPENCV_NONFREE_FOUND
 # OpenCV<=3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND
 # OpenCV>3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND and OPENCV_ENABLE_NONFREE is defined
@@ -790,17 +867,18 @@ ENDIF()

 IF(NOT G2O_FOUND)
   SET(G2O "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
-   IF(NOT G2O_CPP11)
   SET(G2O_CPP_CONF "//")
+ELSE()
+   IF(NOT G2O_CPP11)
+     SET(G2O_CPP_CONF "//")
   ENDIF(NOT G2O_CPP11)
 ENDIF()
 IF(NOT GTSAM_FOUND)
   SET(GTSAM "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
 ENDIF()
+IF(NOT MRPT_FOUND)
+   SET(MRPT "//")
+ENDIF(NOT MRPT_FOUND)
 IF(NOT CERES_FOUND)
   SET(CERES "//")
 ENDIF(NOT CERES_FOUND)
@@ -812,8 +890,6 @@ IF(NOT WITH_VERTIGO)
 ENDIF(NOT WITH_VERTIGO)
 IF(NOT cvsba_FOUND)
   SET(CVSBA "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES})
 ENDIF()
 IF(NOT libpointmatcher_FOUND)
   SET(POINTMATCHER "//")
@@ -821,9 +897,15 @@ ENDIF(NOT libpointmatcher_FOUND)
 IF(NOT CCCoreLib_FOUND)
   SET(CCCORELIB "//")
 ENDIF(NOT CCCoreLib_FOUND)
+IF(NOT Open3D_FOUND)
+   SET(OPEN3D "//")
+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)
@@ -835,61 +917,61 @@ IF(NOT floam_FOUND)
 ENDIF(NOT floam_FOUND)
 IF(NOT Freenect_FOUND)
   SET(FREENECT "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${Freenect_LIBRARIES})
 ENDIF()
 IF(NOT freenect2_FOUND)
   SET(FREENECT2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
 ENDIF()
 IF(NOT KinectSDK2_FOUND)
   SET(K4W2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES})
 ENDIF()
 IF(NOT k4a_FOUND)
   SET(K4A "//")
+   SET(CONF_WITH_K4A 0)
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${k4a_LIBRARIES})
+   SET(CONF_WITH_K4A 1)
+   IF(WIN32)
+      install(
+         FILES
+         "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/FindK4A.cmake"
+         DESTINATION ${INSTALL_CMAKE_DIR}/Modules/.
+         COMPONENT devel
+      )
+   ENDIF(WIN32)
 ENDIF()
 IF(NOT OpenNI2_FOUND)
   SET(OPENNI2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OpenNI2_LIBRARIES})
 ENDIF()
 IF(NOT DC1394_FOUND)
   SET(DC1394 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${DC1394_LIBRARIES})
 ENDIF()
 IF(NOT FlyCapture2_FOUND)
   SET(FLYCAPTURE2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${FlyCapture2_LIBRARIES})
 ENDIF()
 IF(NOT ZED_FOUND)
   SET(ZED "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
 ENDIF()
 IF(NOT ZEDOC_FOUND)
   SET(ZEDOC "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZEDOC_LIBRARIES}) 
 ENDIF()
 IF(NOT RealSense_FOUND)
   SET(REALSENSE "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) 
 ENDIF()
 IF(NOT RealSenseSlam_FOUND)
   SET(REALSENSESLAM "//")
 ENDIF(NOT RealSenseSlam_FOUND)
 IF(NOT realsense2_FOUND)
   SET(REALSENSE2 "//")
+   SET(CONF_WITH_REALSENSE2 0)
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) 
+   SET(CONF_WITH_REALSENSE2 1)
+   IF(WIN32)
+      install(
+         FILES
+         "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/FindRealSense2.cmake"
+         DESTINATION ${INSTALL_CMAKE_DIR}/Modules/.
+         COMPONENT devel
+      )
+   ENDIF(WIN32)
 ENDIF()
 IF(NOT mynteye_FOUND)
   SET(MYNTEYE "//")
@@ -899,65 +981,45 @@ IF(NOT depthai_FOUND)
   SET(DEPTHAI "//")
 ELSE()
   SET(CONF_DEPTH_AI ON) 
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} depthai::core depthai::opencv) 
 ENDIF()
 IF(NOT octomap_FOUND)
   SET(OCTOMAP "//")
+   SET(CONF_WITH_OCTOMAP 0)
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
+   SET(CONF_WITH_OCTOMAP 1)
 ENDIF()
 IF(NOT CPUTSDF_FOUND)
   SET(CPUTSDF "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) 
 ENDIF()
 IF(NOT open_chisel_FOUND)
   SET(OPENCHISEL "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) 
 ENDIF()
 IF(NOT AliceVision_FOUND)
   SET(ALICE_VISION "//")
 ENDIF(NOT AliceVision_FOUND)
 IF(NOT libfovis_FOUND)
   SET(FOVIS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_LIBRARIES}) 
 ENDIF()
 IF(NOT libviso2_FOUND)
   SET(VISO2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES}) 
 ENDIF()
 IF(NOT dvo_core_FOUND)
   SET(DVO "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) 
 ENDIF()
 IF(NOT okvis_FOUND)
   SET(OKVIS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) 
 ENDIF()
 IF(NOT msckf_vio_FOUND)
   SET(MSCKF_VIO "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${msckf_vio_LIBRARIES}) 
 ENDIF()
 IF(NOT vins_FOUND)
   SET(VINS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
 ENDIF()
 IF(NOT ov_msckf_FOUND)
   SET(OPENVINS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ov_msckf_LIBRARIES}) 
 ENDIF()
 IF(NOT ORB_SLAM_FOUND)
   SET(ORB_SLAM "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM_LIBRARIES})
 ENDIF()
 IF(NOT WITH_ORB_OCTREE)
   SET(ORB_OCTREE "//")
@@ -965,23 +1027,19 @@ ENDIF()
 IF(NOT TORCH_FOUND)
   SET(TORCH "//")
 ENDIF()
-IF(NOT Python3_FOUND)
+IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
   SET(PYTHON "//")
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
   SET(CONF_VTK_QT true)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
 ELSE()
   SET(CONF_VTK_QT false)
 ENDIF()
-IF(VTK_USE_QVTK)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) 
-ENDIF(VTK_USE_QVTK)
 IF(NOT WITH_MADGWICK)
   SET(MADGWICK "//")
 ENDIF()

-CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
+CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/src/include/${PROJECT_PREFIX}/core/Version.h)

 ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
@@ -990,7 +1048,7 @@ IF(ANDROID)
   IF(BUILD_APP)
      ADD_SUBDIRECTORY( app )
   ENDIF(BUILD_APP)
-ELSEIF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
+ELSEIF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
   ADD_SUBDIRECTORY( guilib )
   IF(BUILD_APP)
      ADD_SUBDIRECTORY( app )
@@ -1018,54 +1076,84 @@ ADD_CUSTOM_TARGET(uninstall
  "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")

 ####
-# Setup RTABMapConfig.cmake
+# Global Export Target
 ####
-# Create the RTABMapConfig.cmake and RTABMapConfigVersion files
-file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR}")
-file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
-   
-# ... for the build tree
-set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
-                      "${PROJECT_SOURCE_DIR}/guilib/include" 
-                      "${PROJECT_SOURCE_DIR}/utilite/include")
-set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
-IF(QT4_FOUND OR Qt5_FOUND)
+add_library(rtabmap INTERFACE)
+add_library(rtabmap::rtabmap ALIAS rtabmap)
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
   set(CONF_WITH_GUI ON)
   IF(QT4_FOUND)
      set(CONF_QT_VERSION 4)
-   ELSE()
+   ELSEIF(Qt5_FOUND)
      set(CONF_QT_VERSION 5)
+   ELSE()
+      set(CONF_QT_VERSION 6)
   ENDIF()
+   target_link_libraries(rtabmap INTERFACE rtabmap_utilite rtabmap_core rtabmap_gui)
 ELSE()
   set(CONF_WITH_GUI OFF)
+   target_link_libraries(rtabmap INTERFACE rtabmap_utilite rtabmap_core)
 ENDIF()
-configure_file(RTABMapConfig.cmake.in
-  "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY)
-  
-# ... for the install tree
-set(CONF_INCLUDE_DIRS "\${RTABMap_CMAKE_DIR}/${REL_INCLUDE_DIR}")
-set(CONF_LIB_DIR "\${RTABMap_CMAKE_DIR}/${REL_LIB_DIR}")
-configure_file(RTABMapConfig.cmake.in
-  "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake" @ONLY)
-  
-# ... for both
-configure_file(RTABMapConfigVersion.cmake.in
-  "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake" @ONLY)
+install(TARGETS rtabmap EXPORT rtabmapTargets)
+export(EXPORT rtabmapTargets
+  FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake"
+  NAMESPACE rtabmap::
+)
+install(EXPORT rtabmapTargets
+  FILE
+    ${PROJECT_NAME}Targets.cmake
+  DESTINATION
+    ${INSTALL_CMAKE_DIR}
+  NAMESPACE rtabmap::
+  COMPONENT
+    devel
+)

-# Install the RTABMapConfig.cmake and RTABMapConfigVersion.cmake
-install(FILES
-  "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake"
-  "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake"
-  DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT devel)
+####
+# Setup RTABMapConfig.cmake
+####
+include(CMakePackageConfigHelpers)
+write_basic_package_version_file(
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+  VERSION ${PROJECT_VERSION}
+  COMPATIBILITY AnyNewerVersion
+)
+
+# Build tree:
+SET(CONF_MODULES_DIR "../cmake_modules")
+configure_file(
+  ${PROJECT_NAME}Config.cmake.in
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake"
+  @ONLY
+)
+
+# Install tree:
+SET(CONF_MODULES_DIR "Modules")
+configure_file(
+  ${PROJECT_NAME}Config.cmake.in
+  "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake"
+  @ONLY
+)
+install(
+  FILES
+    "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake"
+    "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+  DESTINATION
+    ${INSTALL_CMAKE_DIR}
+  COMPONENT
+    devel
+)
+  
 ####

 ### Install package.xml for catkin
-install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PREFIX}")
+install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PREFIX}" COMPONENT devel)

 #######################
 # CPACK (Packaging)
 #######################
 IF(BUILD_AS_BUNDLE)
+  SET(CMAKE_INSTALL_SYSTEM_RUNTIME_COMPONENT runtime)
  INCLUDE(InstallRequiredSystemLibraries)
 ENDIF(BUILD_AS_BUNDLE)

@@ -1082,6 +1170,7 @@ SET(CPACK_PACKAGE_VERSION_PATCH "${PROJECT_VERSION_PATCH}")
 #SET(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}")
 SET(CPACK_PACKAGE_CONTACT "matlabbe@gmail.com")

+
 set(CPACK_SOURCE_IGNORE_FILES 
  "\\\\.svn/" 
  "\\\\.settings/" 
@@ -1209,6 +1298,9 @@ MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}
 ELSEIF(Qt5_FOUND)
 MESSAGE(STATUS "  With Qt ${Qt5_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
+ELSEIF(Qt6_FOUND)
+MESSAGE(STATUS "  With Qt ${Qt6_VERSION}            = YES (License: Open Source or Commercial)")
+MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")

 ELSEIF(NOT WITH_QT)
 MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
@@ -1236,7 +1328,7 @@ ELSE()
 MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
 ENDIF()

-IF(Python3_FOUND)
+IF(WITH_PYTHON AND Python3_FOUND)
 MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}            = YES (License: PSF)")
 ELSEIF(NOT WITH_PYTHON)
 MESSAGE(STATUS "  With Python3              = NO (WITH_PYTHON=OFF)")
@@ -1259,7 +1351,7 @@ MESSAGE(STATUS "  With FastCV               = NO (FastCV not found)")
 ENDIF()

 IF(PDAL_FOUND)
-MESSAGE(STATUS "  With PDAL                 = YES (License: BSD)")
+MESSAGE(STATUS "  With PDAL ${PDAL_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_PDAL)
 MESSAGE(STATUS "  With PDAL                 = NO (WITH_PDAL=OFF)")
 ELSE()
@@ -1275,7 +1367,7 @@ MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
 ENDIF()

 IF(G2O_FOUND)
-MESSAGE(STATUS " *With g2o                  = YES (License: BSD)")
+MESSAGE(STATUS " *With g2o ${g2o_VERSION}            = YES (License: BSD)")
 ELSEIF(NOT WITH_G2O)
 MESSAGE(STATUS " *With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
@@ -1283,7 +1375,7 @@ MESSAGE(STATUS " *With g2o                  = NO (g2o not found)")
 ENDIF()

 IF(GTSAM_FOUND)
-MESSAGE(STATUS " *With GTSAM                = YES (License: BSD)")
+MESSAGE(STATUS " *With GTSAM ${GTSAM_VERSION}          = YES (License: BSD)")
 ELSEIF(NOT WITH_GTSAM)
 MESSAGE(STATUS " *With GTSAM                = NO (WITH_GTSAM=OFF)")
 ELSE()
@@ -1291,11 +1383,23 @@ MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()

 IF(CERES_FOUND)
+IF(WITH_CERES)
 MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = YES (License: BSD)")
-ELSEIF(NOT WITH_CERES)
-MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (WITH_CERES=OFF)")
 ELSE()
-MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (Ceres not found)")
+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()
+MESSAGE(STATUS " *With Ceres                = NO (Ceres not found)")
+ENDIF()
+
+IF(MRPT_FOUND)
+MESSAGE(STATUS "  With MRPT ${MRPT_VERSION}           = YES (License: BSD)")
+ELSEIF(NOT WITH_MRPT)
+MESSAGE(STATUS "  With MRPT                 = NO (WITH_MRPT=OFF)")
+ELSE()
+MESSAGE(STATUS "  With MRPT                 = NO (MRPT not found)")
 ENDIF()

 IF(G2O_FOUND OR GTSAM_FOUND)
@@ -1317,7 +1421,7 @@ MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()

 IF(libpointmatcher_FOUND)
-MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
+MESSAGE(STATUS " *With libpointmatcher ${libpointmatcher_VERSION} = YES (License: BSD)")
 ELSEIF(NOT WITH_POINTMATCHER)
 MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
 ELSE()
@@ -1332,10 +1436,28 @@ ELSE()
 MESSAGE(STATUS "  With CCCoreLib            = NO (CCCoreLib not found)")
 ENDIF()

+IF(Open3D_FOUND)
+MESSAGE(STATUS "  With Open3D               = YES (License: MIT)")
+ELSEIF(NOT WITH_OPEN3D)
+MESSAGE(STATUS "  With Open3D               = NO (WITH_OPEN3D=OFF)")
+ELSEIF(${CMAKE_VERSION} VERSION_LESS "3.19.0")
+MESSAGE(STATUS "  With Open3D               = NO (Open3D requires CMake>=3.19)")
+ELSE()
+MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
+ENDIF()
+
+IF(opengv_FOUND)
+MESSAGE(STATUS "  With OpenGV ${opengv_VERSION}           = 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)
-MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+MESSAGE(STATUS "  With OCTOMAP ${octomap_VERSION}        = YES (License: BSD)")
 ELSEIF(NOT WITH_OCTOMAP)
 MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
 ELSE()
@@ -1456,7 +1578,11 @@ MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
 ENDIF()

 IF(realsense2_FOUND)
+IF(WIN32)
 MESSAGE(STATUS "  With RealSense2           = YES (License: Apache-2)")
+ELSE()
+MESSAGE(STATUS "  With RealSense2 ${realsense2_VERSION}    = YES (License: Apache-2)")
+ENDIF()
 ELSEIF(NOT WITH_REALSENSE2)
 MESSAGE(STATUS "  With RealSense2           = NO (WITH_REALSENSE2=OFF)")
 ELSE()
@@ -1472,7 +1598,7 @@ MESSAGE(STATUS "  With MyntEyeS             = NO (mynteye s sdk not found)")
 ENDIF()

 IF(depthai_FOUND)
-MESSAGE(STATUS "  With DepthAI              = YES (License: MIT)")
+MESSAGE(STATUS "  With DepthAI ${depthai_VERSION}       = YES (License: MIT)")
 ELSEIF(NOT WITH_DEPTHAI)
 MESSAGE(STATUS "  With DepthAI              = NO (WITH_DEPTHAI=OFF)")
 ELSE()
@@ -1566,12 +1692,12 @@ ENDIF()
 MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")
 MESSAGE(STATUS "--------------------------------------------")

-IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
+IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT WITH_CERES AND NOT CERES_FOUND)
 MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of these options:
       g2o (https://github.com/RainerKuemmerle/g2o)
       GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
       Ceres (http://ceres-solver.org)
       set -DWITH_TORO=ON")
-ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
+ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT WITH_CERES AND NOT CERES_FOUND)

 # vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :
--- a/README.md
+++ b/README.md
@@ -1,13 +1,12 @@
-rtabmap ![.](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap
 =======

 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)

 [![Release][release-image]][releases]
 [![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/RTABMapConfig.cmake.in
+++ b/RTABMapConfig.cmake.in
@@ -1,99 +1,91 @@
-# - Config file for the RTABMap package
-# Components:
-#  core    (required)
-#  gui     (optional)
-#  utilite (required)
-# It defines the following variables
-#  RTABMap_INCLUDE_DIRS - include directories for RTABMap
-#  RTABMap_LIBRARIES    - libraries to link against
-#  RTABMap_CORE         - core library
-#  RTABMap_UTILITE      - utilite library
-#  RTABMap_GUI          - gui library (set if RTABMap is built with Qt)
+include(CMakeFindDependencyMacro)

-# Compute paths
-get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
-set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
+# Mandatory dependencies
+find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)

-#core lib
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-
-IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
-   SET(RTABMap_CORE
-      debug ${RTABMap_CORE_DEBUG}
-      optimized ${RTABMap_CORE_RELEASE}
-   )
-ELSEIF(RTABMap_CORE_DEBUG)
-   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
+    
+    if(@CONF_QT_VERSION@ EQUAL 6)
+        find_dependency(Qt6 COMPONENTS Widgets Core Gui OpenGL)
+    elseif(@CONF_QT_VERSION@ EQUAL 5)
+        find_dependency(Qt5 COMPONENTS Widgets Core Gui OpenGL)
+    else() # Qt4
+        find_dependency(Qt4 COMPONENTS QtCore QtGui)
+    endif()
+    set(RTABMap_QT_VERSION @CONF_QT_VERSION@)
 ELSE()
-   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation)
+ENDIF()
+set(RTABMap_DEFINITIONS ${PCL_DEFINITIONS})
+add_definitions(${RTABMap_DEFINITIONS}) # To include -march=native if set
+
+# Optional dependencies
+IF(EXISTS "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
+    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
 ENDIF()

-#utilite lib
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-
-IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
-   SET(RTABMap_UTILITE
-      debug ${RTABMap_UTILITE_DEBUG}
-      optimized ${RTABMap_UTILITE_RELEASE}
-   )
-ELSEIF(RTABMap_UTILITE_DEBUG)
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
-ELSE()
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
+IF(@CONF_WITH_REALSENSE2@)
+    IF(WIN32)
+        find_dependency(RealSense2)
+    ELSE()
+        find_dependency(realsense2)
+    ENDIF()
 ENDIF()

-set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
+IF(@CONF_WITH_K4A@)
+    IF(WIN32)
+        find_dependency(K4A)
+    ELSE()
+        find_dependency(k4a)
+        find_dependency(k4arecord)
+    ENDIF()
+ENDIF()

-list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
-set(WITH_GUI ON)
-if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
-   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
-   if (${_index} EQUAL -1)
-      set(WITH_GUI OFF)
-   endif()
-endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+IF(@CONF_WITH_OCTOMAP@)
+  find_dependency(octomap)
+ENDIF()

+# Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies)
+set(RTABMap_INCLUDE_DIRS "")
+set(RTABMap_LIBRARIES "")
+set(RTABMap_TARGETS "")

-#gui lib (OFF if RTAB-Map is not built with Qt)
-if(@CONF_WITH_GUI@ AND ${WITH_GUI})
-   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-   
-   IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
-      SET(RTABMap_GUI
-         debug ${RTABMap_GUI_DEBUG}
-         optimized ${RTABMap_GUI_RELEASE}
-      )
-   ELSEIF(RTABMap_GUI_RELEASE)
-      SET(RTABMap_GUI ${RTABMap_GUI_RELEASE})
-   ELSEIF(RTABMap_GUI_DEBUG)
-      SET(RTABMap_GUI ${RTABMap_GUI_DEBUG})
-   ENDIF()
-   
-   set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
-elseif(${WITH_GUI})
-   MESSAGE(WARNING "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
-endif()
+set(_RTABMap_supported_components utilite core gui)

-# Dependencies
-if(@CONF_VTK_QT@ AND ${WITH_GUI})
-   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
-endif(@CONF_VTK_QT@ AND ${WITH_GUI})
-if(@CONF_DEPTH_AI@)
-   FIND_PACKAGE(depthai 2 QUIET REQUIRED)
-endif(@CONF_DEPTH_AI@)
-SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
+foreach(_comp ${_RTABMap_supported_components})
+  if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    include("${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    set(RTABMap_${_comp}_FOUND True)
+    set(RTABMap_TARGETS
+      ${RTABMap_TARGETS}
+      rtabmap::${_comp})
+    get_target_property(RTABMap_${_comp}_INCLUDE_DIRS rtabmap::${_comp} INTERFACE_INCLUDE_DIRECTORIES)
+    get_target_property(RTABMap_${_comp}_LIBRARIES rtabmap::${_comp} INTERFACE_LINK_LIBRARIES)
+    set(RTABMap_INCLUDE_DIRS 
+      ${RTABMap_INCLUDE_DIRS}
+      ${RTABMap_${_comp}_INCLUDE_DIRS})
+    set(RTABMap_LIBRARIES 
+      ${RTABMap_LIBRARIES}
+      rtabmap::${_comp})
+    if(RTABMap_${_comp}_LIBRARIES)
+      set(RTABMap_LIBRARIES 
+        ${RTABMap_LIBRARIES}
+        ${RTABMap_${_comp}_LIBRARIES})
+    endif()
+  else()
+    set(RTABMap_${_comp}_FOUND False)
+  endif()
+endforeach()

-#backward compatibilities
-set(RTABMAP_CORE ${RTABMap_CORE})
-set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-if(RTABMap_GUI)
-   set(RTABMAP_GUI ${RTABMap_GUI})
-   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
-endif(RTABMap_GUI)
+include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")

-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
-mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)
+foreach(_comp ${RTABMap_FIND_COMPONENTS})
+  if (NOT ";${_RTABMap_supported_components};" MATCHES ";${_comp};")
+    set(RTABMap_${_comp}_FOUND False)
+    if(${RTABMap_FIND_REQUIRED_${_comp}})
+      set(RTABMap_FOUND False)
+      set(RTABMap_NOT_FOUND_MESSAGE "Unsupported or not found required component: ${_comp}")
+    endif()
+  endif()
+endforeach()
--- a/RTABMapConfigVersion.cmake.in
+++ b/RTABMapConfigVersion.cmake.in
@@ -1,11 +0,0 @@
-set(PACKAGE_VERSION "@RTABMAP_VERSION@")
-
-# Check whether the requested PACKAGE_FIND_VERSION is compatible
-if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
-  set(PACKAGE_VERSION_COMPATIBLE FALSE)
-else()
-  set(PACKAGE_VERSION_COMPATIBLE TRUE)
-  if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
-    set(PACKAGE_VERSION_EXACT TRUE)
-  endif()
-endif()
--- a/Version.h.in
+++ b/Version.h.in
@@ -40,9 +40,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@NONFREE@#define RTABMAP_NONFREE
@TORO@#define RTABMAP_TORO
@G2O@#define RTABMAP_G2O
-@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
@GTSAM@#define RTABMAP_GTSAM
@CERES@#define RTABMAP_CERES
+@MRPT@#define RTABMAP_MRPT
@VERTIGO@#define RTABMAP_VERTIGO
@OPENNI2@#define RTABMAP_OPENNI2
@FREENECT@#define RTABMAP_FREENECT
@@ -52,7 +53,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@CVSBA@#define RTABMAP_CVSBA
@POINTMATCHER@#define RTABMAP_POINTMATCHER
@CCCORELIB@#define RTABMAP_CCCORELIB
+@OPEN3D@#define RTABMAP_OPEN3D
@FASTCV@#define RTABMAP_FASTCV
+@OPENGV@#define RTABMAP_OPENGV
@PDAL@#define RTABMAP_PDAL
@LOAM@#define RTABMAP_LOAM
@FLOAM@#define RTABMAP_FLOAM
--- 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/CMakeLists.txt
+++ b/app/android/jni/CMakeLists.txt
@@ -3,6 +3,7 @@ SET(INCLUDE_DIRS
    ${CMAKE_CURRENT_SOURCE_DIR}
    ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
    ${CMAKE_CURRENT_SOURCE_DIR}/third-party/include
+	${PROJECT_BINARY_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/utilite/include
    ${OpenCV_INCLUDE_DIRS}
--- 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);
@@ -432,23 +432,25 @@ LaserScan CameraMobile::scanFromPointCloudData(
            //get color from rgb image
            cv::Point3f org= pt;
            pt = util3d::transformPoint(pt, opticalRotationInv);
-            int u,v;
-            model.reproject(pt.x, pt.y, pt.z, u, v);
-            unsigned char r=255,g=255,b=255;
-            if(model.inFrame(u, v))
+            if(pt.z > 0)
            {
-                b=rgb.at<cv::Vec3b>(v,u).val[0];
-                g=rgb.at<cv::Vec3b>(v,u).val[1];
-                r=rgb.at<cv::Vec3b>(v,u).val[2];
-                if(kpts)
-                    kpts->push_back(cv::KeyPoint(u,v,kptsSize));
-                if(kpts3D)
-                    kpts3D->push_back(org);
-                
-                *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
-                ++oi;
+                int u,v;
+                model.reproject(pt.x, pt.y, pt.z, u, v);
+                unsigned char r=255,g=255,b=255;
+                if(model.inFrame(u, v))
+                {
+                    b=rgb.at<cv::Vec3b>(v,u).val[0];
+                    g=rgb.at<cv::Vec3b>(v,u).val[1];
+                    r=rgb.at<cv::Vec3b>(v,u).val[2];
+                    if(kpts)
+                        kpts->push_back(cv::KeyPoint(u,v,kptsSize));
+                    if(kpts3D)
+                        kpts3D->push_back(org);
+                    
+                    *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+                    ++oi;
+                }
            }
-
            //confidence
            //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);

--- 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();
@@ -448,7 +490,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			poses,
 			links,
 			true,
-			true,
+			false, // Make sure poses are the same than optimized mesh (in case we switched RGBD/OptimizedFromGraphEnd)
 			&signatures,
 			true,
 			true,
@@ -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
@@ -1399,7 +1446,14 @@ int RTABMapApp::Render()
                    }
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && landmark!=0)
 					{
-						main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        if(rejected)
+                        {
+                            main_scene_.setBackgroundColor(0.5, 0.325f, 0); // dark orange
+                        }
+                        else
+                        {
+                            main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        }
 					}
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && rejected>0)
 					{
@@ -1439,12 +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
@@ -1571,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
 									{
@@ -1856,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
@@ -2046,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())
 			{
@@ -2076,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)));
 				}
 			}
 		}
@@ -2089,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)
 			{
@@ -2108,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_;
@@ -2550,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!");
@@ -3704,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);

@@ -594,6 +590,10 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+        else
+        {
+            trace_->ClearVertexArray();
+        }
 	}

 	if(gridVisible_ && !renderBackgroundCamera)
@@ -629,14 +629,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 				cloud->getPose().z() - openglCamera.z());
 		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];

-		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTextures_[0]:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}

 	if(onlineBlending)
 	{
-        if(renderBackgroundCamera)
+        if(renderBackgroundCamera && meshRendering_)
        {
-            background_renderer_->Draw(uvsTransformed, depthTextures_[1], screenWidth_, screenHeight_, meshRendering_?mapping:false);
+            background_renderer_->Draw(uvsTransformed, depthTexture_, screenWidth_, screenHeight_, mapping);
        }
        
 		glDisable (GL_BLEND);
--- 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 = 7;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				EXCLUDED_ARCHS = "";
@@ -1006,7 +1007,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.21.0;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1019,7 +1020,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1038,7 +1039,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 7;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				FRAMEWORK_SEARCH_PATHS = (
@@ -1063,7 +1064,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.21.0;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -1077,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
--- 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)
@@ -477,13 +489,6 @@ void setGridVisibleNative(const void *object, bool visible)
    else
        UERROR("object is null!");
 }
-void setRawScanSavedNative(const void *object, bool enabled)
-{
-    if(object)
-        native(object)->setRawScanSaved(enabled);
-    else
-        UERROR("object is null!");
-}
 void setFullResolutionNative(const void *object, bool enabled)
 {
    if(object)
--- 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,
@@ -96,7 +97,6 @@ void setGraphOptimizationNative(const void *object, bool enabled);
 void setNodesFilteringNative(const void *object, bool enabled);
 void setGraphVisibleNative(const void *object, bool visible);
 void setGridVisibleNative(const void *object, bool visible);
-void setRawScanSavedNative(const void *object, bool enabled);
 void setFullResolutionNative(const void *object, bool enabled);
 void setSmoothingNative(const void *object, bool enabled);
 void setAppendModeNative(const void *object, bool enabled);
--- 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)))
    }
@@ -401,9 +405,6 @@ class RTABMap {
    func setGridVisible(visible: Bool) {
        setGridVisibleNative(native_rtabmap, visible)
    }
-    func setRawScanSaved(enabled: Bool) {
-        setRawScanSavedNative(native_rtabmap, enabled)
-    }
    func setFullResolution(enabled: Bool) {
        setFullResolutionNative(native_rtabmap, enabled)
    }
@@ -486,7 +487,7 @@ func getPreviewImage(databasePath: String) -> UIImage?
            let bitmap = CIImage(bitmapData: data, bytesPerRow: Int(image.width*image.channels), size: CGSize(width: Int(image.width), height: Int(image.height)), format: CIFormat.BGRA8, colorSpace: nil)
            return UIImage(ciImage: bitmap)
        }
-        return nil
+        return UIImage(named: "RTAB-Map1024")
    }
    return imageOut
 }
--- 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 {
@@ -1315,7 +1319,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        // update preference
        rtabmap!.setOnlineBlending(enabled: defaults.bool(forKey: "Blending"));
        rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering"));
-        rtabmap!.setRawScanSaved(enabled: defaults.bool(forKey: "SaveRawScan"));
        rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode"));
        rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing"));
        rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode"));
@@ -1340,6 +1343,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        rtabmap!.setMappingParameter(key: "Vis/FeatureType", value: defaults.string(forKey: "FeatureType")!);
        rtabmap!.setMappingParameter(key: "Mem/NotLinkedNodesKept", value: defaults.bool(forKey: "SaveAllFramesInDatabase") ? "true" : "false");
        rtabmap!.setMappingParameter(key: "RGBD/OptimizeFromGraphEnd", value: defaults.bool(forKey: "OptimizationfromGraphEnd") ? "true" : "false");
+        rtabmap!.setMappingParameter(key: "RGBD/MaxOdomCacheSize", value: defaults.string(forKey: "MaximumOdometryCacheSize")!);
        rtabmap!.setMappingParameter(key: "Optimizer/Strategy", value: defaults.string(forKey: "GraphOptimizer")!);
        rtabmap!.setMappingParameter(key: "RGBD/ProximityBySpace", value: defaults.string(forKey: "ProximityDetection")!);

@@ -1413,12 +1417,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        
        present(alertController, animated: true)
        
-        setGLCamera(type: 1);
+        setGLCamera(type: 0);
        startCamera();
    }
    
    func newScan()
    {
+        print("databases.size() = \(databases.size())")
+        if(databases.count >= 5 && !mReviewRequested && self.depthSupported)
+        {
+            SKStoreReviewController.requestReviewInCurrentScene()
+            mReviewRequested = true
+        }
+        
        if(mState == State.STATE_VISUALIZING)
        {
            closeVisualization()
@@ -1426,15 +1437,121 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        
        mMapNodes = 0;
        self.openedDatabasePath = nil
-        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
-        
+        let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
        let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory")
-        self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false)
-
-        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) &&
+           FileManager.default.fileExists(atPath: tmpDatabase.path) &&
+           tmpDatabase.fileSize > 1024*1024) // > 1MB
        {
-            self.setGLCamera(type: 1);
-            self.startCamera();
+            dismiss(animated: true, completion: {
+                let msg = "The previous session (\(tmpDatabase.fileSizeString)) was not correctly saved, do you want to recover it?"
+                let alert = UIAlertController(title: "Recovery", message: msg, preferredStyle: .alert)
+                let alertActionNo = UIAlertAction(title: "Ignore", style: .destructive) {
+                    (UIAlertAction) -> Void in
+                    do {
+                        try FileManager.default.removeItem(at: tmpDatabase)
+                    }
+                    catch {
+                        print("Could not clear tmp database: \(error)")
+                    }
+                    self.newScan()
+                }
+                alert.addAction(alertActionNo)
+                let alertActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                    (UIAlertAction) -> Void in
+                    // do nothing
+                }
+                alert.addAction(alertActionCancel)
+                let alertActionYes = UIAlertAction(title: "Yes", style: .default) {
+                    (UIAlertAction2) -> Void in
+
+                    let fileName = Date().getFormattedDate(format: "yyMMdd-HHmmss") + ".db"
+                    let outputDbPath = self.getDocumentDirectory().appendingPathComponent(fileName).path
+                    
+                    var indicator: UIActivityIndicatorView?
+                    
+                    let alertView = UIAlertController(title: "Recovering", message: "Please wait while recovering data...", preferredStyle: .alert)
+                    let alertViewActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                        (UIAlertAction) -> Void in
+                        self.dismiss(animated: true, completion: {
+                            self.progressView = nil
+                            
+                            indicator = UIActivityIndicatorView(style: .large)
+                            indicator?.frame = CGRect(x: 0.0, y: 0.0, width: 60.0, height: 60.0)
+                            indicator?.center = self.view.center
+                            self.view.addSubview(indicator!)
+                            indicator?.bringSubviewToFront(self.view)
+                            
+                            indicator?.startAnimating()
+                            self.rtabmap!.cancelProcessing();
+                        })
+                    }
+                    alertView.addAction(alertViewActionCancel)
+                    
+                    let previousState = self.mState
+                    self.updateState(state: .STATE_PROCESSING);
+                    
+                    self.present(alertView, animated: true, completion: {
+                        //  Add your progressbar after alert is shown (and measured)
+                        let margin:CGFloat = 8.0
+                        let rect = CGRect(x: margin, y: 84.0, width: alertView.view.frame.width - margin * 2.0 , height: 2.0)
+                        self.progressView = UIProgressView(frame: rect)
+                        self.progressView!.progress = 0
+                        self.progressView!.tintColor = self.view.tintColor
+                        alertView.view.addSubview(self.progressView!)
+                        
+                        var success : Bool = false
+                        DispatchQueue.background(background: {
+                            
+                            success = self.rtabmap!.recover(from: tmpDatabase.path, to: outputDbPath)
+                            
+                        }, completion:{
+                            if(indicator != nil)
+                            {
+                                indicator!.stopAnimating()
+                                indicator!.removeFromSuperview()
+                            }
+                            if self.progressView != nil
+                            {
+                                self.dismiss(animated: self.openedDatabasePath == nil, completion: {
+                                    if(success)
+                                    {
+                                        let alertSaved = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully recovered!", fileName), preferredStyle: .alert)
+                                        let yes = UIAlertAction(title: "OK", style: .default) {
+                                            (UIAlertAction) -> Void in
+                                            self.openDatabase(fileUrl: URL(fileURLWithPath: outputDbPath))
+                                        }
+                                        alertSaved.addAction(yes)
+                                        self.present(alertSaved, animated: true, completion: nil)
+                                    }
+                                    else
+                                    {
+                                        self.updateState(state: previousState);
+                                        self.showToast(message: "Recovery failed!", seconds: 4)
+                                    }
+                                })
+                            }
+                            else
+                            {
+                                self.showToast(message: "Recovery canceled", seconds: 2)
+                                self.updateState(state: previousState);
+                            }
+                        })
+                    })
+                }
+                alert.addAction(alertActionYes)
+                self.present(alert, animated: true, completion: nil)
+            })
+        }
+        else
+        {
+            self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false, clearDatabase: true)
+
+            if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+            {
+                self.setGLCamera(type: 0);
+                self.startCamera();
+            }
        }
    }
    
@@ -1518,12 +1635,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            
            self.openedDatabasePath = URL(fileURLWithPath: filePath)
            
-            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved on the SD-CARD!", fileName), preferredStyle: .alert)
+            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved!", fileName), preferredStyle: .alert)
            let yes = UIAlertAction(title: "OK", style: .default) {
                (UIAlertAction) -> Void in
            }
            alert.addAction(yes)
            self.present(alert, animated: true, completion: nil)
+            do {
+                let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
+                try FileManager.default.removeItem(at: tmpDatabase)
+            }
+            catch {
+                print("Could not clear tmp database: \(error)")
+            }
            self.updateDatabases()
            self.updateState(state: previousState)
        })
@@ -1559,6 +1683,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                indicator?.bringSubviewToFront(self.view)
                
                indicator?.startAnimating()
+                
+                self.rtabmap!.cancelProcessing()
            })
            
        }))
@@ -1663,6 +1789,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
            self.dismiss(animated: true)
            self.progressView = nil
+            self.rtabmap!.cancelProcessing()
        }))

        let previousState = mState
@@ -1799,7 +1926,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            stopMapping(ignoreSaving: true)
        }
        
-        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
        openedDatabasePath = fileUrl;
        let fileName: String = self.openedDatabasePath!.lastPathComponent
        
@@ -1811,7 +1937,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        updateState(state: .STATE_PROCESSING);
        var status = 0
        DispatchQueue.background(background: {
-            status = self.rtabmap!.openDatabase(databaseSource: self.openedDatabasePath!.path, databasePath: tmpDatabase.path, databaseInMemory: true, optimize: false)
+            status = self.rtabmap!.openDatabase(databasePath: self.openedDatabasePath!.path, databaseInMemory: true, optimize: false, clearDatabase: false)
        }, completion:{
            // main thread
            if(status == -1) {
@@ -1973,6 +2099,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
            self.dismiss(animated: true)
            self.progressView = nil
+            self.rtabmap!.cancelProcessing()
        }))
        
        let previousState = mState;
@@ -2072,7 +2199,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                    }
                    .sorted(by: { $0.1 > $1.1 }) // sort descending modification dates
                    .map { $0.0 } // extract file names
-            databases = data.filter{ $0.pathExtension == "db" }
+            databases = data.filter{ $0.pathExtension == "db" && $0.lastPathComponent != RTABMAP_TMP_DB && $0.lastPathComponent != RTABMAP_RECOVERY_DB }
            
        } catch {
            print("Error while enumerating files : \(error.localizedDescription)")
@@ -2210,6 +2337,14 @@ extension Date {
        dateformat.dateFormat = format
        return dateformat.string(from: self)
    }
+    
+    var millisecondsSince1970:Int64 {
+        Int64((self.timeIntervalSince1970 * 1000.0).rounded())
+    }
+    
+    init(milliseconds:Int64) {
+        self = Date(timeIntervalSince1970: TimeInterval(milliseconds) / 1000)
+    }
 }

 extension DispatchQueue {
@@ -2322,3 +2457,11 @@ extension UserDefaults {
        setDefaultsFromSettingsBundle()
    }
 }
+
+extension SKStoreReviewController {
+    public static func requestReviewInCurrentScene() {
+        if let scene = UIApplication.shared.connectedScenes.first(where: { $0.activationState == .foregroundActive }) as? UIWindowScene {
+            requestReview(in: scene)
+        }
+    }
+}
--- 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
@@ -55,9 +59,11 @@ cd $pwd

 # FLANN
 echo "wget flann..."
-curl -L http://www.cs.ubc.ca/research/flann/uploads/FLANN/flann-1.8.4-src.zip -o flann-1.8.4-src.zip
-unzip -qq flann-1.8.4-src.zip
-cd flann-1.8.4-src
+git clone https://github.com/flann-lib/flann.git
+cd flann
+git checkout 1.8.4
+curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
+git apply flann_ios.patch
 mkdir build
 cd build
 # comment "add_subdirectory( test )" in top CMakeLists.txt
@@ -66,7 +72,7 @@ cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_O
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
-#rm -r flann-1.8.4-src.zip flann-1.8.4-src
+#rm -r flann

 # GTSAM
 git clone https://bitbucket.org/gtborg/gtsam.git
@@ -105,7 +111,7 @@ git checkout tags/v8.2.0
 git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
 mkdir build
 cd build
-cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DIOS_SIMULATOR_ARCHITECTURES=arm64 -DModule_vtkFiltersModeling=ON ..
+cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
 # For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES=""
 cmake --build . --config Release
 cd $pwd
@@ -134,6 +140,8 @@ cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.4.2
+curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
+git apply opencv_ios.patch
 mkdir build
 cd build
 # add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
@@ -145,6 +153,10 @@ cd $pwd

 mkdir rtabmap
 cd rtabmap
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../..
+cmake -DANDROID_PREBUILD=ON ../../../../..
+cmake --build . --config Release
+mkdir ios
+cd ios
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install
--- 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-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
 Copyright (c) XXX, contributors, all rights reserved.

 Redistribution and use in source and binary forms, with or without
--- a/app/ios/Settings.bundle/Mapping.plist
+++ b/app/ios/Settings.bundle/Mapping.plist
@@ -552,6 +552,50 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don&apos;t teleport to a location very similar to one we previously localized on.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Maximum odometry cache size</string>
+			<key>Key</key>
+			<string>MaximumOdometryCacheSize</string>
+			<key>DefaultValue</key>
+			<string>10</string>
+			<key>Titles</key>
+			<array>
+				<string>500</string>
+				<string>200</string>
+				<string>100</string>
+				<string>75</string>
+				<string>50</string>
+				<string>40</string>
+				<string>30</string>
+				<string>20</string>
+				<string>10</string>
+				<string>5</string>
+				<string>Disabled</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>500</string>
+				<string>200</string>
+				<string>100</string>
+				<string>75</string>
+				<string>50</string>
+				<string>40</string>
+				<string>30</string>
+				<string>20</string>
+				<string>10</string>
+				<string>5</string>
+				<string>0</string>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -708,22 +752,6 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
-		<dict>
-			<key>Type</key>
-			<string>PSGroupSpecifier</string>
-			<key>FooterText</key>
-			<string>Save raw point clouds in database.</string>
-		</dict>
-		<dict>
-			<key>Type</key>
-			<string>PSToggleSwitchSpecifier</string>
-			<key>Title</key>
-			<string>Save Raw Scan</string>
-			<key>Key</key>
-			<string>SaveRawScan</string>
-			<key>DefaultValue</key>
-			<true/>
-		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
--- 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-2023, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
 			<key>Title</key>
 			<string>About</string>
 			<key>Type</key>
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.12</string>
+			<string>0.21.0</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>
--- a/app/src/CMakeLists.txt
+++ b/app/src/CMakeLists.txt
@@ -3,31 +3,6 @@ SET(SRC_FILES
    main.cpp
 )

-SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/utilite/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/guilib/include
-    ${CMAKE_CURRENT_SOURCE_DIR}
-    ${OpenCV_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-)
-
-IF(QT4_FOUND)
-    INCLUDE(${QT_USE_FILE})
-ENDIF(QT4_FOUND)
-
-SET(LIBRARIES
-    ${QT_LIBRARIES} 
-    ${OpenCV_LIBS}
-    ${PCL_LIBRARIES}
-)
-
-# rc.exe has problems with these defintions... commented!
-#add_definitions(${PCL_DEFINITIONS})
-
-# Make sure the compiler can find include files from our library.
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
 # For Apple set the icns file containing icons
 IF(APPLE AND BUILD_AS_BUNDLE)
  # set how it shows up in the Info.plist file
@@ -49,40 +24,37 @@ IF(WIN32)
  ELSE( MINGW )
    SET(SRC_FILES ${SRC_FILES} ${PROJECT_NAME}.rc)
  ENDIF( MINGW )
-ENDIF(WIN32)
+ENDIF(WIN32)
+
+IF(BUILD_AS_BUNDLE)
+  ADD_DEFINITIONS("-DBUILD_AS_BUNDLE")
+ENDIF()

 # Add binary
 IF(APPLE AND BUILD_AS_BUNDLE)
-  ADD_EXECUTABLE(rtabmap MACOSX_BUNDLE ${SRC_FILES})
-ELSEIF(MINGW)
-  ADD_EXECUTABLE(rtabmap WIN32 ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES})
+ELSEIF(WIN32 AND BUILD_AS_BUNDLE)
+  ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES})
 ELSE()
-  ADD_EXECUTABLE(rtabmap ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
 ENDIF()
-TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
-IF(Qt5_FOUND)
-    IF(Qt5Svg_FOUND)
-        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
-    ELSE()
-        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
-    ENDIF()
-ENDIF(Qt5_FOUND)
+TARGET_LINK_LIBRARIES(rtabmap_app rtabmap_gui)

 IF(APPLE AND BUILD_AS_BUNDLE)
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
    OUTPUT_NAME ${CMAKE_BUNDLE_NAME})
 ELSEIF(WIN32)
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
    OUTPUT_NAME ${PROJECT_NAME})
 ELSE()
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
    OUTPUT_NAME ${PROJECT_PREFIX})
 ENDIF()

 #---------------------------
 # Installation stuff
 #--------------------------- 
-INSTALL(TARGETS rtabmap
+INSTALL(TARGETS rtabmap_app
        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
        BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
      
@@ -134,17 +106,51 @@ 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'
-  # Exclude debug libraries
-  IF(QT_PLUGINS_DIR)
-    INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
-          DESTINATION ${plugin_dest_dir}/plugins 
-          COMPONENT runtime 
-          REGEX ".*d4.dll" EXCLUDE
-          REGEX ".*d4.a" EXCLUDE)
-  ELSE()
+  IF(Qt6_FOUND)
+	# Reference: https://doc-snapshots.qt.io/qt6-6.4/qt-deploy-runtime-dependencies.html
+	# The following script must only be executed at install time
+	set(deploy_script "${CMAKE_CURRENT_BINARY_DIR}/deploy_app$<CONFIG>.cmake")
+	file(GENERATE OUTPUT ${deploy_script} CONTENT "
+	# Including the file pointed to by QT_DEPLOY_SUPPORT ensures the generated
+	# deployment script has access to qt_deploy_runtime_dependencies()
+	include(\"${QT_DEPLOY_SUPPORT}\")
+	qt_deploy_runtime_dependencies(
+		EXECUTABLE \"${APPS}\"
+		VERBOSE
+		PLUGINS_DIR ${plugin_dest_dir}/plugins
+	)")
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appDebug.cmake"
+		CONFIGURATIONS Debug
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake"
+		CONFIGURATIONS Release
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake"
+		CONFIGURATIONS RelWithDebInfo
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake"
+		CONFIGURATIONS MinSizeRel
+		COMPONENT runtime)
+  ELSEIF(Qt5_FOUND)
 	#Qt5
 	foreach(plugin ${Qt5Gui_PLUGINS})
 		get_target_property(plugin_loc ${plugin} LOCATION)
@@ -153,17 +159,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()
+		  DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+		  COMPONENT runtime)
+	endforeach()
 	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
 		IF(WIN32)
-			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
-		ELSEIF(APPLE)
-			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+			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)
@@ -175,22 +181,31 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 			#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		ENDIF(EXISTS ${plugin_loc})
 	ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+  ELSEIF(QT_PLUGINS_DIR) # Qt4
+    # Install needed Qt plugins by copying directories from the qt installation
+	# One can cull what gets copied by using 'REGEX "..." EXCLUDE'
+	# Exclude debug libraries
+	INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
+		DESTINATION ${plugin_dest_dir}/plugins 
+		COMPONENT runtime 
+		REGEX ".*d4.dll" EXCLUDE
+		REGEX ".*d4.a" EXCLUDE)
  ENDIF()
-  
+ 
  # install a qt.conf file
  # this inserts some cmake code into the install script to write the file
  SET(QT_CONF_FILE [Paths]\nPlugins=plugins)
  IF(APPLE)
-    SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
+	SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
  ENDIF(APPLE)
  INSTALL(CODE "
-    file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
-    " COMPONENT runtime)
-
+	file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
+	" COMPONENT runtime)
+ 
  # directories to look for dependencies
-  SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_SOURCE_DIR}/bin)
+  SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_BINARY_DIR}/bin)
  IF(APPLE)
-    SET(DIRS ${DIRS} /usr/local /usr/local/lib)
+    SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current)
  ENDIF(APPLE)

  # Now the work of copying dependencies into the bundle/package
--- a/app/src/main.cpp
+++ b/app/src/main.cpp
@@ -36,10 +36,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UConversion.h"

+#include <vtkVersionMacros.h>
 #include <vtkObject.h>

-#ifdef RTABMAP_PYTHON
-#include "rtabmap/core/PythonInterface.h"
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+#include <QVTKRenderWidget.h>
 #endif

 using namespace rtabmap;
@@ -54,13 +55,13 @@ 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
+#if VTK_MAJOR_VERSION >= 8 && defined(BUILD_AS_BUNDLE)
 	vtkObject::GlobalWarningDisplayOff();
 #endif
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+    // needed to ensure appropriate OpenGL context is created for VTK rendering.
+    QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());
+#endif

 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
--- 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/bin/.gitignore
+++ b/bin/.gitignore
@@ -1,5 +0,0 @@
-# Ignore everything in this directory
-*
-# Except this file
-!.gitignore
-!data
--- a/build/.gitignore
+++ b/build/.gitignore
@@ -1,4 +1,4 @@
 # Ignore everything in this directory
 *
 # Except this file
-!.gitignore
+!.gitignore
--- a/cmake_modules/FindEigen3.cmake
+++ b/cmake_modules/FindEigen3.cmake
@@ -0,0 +1,107 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+#
+# and the following imported target:
+#
+#  Eigen3::Eigen - The header-only Eigen library
+#
+# This module reads hints about search locations from 
+# the following environment variables:
+#
+# EIGEN3_ROOT
+# EIGEN3_ROOT_DIR
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif()
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif()
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif()
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif()
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else()
+    set(EIGEN3_VERSION_OK TRUE)
+  endif()
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif()
+endmacro()
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+  set(Eigen3_FOUND ${EIGEN3_VERSION_OK})
+
+else ()
+  
+  # search first if an Eigen3Config.cmake is available in the system,
+  # if successful this would set EIGEN3_INCLUDE_DIR and the rest of
+  # the script will work as usual
+  find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)
+
+  if(NOT EIGEN3_INCLUDE_DIR)
+    find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+        HINTS
+        ENV EIGEN3_ROOT 
+        ENV EIGEN3_ROOT_DIR
+        PATHS
+        ${CMAKE_INSTALL_PREFIX}/include
+        ${KDE4_INCLUDE_DIR}
+        PATH_SUFFIXES eigen3 eigen
+      )
+  endif()
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif()
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif()
+
+if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen)
+  add_library(Eigen3::Eigen INTERFACE IMPORTED)
+  set_target_properties(Eigen3::Eigen PROPERTIES
+    INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}")
+endif()
--- 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/cmake_modules/FindZEDOC.cmake
+++ b/cmake_modules/FindZEDOC.cmake
@@ -17,7 +17,7 @@ ENDIF (ZEDOC_INCLUDE_DIR AND ZEDOC_LIBRARY)

 IF (ZEDOC_FOUND)
   # show which ZEDOC was found only if not quiet
-   IF (NOT _FIND_QUIETLY)
+   IF (NOT ZEDOC_FIND_QUIETLY)
      MESSAGE(STATUS "Found ZEDOC: ${ZEDOC_LIBRARIES}")
   ENDIF (NOT ZEDOC_FIND_QUIETLY)
 ELSE (ZEDOC_FOUND)
--- a/corelib/include/rtabmap/core/.gitignore
+++ b/corelib/include/rtabmap/core/.gitignore
@@ -1 +0,0 @@
-/Version.h
--- a/corelib/include/rtabmap/core/BayesFilter.h
+++ b/corelib/include/rtabmap/core/BayesFilter.h
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef BAYESFILTER_H_
 #define BAYESFILTER_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <opencv2/core/core.hpp>
 #include <list>
@@ -41,7 +41,7 @@ namespace rtabmap {
 class Memory;
 class Signature;

-class RTABMAP_EXP BayesFilter
+class RTABMAP_CORE_EXPORT BayesFilter
 {
 public:
 	BayesFilter(const ParametersMap & parameters = ParametersMap());
--- a/corelib/include/rtabmap/core/Camera.h
+++ b/corelib/include/rtabmap/core/Camera.h
@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #pragma once

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <opencv2/highgui/highgui.hpp>
 #include "rtabmap/core/SensorData.h"
@@ -47,7 +47,7 @@ namespace rtabmap
 * Class Camera
 *
 */
-class RTABMAP_EXP Camera
+class RTABMAP_CORE_EXPORT Camera
 {
 public:
 	virtual ~Camera();
--- a/corelib/include/rtabmap/core/CameraModel.h
+++ b/corelib/include/rtabmap/core/CameraModel.h
@@ -30,12 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <opencv2/opencv.hpp>

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/Transform.h"

 namespace rtabmap {

-class RTABMAP_EXP CameraModel
+class RTABMAP_CORE_EXPORT CameraModel
 {
 public:
 	/**
@@ -159,7 +159,7 @@ private:
 	Transform localTransform_;
 };

-RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const CameraModel& model);
+RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const CameraModel& model);

 } /* namespace rtabmap */
 #endif /* CAMERAMODEL_H_ */
--- a/corelib/include/rtabmap/core/CameraThread.h
+++ b/corelib/include/rtabmap/core/CameraThread.h
@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #pragma once

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Transform.h>
@@ -52,7 +52,7 @@ class IMUFilter;
 * Class CameraThread
 *
 */
-class RTABMAP_EXP CameraThread :
+class RTABMAP_CORE_EXPORT CameraThread :
 	public UThread,
 	public UEventsSender
 {
@@ -88,7 +88,8 @@ public:
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
 	void disableIMUFiltering();

-	RTABMAP_DEPRECATED(void setScanParameters(
+	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
+	RTABMAP_DEPRECATED void setScanParameters(
 			bool fromDepth,
 			int downsampleStep, // decimation of the depth image in case the scan is from depth image
 			float rangeMin,
@@ -96,7 +97,7 @@ public:
 			float voxelSize,
 			int normalsK,
 			int normalsRadius,
-			bool forceGroundNormalsUp) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.");
+			bool forceGroundNormalsUp);
 	void setScanParameters(
 			bool fromDepth,
 			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
--- a/corelib/include/rtabmap/core/Compression.h
+++ b/corelib/include/rtabmap/core/Compression.h
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef COMPRESSION_H_
 #define COMPRESSION_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <rtabmap/utilite/UThread.h>
 #include <opencv2/opencv.hpp>
@@ -52,7 +52,7 @@ namespace rtabmap {
 *   ct.join();
 *   cv::Mat image = ct.getUncompressedData();
 */
-class RTABMAP_EXP CompressionThread : public UThread
+class RTABMAP_CORE_EXPORT CompressionThread : public UThread
 {
 public:
 	// format : ".png" ".jpg" "" (empty is general)
@@ -70,21 +70,21 @@ private:
 	bool compressMode_;
 };

-std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
-cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
+std::vector<unsigned char> RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png");
+cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");

-cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);

-std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
-cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
+std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(const cv::Mat & data);
+cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat & data);

-cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned long size);

-cv::Mat RTABMAP_EXP compressString(const std::string & str);
-std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str);
+std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);

 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef DBDRIVER_H_
 #define DBDRIVER_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <string>
 #include <list>
@@ -59,7 +59,7 @@ class VisualWord;
 //but never, never try to use the same connection simultaneously in
 //two or more threads."
 //
-class RTABMAP_EXP DBDriver : public UThreadNode
+class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
 {
 public:
 	static DBDriver * create(const ParametersMap & parameters = ParametersMap());
@@ -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
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef DBDRIVERSQLITE3_H_
 #define DBDRIVERSQLITE3_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>

@@ -37,7 +37,7 @@ typedef struct sqlite3 sqlite3;

 namespace rtabmap {

-class RTABMAP_EXP DBDriverSqlite3: public DBDriver {
+class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 public:
 	DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
 	virtual ~DBDriverSqlite3();
@@ -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
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef DBREADER_H_
 #define DBREADER_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/Transform.h>
@@ -43,7 +43,7 @@ namespace rtabmap {

 class DBDriver;

-class RTABMAP_EXP DBReader : public Camera {
+class RTABMAP_CORE_EXPORT DBReader : public Camera {
 public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -54,7 +54,11 @@ 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,
+			 bool priorsIgnored = false);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
@@ -64,7 +68,11 @@ 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,
+			 bool priorsIgnored = false);
 	virtual ~DBReader();

 	virtual bool init(
@@ -75,6 +83,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 +101,10 @@ private:
 	int _cameraIndex;
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
+	bool _featuresIgnored;
+	bool _priorsIgnored;
+	int _startMapId;
+	int _stopMapId;

 	DBDriver * _dbDriver;
 	UTimer _timer;
--- a/corelib/include/rtabmap/core/EpipolarGeometry.h
+++ b/corelib/include/rtabmap/core/EpipolarGeometry.h
@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #pragma once

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
@@ -42,7 +42,7 @@ namespace rtabmap

 class Signature;

-class RTABMAP_EXP EpipolarGeometry
+class RTABMAP_CORE_EXPORT EpipolarGeometry
 {
 public:
 	EpipolarGeometry(const ParametersMap & parameters = ParametersMap());
--- a/corelib/include/rtabmap/core/Features2d.h
+++ b/corelib/include/rtabmap/core/Features2d.h
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef FEATURES2D_H_
 #define FEATURES2D_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/core/core.hpp>
@@ -103,7 +103,7 @@ class CV_ORB;
 #endif

 // Feature2D
-class RTABMAP_EXP Feature2D {
+class RTABMAP_CORE_EXPORT Feature2D {
 public:
 	enum Type {kFeatureUndef=-1,
 		kFeatureSurf=0,
@@ -247,7 +247,7 @@ private:
 };

 //SURF
-class RTABMAP_EXP SURF : public Feature2D
+class RTABMAP_CORE_EXPORT SURF : public Feature2D
 {
 public:
 	SURF(const ParametersMap & parameters = ParametersMap());
@@ -274,7 +274,7 @@ private:
 };

 //SIFT
-class RTABMAP_EXP SIFT : public Feature2D
+class RTABMAP_CORE_EXPORT SIFT : public Feature2D
 {
 public:
 	SIFT(const ParametersMap & parameters = ParametersMap());
@@ -298,7 +298,7 @@ private:
 };

 //ORB
-class RTABMAP_EXP ORB : public Feature2D
+class RTABMAP_CORE_EXPORT ORB : public Feature2D
 {
 public:
 	ORB(const ParametersMap & parameters = ParametersMap());
@@ -329,7 +329,7 @@ private:
 };

 //FAST
-class RTABMAP_EXP FAST : public Feature2D
+class RTABMAP_CORE_EXPORT FAST : public Feature2D
 {
 public:
 	FAST(const ParametersMap & parameters = ParametersMap());
@@ -365,7 +365,7 @@ private:
 };

 //FAST_BRIEF
-class RTABMAP_EXP FAST_BRIEF : public FAST
+class RTABMAP_CORE_EXPORT FAST_BRIEF : public FAST
 {
 public:
 	FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
@@ -384,7 +384,7 @@ private:
 };

 //FAST_FREAK
-class RTABMAP_EXP FAST_FREAK : public FAST
+class RTABMAP_CORE_EXPORT FAST_FREAK : public FAST
 {
 public:
 	FAST_FREAK(const ParametersMap & parameters = ParametersMap());
@@ -406,7 +406,7 @@ private:
 };

 //GFTT
-class RTABMAP_EXP GFTT : public Feature2D
+class RTABMAP_CORE_EXPORT GFTT : public Feature2D
 {
 public:
 	GFTT(const ParametersMap & parameters = ParametersMap());
@@ -428,7 +428,7 @@ private:
 };

 //GFTT_BRIEF
-class RTABMAP_EXP GFTT_BRIEF : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_BRIEF : public GFTT
 {
 public:
 	GFTT_BRIEF(const ParametersMap & parameters = ParametersMap());
@@ -447,7 +447,7 @@ private:
 };

 //GFTT_FREAK
-class RTABMAP_EXP GFTT_FREAK : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_FREAK : public GFTT
 {
 public:
 	GFTT_FREAK(const ParametersMap & parameters = ParametersMap());
@@ -469,7 +469,7 @@ private:
 };

 //SURF_FREAK
-class RTABMAP_EXP SURF_FREAK : public SURF
+class RTABMAP_CORE_EXPORT SURF_FREAK : public SURF
 {
 public:
 	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
@@ -491,7 +491,7 @@ private:
 };

 //GFTT_ORB
-class RTABMAP_EXP GFTT_ORB : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_ORB : public GFTT
 {
 public:
 	GFTT_ORB(const ParametersMap & parameters = ParametersMap());
@@ -508,7 +508,7 @@ private:
 };

 //BRISK
-class RTABMAP_EXP BRISK : public Feature2D
+class RTABMAP_CORE_EXPORT BRISK : public Feature2D
 {
 public:
 	BRISK(const ParametersMap & parameters = ParametersMap());
@@ -530,7 +530,7 @@ private:
 };

 //KAZE
-class RTABMAP_EXP KAZE : public Feature2D
+class RTABMAP_CORE_EXPORT KAZE : public Feature2D
 {
 public:
 	KAZE(const ParametersMap & parameters = ParametersMap());
@@ -557,7 +557,7 @@ private:
 };

 //ORB OCTREE
-class RTABMAP_EXP ORBOctree : public Feature2D
+class RTABMAP_CORE_EXPORT ORBOctree : public Feature2D
 {
 public:
 	ORBOctree(const ParametersMap & parameters = ParametersMap());
@@ -583,7 +583,7 @@ private:
 };

 //SuperPointTorch
-class RTABMAP_EXP SuperPointTorch : public Feature2D
+class RTABMAP_CORE_EXPORT SuperPointTorch : public Feature2D
 {
 public:
 	SuperPointTorch(const ParametersMap & parameters = ParametersMap());
@@ -606,7 +606,7 @@ private:
 };

 //GFTT_DAISY
-class RTABMAP_EXP GFTT_DAISY : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT
 {
 public:
 	GFTT_DAISY(const ParametersMap & parameters = ParametersMap());
@@ -630,7 +630,7 @@ private:
 };

 //SURF_DAISY
-class RTABMAP_EXP SURF_DAISY : public SURF
+class RTABMAP_CORE_EXPORT SURF_DAISY : public SURF
 {
 public:
 	SURF_DAISY(const ParametersMap & parameters = ParametersMap());
--- a/corelib/include/rtabmap/core/FlannIndex.h
+++ b/corelib/include/rtabmap/core/FlannIndex.h
@@ -28,23 +28,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_SRC_FLANNINDEX_H_
 #define CORELIB_SRC_FLANNINDEX_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include <list>
 #include <opencv2/opencv.hpp>

 namespace rtabmap {

-class RTABMAP_EXP FlannIndex
+class RTABMAP_CORE_EXPORT FlannIndex
 {
 public:
 	FlannIndex();
 	virtual ~FlannIndex();

 	void release();
-	unsigned int indexedFeatures() const;
+	size_t indexedFeatures() const;

 	// return Bytes
-	unsigned long memoryUsed() const;
+	size_t memoryUsed() const;

 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
--- a/corelib/include/rtabmap/core/GainCompensator.h
+++ b/corelib/include/rtabmap/core/GainCompensator.h
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_SRC_GAINCOMPENSATOR_H_
 #define CORELIB_SRC_GAINCOMPENSATOR_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -41,7 +41,7 @@ namespace rtabmap {
 /**
 * Works like cv::GainCompensator but with point clouds
 */
-class RTABMAP_EXP GainCompensator {
+class RTABMAP_CORE_EXPORT GainCompensator {
 public:
 	GainCompensator(double maxCorrespondenceDistance = 0.02, double minOverlap = 0.0, double alpha = 0.01, double beta = 10);
 	virtual ~GainCompensator();
--- a/corelib/include/rtabmap/core/GeodeticCoords.h
+++ b/corelib/include/rtabmap/core/GeodeticCoords.h
@@ -43,13 +43,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef GEODETICCOORDS_H_
 #define GEODETICCOORDS_H_

-#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/rtabmap_core_export.h>

 #include <opencv2/core/core.hpp>

 namespace rtabmap {

-class RTABMAP_EXP GeodeticCoords
+class RTABMAP_CORE_EXPORT GeodeticCoords
 {
 public:
 	GeodeticCoords();
--- a/Show More
+++ b/Show More