bump 0.20.23

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

 on:
  push:
-    branches: [ master ]
+    branches: 
+      - '**'
  pull_request:
-    branches: [ master ]
+    branches: 
+      - '**'

 env:
-  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
  BUILD_TYPE: Release

 jobs:
  build:
-    # The CMake configure and build commands are platform agnostic and should work equally
-    # well on Windows or Mac.  You can convert this to a matrix build if you need
-    # cross-platform coverage.
-    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    name: ${{ matrix.os }}
    runs-on: ${{ matrix.os }}
    strategy:
      matrix:
-        os: [ubuntu-20.04, ubuntu-18.04]
-        include:
-        - os: ubuntu-20.04
-          ros_distro: 'noetic'
-        - os: ubuntu-18.04
-          ros_distro: 'melodic'
+        os: [ubuntu-22.04, ubuntu-20.04, ubuntu-18.04]

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

    - uses: actions/checkout@v2

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

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

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

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

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

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

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

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

 IF(WITH_PYTHON)
-    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy)
    IF(Python3_FOUND)
        MESSAGE(STATUS "Found Python3")
    ENDIF(Python3_FOUND)
@@ -714,6 +727,13 @@ IF(WITH_FASTCV)
    ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)

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

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

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

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

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

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

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

 RTAB-Map library and standalone application.

-For more information, visit the [RTAB-Map's home page](http://introlab.github.io/rtabmap) or the [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
+ * For more information (e.g., papers, major updates), visit [RTAB-Map's home page](http://introlab.github.io/rtabmap).
+ * For installation instructions and examples, visit [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).

 To use RTAB-Map under ROS, visit the [rtabmap](http://wiki.ros.org/rtabmap) page on the ROS wiki.

@@ -25,3 +25,52 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
 <a href="https://introlab.3it.usherbrooke.ca/">
 <img src="https://github.com/introlab/16SoundsUSB/blob/master/images/IntRoLab.png" alt="IntRoLab" height="100">
 </a>
+
+#### CI Latest
+
+  <table>
+    <tbody>
+        <tr>
+           <td>Linux</td>
+           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
+           </td>
+        </tr>
+        <tr>
+           <td>Windows</td>
+           <td><a href="https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true" alt="Build Status"/>
+           </td>
+        </tr>
+     </tbody>
+  </table>
+ 
+ #### ROS Binaries
+ 
+ `ros-$ROS_DISTRO-rtabmap`
+ 
+ <table>
+    <tbody>
+        <tr>
+           <td rowspan="2">ROS 1</td>
+           <td>Melodic</td>
+            <td><a href="http://build.ros.org/job/Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Noetic</td>
+            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td rowspan="3">ROS 2</td>
+            <td>Foxy</td>
+            <td><a href="http://build.ros2.org/job/Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Humble</td>
+            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Rolling</td>
+            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+    </tbody>
+</table>
+ 
--- a/Version.h.in
+++ b/Version.h.in
@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@CCCORELIB@#define RTABMAP_CCCORELIB
@OPEN3D@#define RTABMAP_OPEN3D
@FASTCV@#define RTABMAP_FASTCV
+@OPENGV@#define RTABMAP_OPENGV
@PDAL@#define RTABMAP_PDAL
@LOAM@#define RTABMAP_LOAM
@FLOAM@#define RTABMAP_FLOAM
--- a/app/android/AndroidManifest.xml.in
+++ b/app/android/AndroidManifest.xml.in
@@ -8,7 +8,6 @@
    <uses-permission android:name="android.permission.CAMERA" />
    <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
    <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
-    <uses-permission android:name="android.permission.ACCESS_MEDIA_LOCATION" />
    <uses-permission android:name="android.permission.INTERNET" />
    <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
--- a/app/android/jni/RTABMapApp.cpp
+++ b/app/android/jni/RTABMapApp.cpp
@@ -2130,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)));
 				}
 			}
 		}
--- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java
@@ -125,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";
@@ -303,7 +303,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}

 		public void onServiceDisconnected(ComponentName name) {
-			// Handle this if you need to gracefully shutsaveDatabasedown/retry
+			// Handle this if you need to gracefully shutdown/retry
 			// in the event that Tango itself crashes/gets upgraded while running.
 			mToast.makeText(getApplicationContext(), 
 					String.format("Tango disconnected!"), mToast.LENGTH_LONG).show();
@@ -504,10 +504,28 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mTotalLoopClosures = 0;
 		mLastFastMovementNotificationStamp = System.currentTimeMillis()/1000;
 		
+		
+		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 && 
-				getActivity().getExternalFilesDirs(null).length >=1)
+			(targetSdkVersion < 30 || getActivity().getExternalFilesDirs(null).length >=1))
 		{
-			File extStore = getActivity().getExternalFilesDirs(null)[0];
+			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();
@@ -3603,22 +3621,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				File exportDir = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR);
 				exportDir.mkdirs();
 				
-				// cleanup old zip
-				fileNames = Util.loadFileList(mWorkingDirectory + RTABMAP_EXPORT_DIR, false);
-				if(!DISABLE_LOG) Log.i(TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMAP_EXPORT_DIR));
-				for(int i=0; i<fileNames.length; ++i)
-				{
-					File f = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + "/" + fileNames[i]);
-					if(f.delete())
-					{
-						if(!DISABLE_LOG) Log.i(TAG, String.format("Deleted \"%s\"", f.getPath()));
-					}
-					else
-					{
-						if(!DISABLE_LOG) Log.i(TAG, String.format("Failed deleting \"%s\"", f.getPath()));
-					}
-				}
-
+				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))
 				{							
@@ -3660,41 +3663,30 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							final File f = new File(zipOutput);
 							final int fileSizeMB = (int)f.length()/(1024 * 1024);

-							// Save to public Documents/RTAB-Map folder
-							/*ContentValues values = new ContentValues();
-				            values.put(MediaStore.MediaColumns.DISPLAY_NAME, fileName); //file name                     
-				            values.put(MediaStore.MediaColumns.MIME_TYPE, "application/zip");        //file extension, will automatically add to file
-				            values.put(MediaStore.MediaColumns.RELATIVE_PATH, Environment.DIRECTORY_DOCUMENTS + "/RTAB-Map");     //end "/" is not mandatory
-				            Uri uri = getContentResolver().insert(MediaStore.Files.getContentUri("external"),values);
-				            if (uri != null) {
-				            	OutputStream out;
-								try {
-									out = getApplicationContext().getContentResolver().openOutputStream(uri);
-									
-									InputStream in = new FileInputStream(zipOutput);
-					        		byte[] buf = new byte[1024];
-					        		int len;
-					        		while ((len = in.read(buf)) > 0) {
-					        			out.write(buf, 0, len);
-					        		}
-					        		in.close();
-					        		out.close();
-					        		
-					        		f.delete(); // remove private file
-								} catch (IOException e) {
-									Log.e(TAG, e.getMessage());
-								}
-				            } */
+							AlertDialog d = new AlertDialog.Builder(getActivity())
+									.setCancelable(false)
+									.setTitle("Mesh Saved!")
+									.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, 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..."));

-							// Send to...
-							Intent shareIntent = new Intent();
-							shareIntent.setAction(Intent.ACTION_SEND);
-							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..."));
-
-							resetNoTouchTimer(true);
+											resetNoTouchTimer(true);
+										}
+									})
+									.setNegativeButton("No", new DialogInterface.OnClickListener() {
+										public void onClick(DialogInterface dialog, int which) {
+											resetNoTouchTimer(true);
+										}
+									}).create();
+							d.setCanceledOnTouchOutside(false);
+							d.show();
 						}
 					});
 				}
--- 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
@@ -982,7 +982,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 13;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				EXCLUDED_ARCHS = "";
@@ -1007,7 +1007,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.17;
+				MARKETING_VERSION = 0.20.19;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1039,7 +1039,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 13;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				FRAMEWORK_SEARCH_PATHS = (
@@ -1064,7 +1064,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.17;
+				MARKETING_VERSION = 0.20.19;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
--- a/app/ios/RTABMapApp/.gitignore
+++ b/app/ios/RTABMapApp/.gitignore
@@ -0,0 +1 @@
+Libraries
--- 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/ViewController.swift
+++ b/app/ios/RTABMapApp/ViewController.swift
@@ -115,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 {
@@ -1437,7 +1438,7 @@ 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")
        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) &&
           FileManager.default.fileExists(atPath: tmpDatabase.path) &&
@@ -1642,7 +1643,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            alert.addAction(yes)
            self.present(alert, animated: true, completion: nil)
            do {
-                let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
+                let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
                try FileManager.default.removeItem(at: tmpDatabase)
            }
            catch {
@@ -2199,7 +2200,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                    }
                    .sorted(by: { $0.1 > $1.1 }) // sort descending modification dates
                    .map { $0.0 } // extract file names
-            databases = data.filter{ $0.pathExtension == "db" }
+            databases = data.filter{ $0.pathExtension == "db" && $0.lastPathComponent != RTABMAP_TMP_DB && $0.lastPathComponent != RTABMAP_RECOVERY_DB }
            
        } catch {
            print("Error while enumerating files : \(error.localizedDescription)")
--- a/app/ios/RTABMapApp/Libraries/install_deps.sh
+++ b/app/ios/RTABMapApp/Libraries/install_deps.sh
@@ -1,7 +1,11 @@
 #!/bin/bash

+set -euxo pipefail
+
 # Tested on Apple Silicon Mac, with cmake 3.19.2.

+mkdir Libraries
+cd Libraries
 pwd=$(pwd)
 prefix=$pwd
 sysroot=iphoneos
@@ -107,7 +111,7 @@ git checkout tags/v8.2.0
 git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
 mkdir build
 cd build
-cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DIOS_SIMULATOR_ARCHITECTURES=arm64 -DModule_vtkFiltersModeling=ON ..
+cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
 # For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES=""
 cmake --build . --config Release
 cd $pwd
--- a/app/ios/Settings.bundle/Root.plist
+++ b/app/ios/Settings.bundle/Root.plist
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.17</string>
+			<string>0.20.19</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>
--- a/app/src/main.cpp
+++ b/app/src/main.cpp
@@ -38,10 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <vtkObject.h>

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

 int main(int argc, char* argv[])
@@ -54,10 +50,6 @@ int main(int argc, char* argv[])
 	CoInitialize(nullptr);
 #endif

-#ifdef RTABMAP_PYTHON
-	PythonInterface python; // Make sure we initialize python in main thread
-#endif
-
 #if VTK_MAJOR_VERSION >= 8
 	vtkObject::GlobalWarningDisplayOff();
 #endif
--- a/archive/2020-IlluminationInvariant/README.md
+++ b/archive/2020-IlluminationInvariant/README.md
@@ -1,22 +0,0 @@
-
-## Multi-Session Visual SLAM for Illumination Invariant Localization in Indoor Environments
-
-* Paper: https://arxiv.org/abs/2103.03827
-
-* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
-![Overview](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_overview.jpg)]
-
-
-## Description
-
-This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using Multi-Session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg)]
-
-The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg)]
-
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_map_merged_999.jpg)]
-
--- a/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg
+++ b/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg
--- a/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg
+++ b/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg
--- a/archive/2020-IlluminationInvariant/scripts/export_stats.sh
+++ b/archive/2020-IlluminationInvariant/scripts/export_stats.sh
@@ -1,19 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14) #0 1 6 7 8 9 11 12 13 14
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPORT_TOOL="${PREFIX}rtabmap-report"
-
-for d in "${DETECTOR[@]}"
-do
-  $REPORT_TOOL --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m "$SKIP/$d/loc"
-  $REPORT_TOOL --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ "$SKIP/$d/consecutive_loc"
-done
-
--- a/archive/2020-IlluminationInvariant/scripts/reprocess_maps.sh
+++ b/archive/2020-IlluminationInvariant/scripts/reprocess_maps.sh
@@ -1,42 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
-
-[ ! -d "$SKIP" ] && mkdir $SKIP
-[ ! -d "$SKIP/$TYPE" ] && mkdir $SKIP/$TYPE
-# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
-DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
-
-PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
-
-if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
-then
-  # binary descriptors
-  PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
-else
-  # float descriptors
-  PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
-if
-
-echo $PARAMS
-for db in "${DATABASES[@]}"
-do
-  $REPROCESS_TOOL --skip $SKIP --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 4 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --uerror $PARAMS $db $SKIP/$TYPE/$db
-  $DETECT_MORE_LOOP_CLOSURE_TOOL --uwarn $SKIP/$TYPE/$db
-done
-
--- a/archive/2020-IlluminationInvariant/scripts/reprocess_maps_all.sh
+++ b/archive/2020-IlluminationInvariant/scripts/reprocess_maps_all.sh
@@ -1,16 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./reprocess_maps.sh $d $SKIP
-  ./run_merge.sh $d $SKIP
-done
-
--- a/archive/2020-IlluminationInvariant/scripts/run_all.sh
+++ b/archive/2020-IlluminationInvariant/scripts/run_all.sh
@@ -1,13 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-./reprocess_maps_all.sh $SKIP
-./run_merge.sh $SKIP
-./run_localization_single_all.sh $SKIP
-./run_consecutive_localization_all.sh $SKIP
-
--- a/archive/2020-IlluminationInvariant/scripts/run_consecutive_localization.sh
+++ b/archive/2020-IlluminationInvariant/scripts/run_consecutive_localization.sh
@@ -1,31 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
-TARGETS=($SKIP/$TYPE'/map_190321-172717.db;'$SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-182709.db;' $SKIP/$TYPE'/map_190321-182709.db;'$SKIP/$TYPE'/map_190321-185608.db' $SKIP/$TYPE'/map_190321-185608.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-193556.db' )
-
-
-[ ! -d "$SKIP/$TYPE/consecutive_loc" ] && mkdir $SKIP/$TYPE/consecutive_loc
-
-for i in ${!SOURCE[@]}
-do
-    db=${SOURCE[$i]}
-    loc_dbs=${TARGETS[$i]}
-    $REPROCESS_TOOL --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --uwarn "$SKIP/$TYPE/$db;$loc_dbs" $SKIP/$TYPE/consecutive_loc/loc_$db
-done
-
--- a/archive/2020-IlluminationInvariant/scripts/run_consecutive_localization_all.sh
+++ b/archive/2020-IlluminationInvariant/scripts/run_consecutive_localization_all.sh
@@ -1,15 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./run_consecutive_localization.sh $d $SKIP
-done
-
--- a/archive/2020-IlluminationInvariant/scripts/run_localization_accuracy.sh
+++ b/archive/2020-IlluminationInvariant/scripts/run_localization_accuracy.sh
@@ -1,38 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
-LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
-
-[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/accuracy
-
-db=merged_9999.db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
-
-
--- a/archive/2020-IlluminationInvariant/scripts/run_localization_single.sh
+++ b/archive/2020-IlluminationInvariant/scripts/run_localization_single.sh
@@ -1,32 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_9999_reduced.db'
-DATABASES=(  'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db'  )
-# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
-LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
-
-[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/loc
-
-echo $PARAMS
-for db in "${DATABASES[@]}"
-do
-  $REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/loc/loc_$db
-done
-
-
--- a/archive/2020-IlluminationInvariant/scripts/run_localization_single_all.sh
+++ b/archive/2020-IlluminationInvariant/scripts/run_localization_single_all.sh
@@ -1,15 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./run_localization_single.sh $d $SKIP
-done
-
--- a/archive/2020-IlluminationInvariant/scripts/run_merge.sh
+++ b/archive/2020-IlluminationInvariant/scripts/run_merge.sh
@@ -1,38 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
-
-DATABASES="$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-185608.db;$SKIP/$TYPE/map_190321-193556.db"
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$DATABASES" $SKIP/$TYPE/merged_9999.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999.db
-
-#$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 --Mem/ReduceGraph true --Vis/MinInliers 60 "$DATABASES" $SKIP/$TYPE/merged_9999_reduced.db
-#$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999_reduced.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_16.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_16.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-185608.db" $SKIP/$TYPE/merged_135.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_135.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_246.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_246.db
-
-
-
--- a/archive/2020-IlluminationInvariant/scripts/valgrind.sh
+++ b/archive/2020-IlluminationInvariant/scripts/valgrind.sh
@@ -1,19 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 8 9 11 12 14)
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPORT_TOOL="${PREFIX}rtabmap-report"
-
-for d in "${DETECTOR[@]}"
-do
-  valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 ${PREFIX}rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false  "${SKIP}/${d}/merged_9999.db;map_190321-164651.db" output.db
-  rm output.db
-done
-
--- a/archive/2022-IlluminationInvariant/README.md
+++ b/archive/2022-IlluminationInvariant/README.md
@@ -0,0 +1,106 @@
+
+## Multi-Session Visual SLAM for Illumination Invariant Re-Localization in Indoor Environments
+
+* Paper: https://doi.org/10.3389/frobt.2022.801886
+
+* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
+![Overview](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_overview.jpg)
+
+
+## Description
+
+This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using multi-session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_single_percentage.jpg)
+
+The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_merged_percentage.jpg)
+
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_map_merged_999.jpg)
+
+
+## Dataset
+
+We provide two formats: the first one is more general and the second one is used to produce the results in this paper with RTAB-Map. Please open issue if the links are outdated.
+
+* [Images](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EV8F4PZUxOxLhwAyEehlzKwBjF-9xNuxR32Q4mUjx5u-rA?e=eCJ3TW):
+  * `rgb`: folder containing *.jpg color camera images
+  * `depth`: folder containing *.png 16bits mm depth images
+  * `calib`: folder containing calibration for each color image. Each calibration contains also the transform between `device` and `camera` frames as `local_transform`.
+  * `device_poses.txt`: VIO poses of each image in `device` frame
+  * `camera_poses.txt`: VIO poses of each image in `camera` frame
+* [RTAB-Map Databases](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EU5fb0jEKzlGhPK3OWjMGLUBnDo1BRAoZwtB2czyeVLE_A?e=Y0JyXY)
+
+
+
+## How reproduce results shown in the paper
+
+1. RTAB-Map should be built from source with those dependencies (don't need to "install" it, we will launch it from build directory in the scripts below to avoid conflicting with another rtabmap already installed): 
+    * Use Ubuntu 20.04+ to avoid any python2/python3 conflicts.
+    * OpenCV built with **xfeatures2d** and **nonfree** modules
+    * [torchlib c++](https://pytorch.org/get-started/locally/) (tested on v1.10.2) to enable [SuperPoint](https://github.com/magicleap/SuperPointPretrainedNetwork)
+    * Git clone [SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork) into scripts directory.
+    * Generate `superpoint_v1.pt` in the scripts directory (can also be downloaded from [here](https://github.com/KinglittleQ/SuperPoint_SLAM/blob/master/superpoint.pt) but may not be compatible with more recent pytorch versions):
+      ```bash
+      cd rtabmap/archive/2022-IlluminationInvariant/scripts
+      wget https://github.com/magicleap/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth
+      wget https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py
+      python trace.py
+      ```
+
+2. Download databases of the dataset and extract them.
+3. Adjust the path inside `rtabmap_latest.sh` script to match where you just built rtabmap with right dependencies.
+4. Run `run_all.sh DATABASES_PATH OUTPUT_PATH`, this script will do the following steps (warning, this could take hours to do...):
+    * Recreate the map databases for each feature type
+    * Create the merged databases
+    * Run localization databases over all map/merged databases
+    * Run consecutive localization experiment
+  
+5. Export statistics with `export_stats.sh` script.
+
+6. Use the MatLab/Octave scripts in this folder to show results you want. Set `dataDir` to directory containing the exported statistics. 
+```
+sudo apt install install octave liboctave-dev
+
+# In octave:
+pkg install -forge control signal
+```
+
+### Docker
+
+1. Create the docker image:
+```
+cd rtabmap
+docker build -t rtabmap_frontiers -f docker/frontiers2022/Dockerfile .
+```
+
+2. Assuming you extracted the databases of the dataset in `~/Downloads/Illumination_invariant_databases`, create an output directory for results:
+```
+mkdir ~/Downloads/Illumination_invariant_databases/results
+```
+
+3. Run script:
+```
+docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
+    --user $(id -u):$(id -g) \
+    -w=/workspace/scripts \
+    -v ~/Downloads/Illumination_invariant_databases:/workspace/databases \
+    -v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
+    rtabmap_frontiers /workspace/scripts/run_all.sh /workspace/databases /workspace/results
+```
+
+4. Export statistics:
+```
+docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
+    --env="DISPLAY=$DISPLAY" \
+    --env="QT_X11_NO_MITSHM=1" \
+    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+    --env="XAUTHORITY=$XAUTH" \
+    --volume="$XAUTH:$XAUTH" \
+    --user $(id -u):$(id -g) \
+    -w=/workspace/results \
+    -v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
+    rtabmap_frontiers /workspace/scripts/export_stats.sh /workspace/results
+```
--- a/archive/2022-IlluminationInvariant/images/fig_map_merged_999.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_map_merged_999.jpg
--- a/archive/2022-IlluminationInvariant/images/fig_merged_percentage.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_merged_percentage.jpg
--- a/archive/2022-IlluminationInvariant/images/fig_overview.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_overview.jpg
--- a/archive/2022-IlluminationInvariant/images/fig_single_percentage.jpg
+++ b/archive/2022-IlluminationInvariant/images/fig_single_percentage.jpg
--- a/archive/2022-IlluminationInvariant/results_common.m
+++ b/archive/2022-IlluminationInvariant/results_common.m
@@ -2,24 +2,35 @@
 clear all
 close all

+# sudo apt install octave-signal
 pkg load signal

-# rtabmap-report --loc 32 Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms . Keypoint/Current_frame/words
-# Right-click on thr legend of the figure, copy all data to clipboard
-# Paste in correction#.txt, inliers#.txt and time#.txt where # is the 
-# number of the descriptor used
+# Use with files generated by export_stats.sh

-skipFrameDir = '0';
-prefix = 'Stat';
-RAMaddOverhead = 1;
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
+resultsToShow = 1; % 1=single 2=Consecutive
+
+prefix = 'Stat'; 
+RAMaddOverhead = 0;
 % Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words'
 % Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1)
-statNames = {'Loop/Odom_correction_norm/m', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/'}; % 'Odometry_average'

-datasets = [ 0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/'}; % 'Odometry_average'
+
+ 
+datasets = [ 0 1 6 7 9 14 11 111 ]; % 0 1 6 7 9 12 14 11
 sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
 sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};

+if resultsToShow == 2
+  sep = [0, 1000, 3000, 5000, 7000, 9000];
+  sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
+  prefix = 'Consecutive';
+  statNames = {'Loop/Distance_since_last_loc/', 'Distance_since_last_loc_under_50cm'};
+endif
+
+MapsN = length(sepName);
+
 allCumResults = {};
 allMaxResults = {};

@@ -35,13 +46,15 @@ statName = strrep(statNames{s},'/','-');
 for d=1:length(datasets)

 if strcmp(statName,'Inliers_ratio_%')
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
-  dataWords = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  dataWords = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
  data(:, 2:end) = data(:, 2:end) ./ dataWords(:, 2:end) * 100;
 elseif strcmp(statName, 'Odometry_average')
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+elseif strcmp(statName, 'Distance_since_last_loc_under_50cm')
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
 else
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
 endif
 sessions = size(data,2)-1;

@@ -69,7 +82,7 @@ for i = 1:sessions
      if datasets(d) == 7
        %% 135 MB overhead for BRISK kernel
        y = y + 135;
-      elseif datasets(d) == 11
+      elseif datasets(d) == 11 || datasets(d) == 111
        %% 645 MB (library cuda) + 800 MB (network) for SuperPoint
        y = y + 645+800;
      elseif datasets(d) == 13 || datasets(d) == 14
@@ -81,6 +94,12 @@ for i = 1:sessions
    if strcmp(statName, 'Loop-Map_id-')
      nonzeros = y;
    end
+    if strcmp(statName, 'Distance_since_last_loc_under_50cm')
+      y(y>0.55) = 0;
+      y(isnan(y)) = 0;
+      y(y>0) = 1;
+      nonzeros = y;
+    end
    if length(nonzeros) > 0
      avgValue = sum(nonzeros)/length(nonzeros);
      avgResultsTmp(i,j) = avgValue;
@@ -103,7 +122,7 @@ for d=1:length(datasets)
  if sum(totalResults{1,d}, 2)
    cumResults(2:end-1,d+1) = sum(absResults{1,d}, 2) ./ sum(totalResults{1,d}, 2);
  endif
-  cumResults(end,d+1) = sum(sum(absResults{1,d}(1:6,1:6).*eye(6,6))) / sum(sum(totalResults{1,d}(1:6,1:6).*eye(6,6)));
+  cumResults(end,d+1) = sum(sum(absResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN))) / sum(sum(totalResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
 end
 cumResults(2:end-1,1) = 1:sessions;

@@ -123,7 +142,7 @@ for d=1:length(datasets)
  if sum(totalResults{1,d}, 2)
    cumMaxResults(2:end-1,d+1) = max(maxResults{1,d}, [], 2);
  endif
-  cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:6,1:6).*eye(6,6)));
+  cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
 end
 cumMaxResults(2:end-1,1) = 1:sessions;

@@ -133,3 +152,18 @@ allMaxResults{2,s} = cumMaxResults;
 endfor % statNames


+if resultsToShow == 2
+  disp('30min') 
+  for d=1:length(datasets)
+    round(sum(absResults{1,d} .* eye(5,5)) / sum(totalResults{1,d} .*eye(5,5)) * 100)
+  endfor
+  disp('60min') 
+  for d=1:length(datasets)
+    round(sum(absResults{1,d} .* [[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) / sum(totalResults{1,d} .*[[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) * 100)
+  endfor
+  disp('120min') 
+  for d=1:length(datasets)
+    round(avgResults{1,d}(1,5) * 100)
+  endfor
+endif
+
--- a/archive/2022-IlluminationInvariant/results_loc_over_time.m
+++ b/archive/2022-IlluminationInvariant/results_loc_over_time.m
@@ -4,16 +4,15 @@ clear all

 pkg load signal

-# rtabmap-report --loc 32 Loop/Map_id/ loc
-# Right-click on thr legend of the figure, copy all data to clipboard
-# Paste in data#.txt where # is the number of the descriptor used
+# Use with files generated by export_stats.sh

+dataDir = 'SET_PATH_TO_RESULTS_DIR';
 resultsToShow = 1; % 1=single loc, 2=merged loc, 3=consecutive
-skipFrameDir = '0';

 datasetPrefix = 'Stat';
-datasets = [0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+datasets = [0 1 6 7 9 14 11 111]; % 0 1 6 7 8 9 11 12
 datasetsName = {'SURF' 'SIFT' 'ORB' 'FAST/FREAK' 'FAST/BRIEF' 'GFTT/FREAK' 'GFTT/BRIEF' 'BRISK' 'GFTT/ORB' 'KAZE' 'ORB-OCTREE' 'SuperPoint' 'SURF/FREAK' 'GFTT/DAISY' 'SURF/DAISY'};
+datasetsName{112} = 'SuperGlue'
 sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
 sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};

@@ -41,13 +40,13 @@ globalc = [];

 for d=1:length(datasets)

-data = dlmread([skipFrameDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);
+data = dlmread([dataDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);

 curvesBeg = 2;
-curvesEnd = size(data,2)-4;
+curvesEnd = size(data,2)-5;  % -4 for '0', -5 for '1'

 if resultsToShow == 2
-  curvesBeg = 8;
+  curvesBeg = 8; % 2 if only 4 merged_reduced in stats, 8 to skip first 6
  curvesEnd = size(data,2);
 elseif resultsToShow == 3
  curvesEnd = size(data,2);
@@ -198,7 +197,24 @@ for d=1:length(datasets)
  data=percentResults{1,d}*100;
  data(isnan(data)) = 0;
  hAxes = gca;
-  imagesc( hAxes, data, [0, 100])
+  % Upscaling the image to reduce anti-aliasing effect in pfd viewers
+  scale = 50;
+  tickXStep = zeros(1, size(data, 2));
+  tickYStep = zeros(1, size(data, 1));
+  dataUp = upsample(upsample(data',scale)',scale);
+  for x=0:size(data, 2)-1
+    for y=1:scale-1
+       dataUp(:,(x*scale+1)+y) = dataUp(:,x*scale+1);
+    endfor
+    tickXStep(1,x+1) = scale/2 + scale*x;
+  endfor
+  for x=0:size(data, 1)-1
+    for y=1:scale-1
+       dataUp((x*scale+1)+y,:) = dataUp(x*scale+1,:);
+    endfor
+    tickYStep(1,x+1) = scale/2 + scale*x;
+  endfor
+  imagesc( hAxes, dataUp, [0, 100])
  %title({"",datasetsName{datasets(d)+1}})
  colors = [ones(100,1) [1:100]'*0.01 [1:100]'*0];
  colors(1,:) = 1;
@@ -211,12 +227,14 @@ for d=1:length(datasets)
    ylabel("Map")
  endif
  xlabel([datasetsName{datasets(d)+1} " Localization"])
-  set (gca, "xaxislocation", "top");
+  set(gca, "xaxislocation", "top");
+  set(gca, 'XTick', tickXStep)
  set(gca, 'XTickLabel', sepName, 'fontsize',7)
+  set(gca, 'YTick', tickYStep)
  if resultsToShow == 3
    set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56'}, 'fontsize',7)
  elseif resultsToShow == 2
-    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+6', 'bundle', 'reduced'}, 'fontsize',7)
+    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+5+6', '1-2-3-4-5-6', 'not set'}, 'fontsize',7)
  else
    set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56', '19:35'}, 'fontsize',7)
  endif
@@ -232,4 +250,4 @@ for d=1:length(datasets)
  endif
 end
 cumResults(2:end-1,1) = 1:curves;
-cumResults
+cumResults
--- a/archive/2022-IlluminationInvariant/results_loc_prob_by_dist.m
+++ b/archive/2022-IlluminationInvariant/results_loc_prob_by_dist.m
@@ -0,0 +1,109 @@
+
+clear all
+close all
+
+# sudo apt install octave-signal
+pkg load signal
+
+# Use with files generated by export_stats.sh
+
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
+prefix = 'Stat';
+
+dataset = 6
+sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
+sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
+
+statName = strrep('Loop/Distance_since_last_loc/','/','-');
+
+data = dlmread([dataDir '/' prefix num2str(dataset) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
+
+sessions = size(data,2)-1
+
+j = 1 % session #
+x3 = data(:,1);
+y3 = data(:,11);
+y3 = y3(x3>=sep(j) & x3<=sep(j+1), :);
+x3 = x3(x3>=sep(j) & x3<=sep(j+1), :);
+
+max = 0;
+for i=2:length(y3)
+  if isfinite(y3(i)) && isfinite(y3(i-1)) && (x3(i) - x3(i-1) < 1.5)
+    v = y3(i);
+    if v>max
+      max = v
+    endif
+  endif
+endfor
+xb = max/2:max:5;
+[nn3, xx3] = hist (y3, xb);
+nn3 = nn3/length(x3);
+nn3(nn3==0) = NaN;
+
+for j=1:6
+
+  x{j} = data(:,1);
+  y{j} = data(:,2);
+  y{j} = y{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
+  x{j} = x{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
+  [nn{j}, xx{j}] = hist (y{j}, xb);
+  %nn{j} = nn{j}/length(x{j});
+  %nn{j} = nn{j}/sum(nn{j});
+
+  for k=length(nn{j}):-1:1
+    if nn{j}(k) != 0
+      break;
+    else
+      nn{j}(k) = NaN;
+    endif
+  endfor
+
+endfor
+
+j = 6 % session #
+x4 = data(:,1);
+y4 = data(:,12);
+y4 = y4(x4>=sep(j) & x4<=sep(j+1), :);
+x4 = x4(x4>=sep(j) & x4<=sep(j+1), :);
+[nn4, xx4] = hist (y4, xb);
+nn4 = nn4/length(x4);
+nn4(nn4==0) = NaN;
+
+figure
+hold on
+%plot(x1,y1, '.-');
+%plot(x2-x2(1),y2, '.-');
+plot(x3-x3(1),y3, '.-');
+%plot(x4-x4(1),y4, '.-');
+%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+legend('Map1+2+3+4+5+6 -> LocA')
+xlabel('Time')
+ylabel('m')
+title('Distance since last loc')
+
+figure
+hold on
+%plot(x1,y1, '.-');
+%plot(x2-x2(1),y2, '.-');
+plot(x{6}-x{6}(1),y{6}, '.-');
+%plot(x4-x4(1),y4, '.-');
+%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+legend('Map1 -> LocF')
+xlabel('Time')
+ylabel('m')
+title('Distance since last loc')
+
+figure
+hold on
+for j=1:6
+plot(xx{j},nn{j}, '-', 'linewidth', 3)
+endfor
+%plot(xx3,nn3, '.-', 'linewidth', 3)
+%plot(xx4,nn4, '.-', 'linewidth', 3)
+%legend('Loc-F', 'Loc-E', 'Loc-D', 'Loc-C', 'Loc-B', 'Loc-A')
+legend('A-16:51', 'B-17:31', 'C-17:58', 'D-18:30', 'E-18:59', 'F-19:42', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+%h = get(gca,'Children');
+%set(gca,'Children',[h(6) h(5) h(4) h(3) h(2) h(1)])
+%set(gca, 'YScale', 'log')
+xlabel(['Distance not localized (m) Step '  num2str(max) ' m'])
+ylabel('Re-Localization Probability on Map 1 (16:46)')
--- a/archive/2022-IlluminationInvariant/scripts/export_stats.sh
+++ b/archive/2022-IlluminationInvariant/scripts/export_stats.sh
@@ -0,0 +1,19 @@
+#!/bin/bash
+
+if [ $# -lt 1 ]
+then
+  echo "No arguments supplied. It should be the data directory (where the reprocessed map databases are saved)."
+  exit
+fi
+DATA=$1
+
+DETECTOR=(0 1 6 7 9 14 11 111) 
+
+source rtabmap_latest.bash
+
+for d in "${DETECTOR[@]}"
+do
+  rtabmap-report --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/ Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
+  rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/ "$DATA/$d/consecutive_loc"
+done
+
--- a/archive/2022-IlluminationInvariant/scripts/reprocess_maps.sh
+++ b/archive/2022-IlluminationInvariant/scripts/reprocess_maps.sh
@@ -0,0 +1,41 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+source rtabmap_latest.bash
+
+[ ! -d "$OUTPUT" ] && mkdir $OUTPUT
+[ ! -d "$OUTPUT/$TYPE" ] && mkdir $OUTPUT/$TYPE
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
+DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
+
+PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
+
+if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
+then
+  # binary descriptors
+  PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
+else
+  # float descriptors
+  PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
+fi
+
+if [ $TYPE -eq 111 ] 
+then
+  PARAMS="--Vis/CorNNType 6 --SuperGlue/Path SuperGluePretrainedNetwork/rtabmap_superglue.py --Reg/RepeatOnce false --Vis/CorGuessWinSize 0 $PARAMS --Kp/DetectorStrategy 11 --Vis/FeatureType 11"
+fi
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  rtabmap-reprocess --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 0 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint_v1.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --Db/TargetVersion "" --Icp/CorrespondenceRatio 0.1 --RGBD/MaxOdomCacheSize 0 --uwarn $PARAMS $INPUT/$db $OUTPUT/$TYPE/$db
+  rtabmap-detectMoreLoopClosures --uwarn $OUTPUT/$TYPE/$db
+done
+
--- a/archive/2022-IlluminationInvariant/scripts/reprocess_maps_all.sh
+++ b/archive/2022-IlluminationInvariant/scripts/reprocess_maps_all.sh
@@ -0,0 +1,18 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./reprocess_maps.sh $d $INPUT $OUTPUT
+  ./run_merge.sh $d $OUTPUT
+done
+
--- a/archive/2022-IlluminationInvariant/scripts/rtabmap_latest.bash
+++ b/archive/2022-IlluminationInvariant/scripts/rtabmap_latest.bash
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+export PATH=~/workspace/rtabmap/build/bin:$PATH
+export LD_LIBRARY_PATH=~/workspace/rtabmap/build/lib:$LD_LIBRARY_PATH
+
--- a/archive/2022-IlluminationInvariant/scripts/run_all.sh
+++ b/archive/2022-IlluminationInvariant/scripts/run_all.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+./reprocess_maps_all.sh $INPUT $OUTPUT
+./run_localization_single_all.sh $INPUT $OUTPUT
+./run_consecutive_localization_all.sh $OUTPUT
+
--- a/archive/2022-IlluminationInvariant/scripts/run_consecutive_localization.sh
+++ b/archive/2022-IlluminationInvariant/scripts/run_consecutive_localization.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
+  exit
+fi
+TYPE=$1
+DATA=$2
+
+source rtabmap_latest.bash
+
+SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
+TARGETS=($DATA/$TYPE'/map_190321-172717.db;'$DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-182709.db;' $DATA/$TYPE'/map_190321-182709.db;'$DATA/$TYPE'/map_190321-185608.db' $DATA/$TYPE'/map_190321-185608.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-193556.db' )
+
+
+[ ! -d "$DATA/$TYPE/consecutive_loc" ] && mkdir $DATA/$TYPE/consecutive_loc
+
+for i in ${!SOURCE[@]}
+do
+    db=${SOURCE[$i]}
+    loc_dbs=${TARGETS[$i]}
+    rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$DATA/$TYPE/$db;$loc_dbs" $DATA/$TYPE/consecutive_loc/loc_$db
+done
+
--- a/archive/2022-IlluminationInvariant/scripts/run_consecutive_localization_all.sh
+++ b/archive/2022-IlluminationInvariant/scripts/run_consecutive_localization_all.sh
@@ -0,0 +1,16 @@
+#!/bin/bash
+
+if [ $# -lt 1 ]
+then
+  echo "No arguments supplied. It should be the data directory (where the reprocessed map databases will be saved)."
+  exit
+fi
+DATA=$1
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_consecutive_localization.sh $d $DATA
+done
+
--- a/archive/2022-IlluminationInvariant/scripts/run_localization_accuracy.sh
+++ b/archive/2022-IlluminationInvariant/scripts/run_localization_accuracy.sh
@@ -0,0 +1,33 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+source rtabmap_latest.bash
+
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
+
+[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/accuracy
+
+db=merged_123456.db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
+
+
--- a/archive/2022-IlluminationInvariant/scripts/run_localization_single.sh
+++ b/archive/2022-IlluminationInvariant/scripts/run_localization_single.sh
@@ -0,0 +1,28 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+
+source rtabmap_latest.bash
+
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db'
+DATABASES=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db')
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
+
+[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/loc
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  rtabmap-reprocess -loc_null --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/loc/loc_$db
+done
+
+
--- a/archive/2022-IlluminationInvariant/scripts/run_localization_single_all.sh
+++ b/archive/2022-IlluminationInvariant/scripts/run_localization_single_all.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_localization_single.sh $d $INPUT $OUTPUT
+done
+
--- a/archive/2022-IlluminationInvariant/scripts/run_merge.sh
+++ b/archive/2022-IlluminationInvariant/scripts/run_merge.sh
@@ -0,0 +1,34 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
+  exit
+fi
+TYPE=$1
+DATA=$2
+
+MIN_INLIERS=20 #20 40 60 80
+
+source rtabmap_latest.bash
+
+DATABASES="$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-185608.db;$DATA/$TYPE/map_190321-193556.db"
+
+# To compute "Ground truth"
+rtabmap-reprocess --uwarn "$DATABASES" $DATA/$TYPE/merged_123456.db
+
+cp $DATA/$TYPE/merged_123456.db $DATA/$TYPE/merged_123456_gt.db
+rtabmap-detectMoreLoopClosures -r 0.5 -i 5 $DATA/$TYPE/merged_123456_gt.db
+
+rtabmap-reprocess --uwarn -gt $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_16.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-185608.db" $DATA/$TYPE/merged_135.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_246.db
+
+# Reduced graph
+rtabmap-reprocess --uwarn -gt --Mem/ReduceGraph true --Vis/MinInliers $MIN_INLIERS $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456_reduced.db
+
+
--- a/archive/2022-IlluminationInvariant/scripts/trace.py
+++ b/archive/2022-IlluminationInvariant/scripts/trace.py
@@ -0,0 +1,9 @@
+import torch
+import torchvision
+from demo_superpoint  import SuperPointNet
+model = SuperPointNet()
+model.load_state_dict(torch.load("superpoint_v1.pth"))
+model.eval()
+example = torch.rand(1, 1, 640, 480)
+traced_script_module = torch.jit.trace(model, example)
+traced_script_module.save("superpoint_v1.pt")
--- a/archive/2022-IlluminationInvariant/scripts/valgrind.sh
+++ b/archive/2022-IlluminationInvariant/scripts/valgrind.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+source rtabmap_latest.bash
+
+for d in "${DETECTOR[@]}"
+do
+  valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false  "${OUTPUT}/${d}/map_190321-164651.db;${INPUT}/loc_190321-165128.db" output.db
+  rm output.db
+done
+
--- a/archive/2022-IlluminationInvariant/showLoopOverTime.m
+++ b/archive/2022-IlluminationInvariant/showLoopOverTime.m
@@ -0,0 +1,108 @@
+##close all
+##clear all
+
+%% Use Export Poses in TORO format, then copy columns
+load vertexes.txt;
+load edges.txt;
+
+set(0,'defaultAxesFontName', 'Times')
+set(0,'defaultTextFontName', 'Times')
+
+%matlab indexes  % rtabmap indexes
+endMap1 = 201; % ID=206 
+endMap2 = 401; % ID=411 
+endMap3 = 604; % ID=621  
+endMap4 = 794; % ID=814
+endMap5 = 968; % ID=990
+endMap6 = 1201; % ID=1230
+
+%% 3D
+
+t = vertexes(:,1);
+
+##figure
+##plot3(vertexes(1:endMap1,2), vertexes(1:endMap1,3), vertexes(1:endMap1,1))
+##hold on
+##plot3(vertexes(endMap1+1:endMap2,2), vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1))
+##plot3(vertexes(endMap2+1:endMap3,2), vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1))
+##plot3(vertexes(endMap3+1:endMap4,2), vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1))
+##plot3(vertexes(endMap4+1:endMap5,2), vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1))
+##plot3(vertexes(endMap5+1:end,2), vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1))
+
+mapIds = zeros(vertexes(end,1), 2); % matlab index to vertexes, map id
+for i=1:size(vertexes,1)
+    mapIds(vertexes(i,1),1) = i;
+    if i <= endMap1
+        mapIds(vertexes(i,1),2) = 1;
+    elseif i<=endMap2
+        mapIds(vertexes(i,1),2) = 2;
+    elseif i<=endMap3
+        mapIds(vertexes(i,1),2) = 3;
+    elseif i<=endMap4
+        mapIds(vertexes(i,1),2) = 4;
+    elseif i<=endMap5
+        mapIds(vertexes(i,1),2) = 5;
+    else
+        mapIds(vertexes(i,1),2) = 6;
+    end
+        
+end
+
+##interLoopClosures = 0;
+##intraLoopClosures = 0;
+##
+##for i=1:size(edges, 1)
+##    if edges(i,2) > edges(i,1)+1
+##        x = [vertexes(mapIds(edges(i,1),1), 2) vertexes(mapIds(edges(i,2),1), 2)];
+##        y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
+##        t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
+##        if mapIds(edges(i,1),2) ~= mapIds(edges(i,2),2)
+##            plot3(x,y,t, 'g')
+##            interLoopClosures = interLoopClosures+1;
+##        else
+##            plot3(x,y,t, 'r')
+##            intraLoopClosures = intraLoopClosures + 1;
+##        end
+##    end
+##end
+##xlabel('x')
+##ylabel('y')
+##zlabel('Node indexes')
+##
+##interLoopClosures
+##intraLoopClosures
+
+%% 2D
+figure
+hold on
+plot([-8 6], [vertexes(endMap1,1) vertexes(endMap1,1)], 'k:')
+plot([-8 6], [vertexes(endMap2,1) vertexes(endMap2,1)], 'k:')
+plot([-8 6], [vertexes(endMap3,1) vertexes(endMap3,1)], 'k:')
+plot([-8 6], [vertexes(endMap4,1) vertexes(endMap4,1)], 'k:')
+plot([-8 6], [vertexes(endMap5,1) vertexes(endMap5,1)], 'k:')
+
+colors = {'r:', 'g:', 'c:', 'y:', 'm:', 'c'};
+
+for i=1:size(edges, 1)
+    if edges(i,2) > edges(i,1)+1
+        y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
+        t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
+        mapId = mapIds(edges(i,1),2);
+        if mapId ~= mapIds(edges(i,2),2) && (mapId == 1 || mapIds(edges(i,2),2) == 1)
+            plot(y,t, 'r')
+        else
+            %plot(y,t, 'r')
+        end
+    end
+end
+
+curveColor = 'b'
+plot(vertexes(1:endMap1,3), vertexes(1:endMap1,1), curveColor)
+plot(vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1), curveColor)
+plot(vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1), curveColor)
+plot(vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1), curveColor)
+plot(vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1), curveColor)
+plot(vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1), 'k')
+
+xlabel('y')
+ylabel('Node indexes')
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -167,7 +167,7 @@ public:
 	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
-	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
+	bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -272,7 +272,7 @@ protected:
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;

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

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

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

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

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

 	DBDriver * _dbDriver;
 	UTimer _timer;
--- a/corelib/include/rtabmap/core/Graph.h
+++ b/corelib/include/rtabmap/core/Graph.h
@@ -159,14 +159,14 @@ std::list<Link> RTABMAP_EXP findLinks(
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
 /**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
 */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
 /**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
 */
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,
--- a/corelib/include/rtabmap/core/MarkerDetector.h
+++ b/corelib/include/rtabmap/core/MarkerDetector.h
@@ -65,16 +65,22 @@ public:

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

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

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

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

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

 	OccupancyGrid * _occupancy;

--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/corelib/include/rtabmap/core/Odometry.h
@@ -122,7 +122,7 @@ private:

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

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

 ////////////////////////////////////////////
@@ -134,7 +138,7 @@ public:
 			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
+			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
 			std::set<int> * outliers = 0);
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -183,7 +183,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
    RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
    RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "",          "Working directory.");
-    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum nodes retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRepublished,           unsigned int, 2, uFormat("Maximum nodes republished when requesting missing data. When %s=false, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if %s=false.", kRGBDEnabled().c_str(), kRtabmapPublishLastSignature().c_str()));
    RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true,  "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
    RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
    RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
@@ -370,6 +371,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess,     bool, false,  uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
    RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes. Raw features are not saved in database.");
    RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
+    RTABMAP_PARAM(RGBD, InvertedReg,                  bool, false,  "On loop closure, do registration from the target to reference instead of reference to target.");
    RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
    RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
    RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
@@ -594,6 +596,7 @@ class RTABMAP_EXP Parameters
 #else
    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #endif
+    RTABMAP_PARAM(Vis, PnPMaxVariance,           float, 0.0,    uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));

    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.1,    uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
@@ -780,8 +783,11 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
-    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,    "Maximum range in which markers will be detected. <=0 for unlimited range.");
-    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,    "Miniminum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
+    RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
+    RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");

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

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

 class RTABMAP_EXP Rtabmap
 {
@@ -222,6 +223,7 @@ public:
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
+	void addNodesToRepublish(const std::vector<int> & ids);

 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -283,6 +285,7 @@ private:
 	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
+	unsigned int _maxRepublished;
 	bool _rawDataKept;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
@@ -323,6 +326,8 @@ private:
 	bool _loopGPS;
 	int _maxOdomCacheSize;
 	bool _createGlobalScanMap;
+	float _markerPriorsLinearVariance;
+	float _markerPriorsAngularVariance;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
--- a/corelib/include/rtabmap/core/Transform.h
+++ b/corelib/include/rtabmap/core/Transform.h
@@ -143,6 +143,8 @@ public:
 	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
+	static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
+	static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);

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

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

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -74,6 +76,8 @@ private:
 	bool outputDepth_;
 	int depthConfidence_;
 	int resolution_;
+	bool imuFirmwareUpdate_;
+	bool imuPublished_;
 	std::shared_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -109,7 +109,7 @@ void segmentObstaclesFromGround(
 					{
 						Eigen::Vector4f min,max;
 						pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
-						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
+						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.at(i)->size() > points)
 						{
 							points = clusteredFlatSurfaces.at(i)->size();
 							biggestFlatSurfaceIndex = i;
--- a/corelib/include/rtabmap/core/odometry/OdometryF2M.h
+++ b/corelib/include/rtabmap/core/odometry/OdometryF2M.h
@@ -83,7 +83,7 @@ private:
 	std::map<int, Transform> bundlePoses_;
 	std::multimap<int, Link> bundleLinks_;
 	std::multimap<int, Link> bundleIMUOrientations_;
-	std::map<int, CameraModel> bundleModels_;
+	std::map<int, std::vector<CameraModel> > bundleModels_;
 	std::map<int, int> bundlePoseReferences_;
 	int bundleSeq_;
 	Optimizer * sba_;
--- a/corelib/include/rtabmap/core/odometry/OdometryMono.h
+++ b/corelib/include/rtabmap/core/odometry/OdometryMono.h
@@ -73,7 +73,7 @@ private:
 	std::map<int, std::map<int, cv::Point3f> > keyFrameWords3D_;
 	std::map<int, Transform> keyFramePoses_;
 	std::multimap<int, Link> keyFrameLinks_;
-	std::map<int, CameraModel> keyFrameModels_;
+	std::map<int, std::vector<CameraModel> > keyFrameModels_;
 	float maxVariance_;
 	float keyFrameThr_;
 };
--- a/corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h
+++ b/corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h
@@ -55,7 +55,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models,
+			const std::map<int, std::vector<CameraModel> > & models,
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
 			std::set<int> * outliers = 0);
--- a/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
+++ b/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
@@ -62,7 +62,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
+			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
 			std::set<int> * outliers = 0);
--- a/corelib/include/rtabmap/core/util3d_motion_estimation.h
+++ b/corelib/include/rtabmap/core/util3d_motion_estimation.h
@@ -48,6 +48,23 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
+			float maxVariance = 0,
+			const Transform & guess = Transform::getIdentity(),
+			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
+			cv::Mat * covariance = 0, // mean reproj error if words3B is not set
+			std::vector<int> * matchesOut = 0,
+			std::vector<int> * inliersOut = 0);
+			
+Transform RTABMAP_EXP estimateMotion3DTo2D(
+			const std::map<int, cv::Point3f> & words3A,
+			const std::map<int, cv::KeyPoint> & words2B,
+			const std::vector<CameraModel> & cameraModels,
+			int minInliers = 10,
+			int iterations = 100,
+			double reprojError = 5.,
+			int flagsPnP = 0,
+			int pnpRefineIterations = 1,
+			float maxVariance = 0,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
 			cv::Mat * covariance = 0, // mean reproj error if words3B is not set
--- a/corelib/include/rtabmap/core/util3d_transforms.h
+++ b/corelib/include/rtabmap/core/util3d_transforms.h
@@ -93,6 +93,9 @@ pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP transformPointCloud(
 cv::Point3f RTABMAP_EXP transformPoint(
 		const cv::Point3f & pt,
 		const Transform & transform);
+cv::Point3d RTABMAP_EXP transformPoint(
+		const cv::Point3d & pt,
+		const Transform & transform);
 pcl::PointXYZ RTABMAP_EXP transformPoint(
 		const pcl::PointXYZ & pt,
 		const Transform & transform);
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -199,6 +199,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
 	SET(LIBRARIES
 		${LIBRARIES}
 		Python3::Python
+		Python3::NumPy
 	)
 	SET(SRC_FILES
 		${SRC_FILES}
@@ -399,9 +400,6 @@ IF(G2O_FOUND)
 		${G2O_LIBRARIES}
 	  )
 	ENDIF()
-	SET(SRC_FILES ${SRC_FILES}
-		optimizer/g2o/edge_se3_xyzprior.cpp
-	)
 	IF(WITH_VERTIGO)
 		SET(SRC_FILES ${SRC_FILES}
 			optimizer/vertigo/g2o/edge_se2Switchable.cpp
@@ -424,16 +422,16 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)

-IF(WITH_CERES AND CERES_FOUND)
-	SET(INCLUDE_DIRS 
-		${INCLUDE_DIRS} 
-		${CERES_INCLUDE_DIRS}
+IF(CERES_FOUND)
+	SET(INCLUDE_DIRS
+	  ${INCLUDE_DIRS}
+	  ${CERES_INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
-		${LIBRARIES}
-		${CERES_LIBRARIES}
+	  ${LIBRARIES}
+	  ${CERES_LIBRARIES}
 	)
-ENDIF(WITH_CERES AND CERES_FOUND)
+ENDIF(CERES_FOUND)

 IF(libpointmatcher_FOUND)
 	SET(INCLUDE_DIRS 
@@ -471,6 +469,13 @@ IF(FastCV_FOUND)
 	)
 ENDIF(FastCV_FOUND)

+IF(opengv_FOUND)
+    SET(LIBRARIES
+        ${LIBRARIES}
+        opengv
+    )
+ENDIF(opengv_FOUND)
+
 IF(PDAL_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -720,29 +725,12 @@ endforeach(arg ${RESOURCES})
 #MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
 #MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")

-IF(ANDROID OR IOS)
-
-   IF(NOT RTABMAP_RES_TOOL)
-      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${PROJECT_BINARY_DIR}/../bin)
-      IF(NOT RTABMAP_RES_TOOL)
-         MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
-      ENDIF(NOT RTABMAP_RES_TOOL)
-   ENDIF(NOT RTABMAP_RES_TOOL)
-
-   ADD_CUSTOM_COMMAND(
-      OUTPUT ${RESOURCES_HEADERS}
-      COMMAND ${RTABMAP_RES_TOOL} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
-      COMMENT "[Creating resources]"
-      DEPENDS ${RESOURCES}
-   )
-ELSE()
-   ADD_CUSTOM_COMMAND(
-      OUTPUT ${RESOURCES_HEADERS}
-      COMMAND ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/rtabmap-res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
-      COMMENT "[Creating resources]"
-      DEPENDS ${RESOURCES} res_tool
-   )
-ENDIF()
+ADD_CUSTOM_COMMAND(
+	OUTPUT ${RESOURCES_HEADERS}
+	COMMAND res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
+	COMMENT "[Creating resources]"
+	DEPENDS ${RESOURCES}
+)

 ####################################
 # Generate resources files END
--- a/corelib/src/CameraModel.cpp
+++ b/corelib/src/CameraModel.cpp
@@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

-CameraModel::CameraModel()
+CameraModel::CameraModel() :
+		localTransform_(opticalRotation())
 {

 }
@@ -339,6 +340,25 @@ bool CameraModel::load(const std::string & filePath)
 				UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
 			}

+			n = fs["local_transform"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				int rows = (int)n["rows"];
+				int cols = (int)n["cols"];
+				std::vector<float> data;
+				n["data"] >> data;
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 4);
+				localTransform_ = Transform(
+						data[0], data[1], data[2], data[3],
+						data[4], data[5], data[6], data[7],
+						data[8], data[9], data[10], data[11]);
+			}
+			else
+			{
+				UWARN("Missing \"local_transform\" field in \"%s\"", filePath.c_str());
+			}
+
 			fs.release();

 			if(isValidForRectification())
@@ -448,6 +468,15 @@ bool CameraModel::save(const std::string & directory) const
 			fs << "}";
 		}

+		if(!localTransform_.isNull())
+		{
+			fs << "local_transform" << "{";
+			fs << "rows" << 3;
+			fs << "cols" << 4;
+			fs << "data" << std::vector<float>((float*)localTransform_.data(), ((float*)localTransform_.data())+12);
+			fs << "}";
+		}
+
 		fs.release();

 		return true;
--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -287,9 +287,10 @@ void CameraThread::mainLoop()
 				model.setLocalTransform(_extrinsicsOdomToCamera);
 				data.setCameraModel(model);
 			}
-			else
+			else if(!data.stereoCameraModels().empty())
 			{
-				StereoCameraModel model = data.stereoCameraModel();
+				UASSERT(data.stereoCameraModels().size()==1);
+				StereoCameraModel model = data.stereoCameraModels()[0];
 				model.setLocalTransform(_extrinsicsOdomToCamera);
 				data.setStereoCameraModel(model);
 			}
@@ -358,7 +359,12 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 		else if(!data.rightRaw().empty())
 		{
-			data.setRGBDImage(data.imageRaw(), cv::Mat(), data.stereoCameraModel().left());
+			std::vector<CameraModel> models;
+			for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+			{
+				models.push_back(data.stereoCameraModels()[i].left());
+			}
+			data.setRGBDImage(data.imageRaw(), cv::Mat(), models);
 		}
 	}

@@ -435,91 +441,116 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 			{
 				data.setRGBDImage(image, depthOrRight, models);
 			}
-			else
+
+
+			std::vector<StereoCameraModel> stereoModels = data.stereoCameraModels();
+			for(unsigned int i=0; i<stereoModels.size(); ++i)
 			{
-				StereoCameraModel stereoModel = data.stereoCameraModel();
-				if(stereoModel.isValidForProjection())
+				if(stereoModels[i].isValidForProjection())
 				{
-					stereoModel.scale(1.0/double(_imageDecimation));
+					stereoModels[i].scale(1.0/double(_imageDecimation));
 				}
-				data.setStereoImage(image, depthOrRight, stereoModel);
+			}
+			if(!stereoModels.empty())
+			{
+				data.setStereoImage(image, depthOrRight, stereoModels);
 			}
 		}
 		if(info) info->timeImageDecimation = timer.ticks();
 	}
-	if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
+	if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1)
 	{
-		UDEBUG("");
-		UTimer timer;
-		cv::Mat tmpRgb;
-		cv::flip(data.imageRaw(), tmpRgb, 1);
+		if(data.cameraModels().size() == 1)
+		{
+			UDEBUG("");
+			UTimer timer;
+			cv::Mat tmpRgb;
+			cv::flip(data.imageRaw(), tmpRgb, 1);

-		UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
-		CameraModel tmpModel = data.cameraModels()[0];
-		if(data.cameraModels()[0].cx())
-		{
-			tmpModel = CameraModel(
-					data.cameraModels()[0].fx(),
-					data.cameraModels()[0].fy(),
-					float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
-					data.cameraModels()[0].cy(),
-					data.cameraModels()[0].localTransform(),
-					data.cameraModels()[0].Tx(),
-					data.cameraModels()[0].imageSize());
+			CameraModel tmpModel = data.cameraModels()[0];
+			if(data.cameraModels()[0].cx())
+			{
+				tmpModel = CameraModel(
+						data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy(),
+						float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
+						data.cameraModels()[0].cy(),
+						data.cameraModels()[0].localTransform(),
+						data.cameraModels()[0].Tx(),
+						data.cameraModels()[0].imageSize());
+			}
+			cv::Mat tmpDepth = data.depthOrRightRaw();
+			if(!data.depthRaw().empty())
+			{
+				cv::flip(data.depthRaw(), tmpDepth, 1);
+			}
+			data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
+			if(info) info->timeMirroring = timer.ticks();
 		}
-		cv::Mat tmpDepth = data.depthOrRightRaw();
-		if(!data.depthRaw().empty())
+		else
 		{
-			cv::flip(data.depthRaw(), tmpDepth, 1);
+			UWARN("Mirroring is not implemented for multiple cameras or stereo...");
 		}
-		data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
-		if(info) info->timeMirroring = timer.ticks();
 	}

 	if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
 	{
+		if(data.stereoCameraModels().size()==1)
+		{
 #if CV_MAJOR_VERSION < 3
-		UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
+			UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
 #else
-		UDEBUG("");
-		UTimer timer;
-		cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
-		std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
-		std::vector<cv::UMat> images;
-		std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1,  cv::Scalar(255)));
-		images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
-		images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
-		compensator->feed(topLeftCorners, images, masks);
-		cv::Mat imgLeft = data.imageRaw().clone();
-		compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
-		cv::Mat imgRight = data.rightRaw().clone();
-		compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
-		data.setStereoImage(imgLeft, imgRight, data.stereoCameraModel());
-		cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
-		UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
-		if(info) info->timeStereoExposureCompensation = timer.ticks();
+			UDEBUG("");
+			UTimer timer;
+			cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
+			std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
+			std::vector<cv::UMat> images;
+			std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1,  cv::Scalar(255)));
+			images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
+			images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
+			compensator->feed(topLeftCorners, images, masks);
+			cv::Mat imgLeft = data.imageRaw().clone();
+			compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
+			cv::Mat imgRight = data.rightRaw().clone();
+			compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
+			data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]);
+			cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
+			UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
+			if(info) info->timeStereoExposureCompensation = timer.ticks();
 #endif
+		}
+		else
+		{
+			UWARN("Stereo exposure compensation only is not implemented to multiple stereo cameras...");
+		}
 	}

-	if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
+	if(_stereoToDepth && !data.imageRaw().empty() && !data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection() && !data.rightRaw().empty())
 	{
-		UDEBUG("");
-		UTimer timer;
-		cv::Mat depth = util2d::depthFromDisparity(
-				_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
-				data.stereoCameraModel().left().fx(),
-				data.stereoCameraModel().baseline());
-		// set Tx for stereo bundle adjustment (when used)
-		CameraModel model = CameraModel(
-				data.stereoCameraModel().left().fx(),
-				data.stereoCameraModel().left().fy(),
-				data.stereoCameraModel().left().cx(),
-				data.stereoCameraModel().left().cy(),
-				data.stereoCameraModel().localTransform(),
-				-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx(),
-				data.stereoCameraModel().left().imageSize());
-		data.setRGBDImage(data.imageRaw(), depth, model);
-		if(info) info->timeDisparity = timer.ticks();
+		if(data.stereoCameraModels().size()==1)
+		{
+			UDEBUG("");
+			UTimer timer;
+			cv::Mat depth = util2d::depthFromDisparity(
+					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
+					data.stereoCameraModels()[0].left().fx(),
+					data.stereoCameraModels()[0].baseline());
+			// set Tx for stereo bundle adjustment (when used)
+			CameraModel model = CameraModel(
+					data.stereoCameraModels()[0].left().fx(),
+					data.stereoCameraModels()[0].left().fy(),
+					data.stereoCameraModels()[0].left().cx(),
+					data.stereoCameraModels()[0].left().cy(),
+					data.stereoCameraModels()[0].localTransform(),
+					-data.stereoCameraModels()[0].baseline()*data.stereoCameraModels()[0].left().fx(),
+					data.stereoCameraModels()[0].left().imageSize());
+			data.setRGBDImage(data.imageRaw(), depth, model);
+			if(info) info->timeDisparity = timer.ticks();
+		}
+		else
+		{
+			UWARN("Stereo to depth is not implemented for multiple stereo cameras...");
+		}
 	}
 	if(_scanFromDepth &&
 		data.cameraModels().size() &&
--- a/corelib/src/DBDriver.cpp
+++ b/corelib/src/DBDriver.cpp
@@ -726,7 +726,7 @@ void DBDriver::getNodeData(
 bool DBDriver::getCalibration(
 		int signatureId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel) const
+		std::vector<StereoCameraModel> & stereoModels) const
 {
 	UDEBUG("");
 	bool found = false;
@@ -735,7 +735,7 @@ bool DBDriver::getCalibration(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		models = _trashSignatures.at(signatureId)->sensorData().cameraModels();
-		stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel();
+		stereoModels = _trashSignatures.at(signatureId)->sensorData().stereoCameraModels();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -743,7 +743,7 @@ bool DBDriver::getCalibration(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getCalibrationQuery(signatureId, models, stereoModel);
+		found = this->getCalibrationQuery(signatureId, models, stereoModels);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -500,6 +500,7 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & out
 				UERROR("Failed to rename just closed db %s to %s", this->getUrl().c_str(), outputUrl.c_str());
 			}
 		}
+		UINFO("Disconnected database %s!", this->getUrl().c_str());
 	}
 }

@@ -1448,7 +1449,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				cv::Mat imageCompressed;
 				cv::Mat depthOrRightCompressed;
 				std::vector<CameraModel> models;
-				StereoCameraModel stereoModel;
+				std::vector<StereoCameraModel> stereoModels;
 				Transform localTransform = Transform::getIdentity();
 				cv::Mat scanCompressed;
 				cv::Mat userDataCompressed;
@@ -1515,8 +1516,16 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 									}
 									else if(type == 1) // stereo
 									{
-										int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
-										UASSERT(bytesRead == dataSize);
+										StereoCameraModel model;
+										int bytesReadTotal = 0;
+										unsigned int bytesRead = 0;
+										while(bytesReadTotal < dataSize &&
+											  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
+										{
+											bytesReadTotal+=bytesRead;
+											stereoModels.push_back(model);
+										}
+										UASSERT(bytesReadTotal == dataSize);
 									}
 									else
 									{
@@ -1589,14 +1598,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 									{
 										localTransform.normalizeRotation();
 									}
-									stereoModel = StereoCameraModel(
+									stereoModels.push_back(StereoCameraModel(
 											dataFloat[0],  // fx
 											dataFloat[1],  // fy
 											dataFloat[2],  // cx
 											dataFloat[3],  // cy
 											dataFloat[4], // baseline
 											localTransform,
-											cv::Size(dataFloat[5],dataFloat[6]));
+											cv::Size(dataFloat[5],dataFloat[6])));
 								}
 								else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 								{
@@ -1606,13 +1615,13 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 									{
 										localTransform.normalizeRotation();
 									}
-									stereoModel = StereoCameraModel(
+									stereoModels.push_back(StereoCameraModel(
 											dataFloat[0],  // fx
 											dataFloat[1],  // fy
 											dataFloat[2],  // cx
 											dataFloat[3],  // cy
 											dataFloat[4], // baseline
-											localTransform);
+											localTransform));
 								}
 								else
 								{
@@ -1632,7 +1641,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						if(fyOrBaseline < 1.0)
 						{
 							//it is a baseline
-							stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+							stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
 						}
 						else
 						{
@@ -1818,7 +1827,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					}
 					else
 					{
-						(*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModel);
+						(*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModels);
 					}
 				}
 				if(userData)
@@ -1850,7 +1859,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 bool DBDriverSqlite3::getCalibrationQuery(
 		int signatureId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel) const
+		std::vector<StereoCameraModel> & stereoModels) const
 {
 	bool found = false;
 	if(_ppDb && signatureId)
@@ -1936,8 +1945,16 @@ bool DBDriverSqlite3::getCalibrationQuery(
 							}
 							else if(type == 1) // stereo
 							{
-								int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
-								UASSERT(bytesRead == dataSize);
+								StereoCameraModel model;
+								int bytesReadTotal = 0;
+								unsigned int bytesRead = 0;
+								while(bytesReadTotal < dataSize &&
+									  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
+								{
+									bytesReadTotal+=bytesRead;
+									stereoModels.push_back(model);
+								}
+								UASSERT(bytesReadTotal == dataSize);
 							}
 							else
 							{
@@ -2010,14 +2027,14 @@ bool DBDriverSqlite3::getCalibrationQuery(
 							{
 								localTransform.normalizeRotation();
 							}
-							stereoModel = StereoCameraModel(
+							stereoModels.push_back(StereoCameraModel(
 									dataFloat[0],  // fx
 									dataFloat[1],  // fy
 									dataFloat[2],  // cx
 									dataFloat[3],  // cy
 									dataFloat[4], // baseline
 									localTransform,
-									cv::Size(dataFloat[5],dataFloat[6]));
+									cv::Size(dataFloat[5],dataFloat[6])));
 						}
 						else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 						{
@@ -2027,13 +2044,13 @@ bool DBDriverSqlite3::getCalibrationQuery(
 							{
 								localTransform.normalizeRotation();
 							}
-							stereoModel = StereoCameraModel(
+							stereoModels.push_back((StereoCameraModel(
 									dataFloat[0],  // fx
 									dataFloat[1],  // fy
 									dataFloat[2],  // cx
 									dataFloat[3],  // cy
 									dataFloat[4], // baseline
-									localTransform);
+									localTransform)));
 						}
 						else
 						{
@@ -2054,7 +2071,7 @@ bool DBDriverSqlite3::getCalibrationQuery(
 				if(fyOrBaseline < 1.0)
 				{
 					//it is a baseline
-					stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+					stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
 				}
 				else
 				{
@@ -3278,7 +3295,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					int dataSize = 0;
 					Transform localTransform;
 					std::vector<CameraModel> models;
-					StereoCameraModel stereoModel;
+					std::vector<StereoCameraModel> stereoModels;

 					// calibration
 					data = sqlite3_column_blob(ppStmt, index);
@@ -3308,8 +3325,16 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 								}
 								else if(type == 1) // stereo
 								{
-									int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
-									UASSERT(bytesRead == dataSize);
+									StereoCameraModel model;
+									int bytesReadTotal = 0;
+									unsigned int bytesRead = 0;
+									while(bytesReadTotal < dataSize &&
+										  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
+									{
+										bytesReadTotal+=bytesRead;
+										stereoModels.push_back(model);
+									}
+									UASSERT(bytesReadTotal == dataSize);
 								}
 								else
 								{
@@ -3383,14 +3408,14 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 								{
 									localTransform.normalizeRotation();
 								}
-								stereoModel = StereoCameraModel(
+								stereoModels.push_back(StereoCameraModel(
 										dataFloat[0],  // fx
 										dataFloat[1],  // fy
 										dataFloat[2],  // cx
 										dataFloat[3],  // cy
 										dataFloat[4], // baseline
 										localTransform,
-										cv::Size(dataFloat[5], dataFloat[6]));
+										cv::Size(dataFloat[5], dataFloat[6])));
 							}
 							else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 							{
@@ -3400,13 +3425,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 								{
 									localTransform.normalizeRotation();
 								}
-								stereoModel = StereoCameraModel(
+								stereoModels.push_back(StereoCameraModel(
 										dataFloat[0],  // fx
 										dataFloat[1],  // fy
 										dataFloat[2],  // cx
 										dataFloat[3],  // cy
 										dataFloat[4], // baseline
-										localTransform);
+										localTransform));
 							}
 							else
 							{
@@ -3415,7 +3440,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						}

 						(*iter)->sensorData().setCameraModels(models);
-						(*iter)->sensorData().setStereoCameraModel(stereoModel);
+						(*iter)->sensorData().setStereoCameraModels(stereoModels);
 					}
 					rc = sqlite3_step(ppStmt);
 				}
@@ -4382,8 +4407,8 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 				   !(*i)->sensorData().depthOrRightCompressed().empty() ||
 				   !(*i)->sensorData().laserScanCompressed().isEmpty() ||
 				   !(*i)->sensorData().userDataCompressed().empty() ||
-				   !(*i)->sensorData().cameraModels().size() ||
-				   !(*i)->sensorData().stereoCameraModel().isValidForProjection())
+				   !(*i)->sensorData().cameraModels().empty() ||
+				   !(*i)->sensorData().stereoCameraModels().empty())
 				{
 					UASSERT((*i)->id() == (*i)->sensorData().id());
 					stepSensorData(ppStmt, (*i)->sensorData());
@@ -5691,13 +5716,15 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 		cy = sensorData.cameraModels()[0].cy();
 		localTransform = sensorData.cameraModels()[0].localTransform();
 	}
-	else if(sensorData.stereoCameraModel().isValidForProjection())
+	else if(sensorData.stereoCameraModels().size())
 	{
-		fx = sensorData.stereoCameraModel().left().fx();
-		fyOrBaseline = sensorData.stereoCameraModel().baseline();
-		cx = sensorData.stereoCameraModel().left().cx();
-		cy = sensorData.stereoCameraModel().left().cy();
-		localTransform = sensorData.stereoCameraModel().left().localTransform();
+		UASSERT_MSG(sensorData.stereoCameraModels().size() == 1,
+				uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
+		fx = sensorData.stereoCameraModels()[0].left().fx();
+		fyOrBaseline = sensorData.stereoCameraModels()[0].baseline();
+		cx = sensorData.stereoCameraModels()[0].left().cx();
+		cy = sensorData.stereoCameraModels()[0].left().cy();
+		localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
 	}

 	if(uStrNumCmp(_version, "0.7.0") >= 0)
@@ -6040,24 +6067,32 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 			}
 		}
 	}
-	else if(sensorData.stereoCameraModel().isValidForProjection())
+	else if(sensorData.stereoCameraModels().size() && sensorData.stereoCameraModels()[0].isValidForProjection())
 	{
 		if(uStrNumCmp(_version, "0.18.0") >= 0)
 		{
-			calibrationData = sensorData.stereoCameraModel().serialize();
-			UASSERT(!calibrationData.empty());
+			for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
+			{
+				UASSERT(sensorData.stereoCameraModels()[i].isValidForProjection());
+				std::vector<unsigned char> data = sensorData.stereoCameraModels()[i].serialize();
+				UASSERT(!data.empty());
+				unsigned int oldSize = calibrationData.size();
+				calibrationData.resize(calibrationData.size() + data.size());
+				memcpy(calibrationData.data()+oldSize, data.data(), data.size());
+			}
 		}
 		else
 		{
-			const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
+			UASSERT_MSG(sensorData.stereoCameraModels().size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
+			const Transform & localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
 			calibration.resize(7+localTransform.size());
-			calibration[0] = sensorData.stereoCameraModel().left().fx();
-			calibration[1] = sensorData.stereoCameraModel().left().fy();
-			calibration[2] = sensorData.stereoCameraModel().left().cx();
-			calibration[3] = sensorData.stereoCameraModel().left().cy();
-			calibration[4] = sensorData.stereoCameraModel().baseline();
-			calibration[5] = sensorData.stereoCameraModel().left().imageWidth();
-			calibration[6] = sensorData.stereoCameraModel().left().imageHeight();
+			calibration[0] = sensorData.stereoCameraModels()[0].left().fx();
+			calibration[1] = sensorData.stereoCameraModels()[0].left().fy();
+			calibration[2] = sensorData.stereoCameraModels()[0].left().cx();
+			calibration[3] = sensorData.stereoCameraModels()[0].left().cy();
+			calibration[4] = sensorData.stereoCameraModels()[0].baseline();
+			calibration[5] = sensorData.stereoCameraModels()[0].left().imageWidth();
+			calibration[6] = sensorData.stereoCameraModels()[0].left().imageHeight();
 			memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
 		}
 	}
--- a/corelib/src/DBReader.cpp
+++ b/corelib/src/DBReader.cpp
@@ -51,7 +51,10 @@ DBReader::DBReader(const std::string & databasePath,
 				   int cameraIndex,
 				   int stopId,
 				   bool intermediateNodesIgnored,
-				   bool landmarksIgnored) :
+				   bool landmarksIgnored,
+				   bool featuresIgnored,
+				   int startMapId,
+				   int stopMapId) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
@@ -62,6 +65,9 @@ DBReader::DBReader(const std::string & databasePath,
 	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
+	_featuresIgnored(featuresIgnored),
+	_startMapId(startMapId),
+	_stopMapId(stopMapId),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -73,6 +79,11 @@ DBReader::DBReader(const std::string & databasePath,
 	{
 		_stopId = _startId;
 	}
+
+	if(_stopMapId>-1 && _stopMapId<_startMapId)
+	{
+		_stopMapId = _startMapId;
+	}
 }

 DBReader::DBReader(const std::list<std::string> & databasePaths,
@@ -84,7 +95,10 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int cameraIndex,
 				   int stopId,
 				   bool intermediateNodesIgnored,
-				   bool landmarksIgnored) :
+				   bool landmarksIgnored,
+				   bool featuresIgnored,
+				   int startMapId,
+				   int stopMapId) :
 	Camera(frameRate),
   _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
@@ -95,6 +109,9 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
+	_featuresIgnored(featuresIgnored),
+	_startMapId(startMapId),
+	_stopMapId(stopMapId),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -106,6 +123,11 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	{
 		_stopId = _startId;
 	}
+
+	if(_stopMapId>-1 && _stopMapId<_startMapId)
+	{
+		_stopMapId = _startMapId;
+	}
 }

 DBReader::~DBReader()
@@ -182,8 +204,8 @@ bool DBReader::init(
 	if(_ids.size())
 	{
 		std::vector<CameraModel> models;
-		StereoCameraModel stereoModel;
-		if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
+		std::vector<StereoCameraModel> stereoModels;
+		if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModels))
 		{
 			if(models.size())
 			{
@@ -204,7 +226,7 @@ bool DBReader::init(
 					}
 				}
 			}
-			else if(stereoModel.isValidForProjection())
+			else if(stereoModels.size() && stereoModels.at(0).isValidForProjection())
 			{
 				_calibrated = true;
 			}
@@ -364,6 +386,15 @@ SensorData DBReader::getNextData(CameraInfo * info)

 			if(_intermediateNodesIgnored && s->getWeight() == -1)
 			{
+				UDEBUG("Ignoring node %d (intermediate nodes ignored)", s->id());
+				++_currentId;
+				delete s;
+				continue;
+			}
+
+			if(s->mapId() < _startMapId || (_stopMapId>=0 && s->mapId() > _stopMapId))
+			{
+				UDEBUG("Ignoring node %d (map id=%d, min=%d max=%d)", s->id(), s->mapId(), _startMapId, _stopMapId);
 				++_currentId;
 				delete s;
 				continue;
@@ -433,20 +464,34 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					infMatrix = links.begin()->second.infMatrix();
 					_previousInfMatrix = infMatrix;
 				}
-				else if(_previousMapId != s->mapId())
-				{
-					// first node, set high variance to make rtabmap trigger a new map
-					infMatrix /= 9999.0;
-					UDEBUG("First node of map %d, variance set to 9999", s->mapId());
-				}
 				else
 				{
-					if(_previousInfMatrix.empty())
+					if(_previousMapId != s->mapId())
 					{
-						_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+						// first node, set high variance to make rtabmap trigger a new map
+						infMatrix /= 9999.0;
+						UDEBUG("First node of map %d, variance set to 9999", s->mapId());
+					}
+					else
+					{
+						// if localization data saved in database, covariance will be set in a prior link
+						_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
+						if(links.size())
+						{
+							// assume the first is the backward neighbor, take its variance
+							infMatrix = links.begin()->second.infMatrix();
+							_previousInfMatrix = infMatrix;
+						}
+						else
+						{
+							if(_previousInfMatrix.empty())
+							{
+								_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+							}
+							// we have a node not linked to map, use last variance
+							infMatrix = _previousInfMatrix;
+						}
 					}
-					// we have a node not linked to map, use last variance
-					infMatrix = _previousInfMatrix;
 				}
 				_previousMapId = s->mapId();
 			}
@@ -530,7 +575,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			data.setId(seq);
 			data.setStamp(s->getStamp());
 			data.setGroundTruth(s->getGroundTruthPose());
-			if(globalPose.isNull())
+			if(!globalPose.isNull())
 			{
 				data.setGlobalPose(globalPose, globalPoseCov);
 			}
@@ -558,13 +603,14 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			cv::Mat descriptors = s->getWordsDescriptors().clone();
 			const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
 			const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
-			if(!keypoints.empty() &&
+			if(!_featuresIgnored &&
+				!keypoints.empty() &&
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
 			{
 				data.setFeatures(keypoints, keypoints3D, descriptors);
 			}
-			else if(!keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
+			else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
 			{
 				UERROR("Missing feature data, features won't be published.");
 			}
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -792,7 +792,9 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 	std::vector<cv::Point3f> keypoints3D;
 	if(keypoints.size())
 	{
-		if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
+		if(!data.rightRaw().empty() && !data.imageRaw().empty() &&
+			!data.stereoCameraModels().empty() &&
+			data.stereoCameraModels()[0].isValidForProjection())
 		{
 			//stereo
 			cv::Mat imageMono;
@@ -808,22 +810,121 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(

 			std::vector<cv::Point2f> leftCorners;
 			cv::KeyPoint::convert(keypoints, leftCorners);
-			std::vector<unsigned char> status;

 			std::vector<cv::Point2f> rightCorners;
-			rightCorners = _stereo->computeCorrespondences(
-					imageMono,
-					data.rightRaw(),
-					leftCorners,
-					status);

-			keypoints3D = util3d::generateKeypoints3DStereo(
-					leftCorners,
-					rightCorners,
-					data.stereoCameraModel(),
-					status,
-					_minDepth,
-					_maxDepth);
+			if(data.stereoCameraModels().size() == 1)
+			{
+				std::vector<unsigned char> status;
+				rightCorners = _stereo->computeCorrespondences(
+						imageMono,
+						data.rightRaw(),
+						leftCorners,
+						status);
+
+				if(ULogger::level() >= ULogger::kWarning)
+				{
+					int rejected = 0;
+					for(size_t i=0; i<status.size(); ++i)
+					{
+						if(status[i]==0)
+						{
+							++rejected;
+						}
+					}
+					if(rejected > (int)status.size()/2)
+					{
+						UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
+								"Optical flow may have failed because images are not calibrated, "
+								"the background is too far (no disparity between the images), "
+								"maximum disparity may be too small (%f) or that exposure between "
+								"left and right images is too different.",
+								rejected,
+								(int)status.size(),
+								_stereo->maxDisparity());
+					}
+				}
+
+				keypoints3D = util3d::generateKeypoints3DStereo(
+						leftCorners,
+						rightCorners,
+						data.stereoCameraModels()[0],
+						status,
+						_minDepth,
+						_maxDepth);
+			}
+			else
+			{
+				int subImageWith = imageMono.cols / data.stereoCameraModels().size();
+				UASSERT(imageMono.cols % subImageWith == 0);
+				std::vector<std::vector<cv::Point2f> > subLeftCorners(data.stereoCameraModels().size());
+				std::vector<std::vector<int> > subIndex(data.stereoCameraModels().size());
+				// Assign keypoints per camera
+				for(size_t i=0; i<leftCorners.size(); ++i)
+				{
+					int cameraIndex = int(leftCorners[i].x / subImageWith);
+					leftCorners[i].x -= cameraIndex*subImageWith;
+					subLeftCorners[cameraIndex].push_back(leftCorners[i]);
+					subIndex[cameraIndex].push_back(i);
+				}
+
+				keypoints3D.resize(keypoints.size());
+				int total = 0;
+				int rejected = 0;
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+				{
+					if(!subLeftCorners[i].empty())
+					{
+						std::vector<unsigned char> status;
+						rightCorners = _stereo->computeCorrespondences(
+								imageMono.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+								data.rightRaw().colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+								subLeftCorners[i],
+								status);
+
+						std::vector<cv::Point3f> subKeypoints3D = util3d::generateKeypoints3DStereo(
+								subLeftCorners[i],
+								rightCorners,
+								data.stereoCameraModels()[i],
+								status,
+								_minDepth,
+								_maxDepth);
+
+						if(ULogger::level() >= ULogger::kWarning)
+						{
+							for(size_t i=0; i<status.size(); ++i)
+							{
+								if(status[i]==0)
+								{
+									++rejected;
+								}
+							}
+							total+=status.size();
+						}
+
+						UASSERT(subIndex[i].size() == subKeypoints3D.size());
+						for(size_t j=0; j<subKeypoints3D.size(); ++j)
+						{
+							keypoints3D[subIndex[i][j]] = subKeypoints3D[j];
+						}
+					}
+				}
+
+				if(ULogger::level() >= ULogger::kWarning)
+				{
+					if(rejected > total/2)
+					{
+						UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
+								"Optical flow may have failed because images are not calibrated, "
+								"the background is too far (no disparity between the images), "
+								"maximum disparity may be too small (%f) or that exposure between "
+								"left and right images is too different.",
+								rejected,
+								total,
+								_stereo->maxDisparity());
+					}
+				}
+			}
 		}
 		else if(!data.depthRaw().empty() && data.cameraModels().size())
 		{
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -56,7 +56,7 @@ bool exportPoses(
 		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & constraints, // required for formats 3 and 4
-		const std::map<int, double> & stamps, // required for format 1
+		const std::map<int, double> & stamps, // required for format 1, 10 and 11
 		const ParametersMap & parameters) // optional for formats 3 and 4
 {
 	UDEBUG("%s", filePath.c_str());
@@ -130,11 +130,11 @@ bool exportPoses(
 						// header
 						if(format == 11)
 						{
-							fprintf(fout, "# timestamp x y z qx qy qz qw id\n");
+							fprintf(fout, "#timestamp x y z qx qy qz qw id\n");
 						}
 						else
 						{
-							fprintf(fout, "# timestamp x y z qx qy qz qw\n");
+							fprintf(fout, "#timestamp x y z qx qy qz qw\n");
 						}
 					}

@@ -523,7 +523,10 @@ bool exportGPS(
 			std::string values;
 			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
 			{
-				values += uFormat("%f,%f,%f ", iter->second.longitude(), iter->second.latitude(), iter->second.altitude());
+				values += uFormat("%s,%s,%s ",
+                                  uReplaceChar(uNumber2Str(iter->second.longitude(), 8, true), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.latitude(), 8, true), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.altitude(), 8, true), ',', '.').c_str());
 			}

 			// switch argb (Qt format) -> abgr
@@ -577,13 +580,13 @@ bool exportGPS(
 			fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
 			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
 			{
-				fprintf(fout, "%f %f %f %f %f %f\n",
-						iter->second.stamp(),
-						iter->second.longitude(),
-						iter->second.latitude(),
-						iter->second.altitude(),
-						iter->second.error(),
-						iter->second.bearing());
+				fprintf(fout, "%f %.*f %.*f %.*f %.*f %.*f\n",
+				    iter->second.stamp(),
+				    8, iter->second.longitude(),
+				    8, iter->second.latitude(),
+				    8, iter->second.altitude(),
+				    8, iter->second.error(),
+				    8, iter->second.bearing());
 			}
 		}

--- a/corelib/src/MarkerDetector.cpp
+++ b/corelib/src/MarkerDetector.cpp
@@ -39,12 +39,16 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters)
 	maxRange_ = Parameters::defaultMarkerMaxRange();
 	minRange_ = Parameters::defaultMarkerMinRange();
 	dictionaryId_ = Parameters::defaultMarkerDictionary();
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    detectorParams_.reset(new cv::aruco::DetectorParameters());
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	detectorParams_ = cv::aruco::DetectorParameters::create();
 #else
 	detectorParams_.reset(new cv::aruco::DetectorParameters());
 #endif
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod();
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
 	detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod();
 #else
 	detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0;
@@ -70,7 +74,11 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 	detectorParams_->minCornerDistanceRate = 0.05;
 	detectorParams_->minDistanceToBorder = 3;
 	detectorParams_->minMarkerDistanceRate = 0.05;
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    int cornerRefinementMethod;
+    Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), cornerRefinementMethod);
+    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod)cornerRefinementMethod;
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
 	Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
 #else
 	int doCornerRefinement = detectorParams_->doCornerRefinement?1:0;
@@ -103,7 +111,10 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 		dictionaryId_ = Parameters::defaultMarkerDictionary();
 	}
 #endif
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    dictionary_.reset(new cv::aruco::Dictionary());
+    *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
 #else
 	dictionary_.reset(new cv::aruco::Dictionary());
@@ -130,12 +141,70 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
    return detections;
 }

+std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
+                           const std::vector<CameraModel> & models,
+                           const cv::Mat & depth,
+                           const std::map<int, float> & markerLengths,
+                           cv::Mat * imageWithDetections)
+{
+	UASSERT(!models.empty() && !image.empty());
+	UASSERT(int((image.cols/models.size())*models.size()) == image.cols);
+	UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols);
+	int subRGBWidth = image.cols/models.size();
+	int subDepthWidth = depth.cols/models.size();
+
+	std::map<int, MarkerInfo> allInfo;
+	for(size_t i=0; i<models.size(); ++i)
+	{
+		cv::Mat subImage(image, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows));
+		cv::Mat subDepth;
+		if(!depth.empty())
+			subDepth = cv::Mat(depth, cv::Rect(subDepthWidth*i, 0, subDepthWidth, depth.rows));
+		CameraModel model = models[i];
+		cv::Mat subImageWithDetections;
+		std::map<int, MarkerInfo> subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0);
+		if(ULogger::level() >= ULogger::kWarning)
+		{
+			for(std::map<int, MarkerInfo>::iterator iter=subInfo.begin(); iter!=subInfo.end(); ++iter)
+			{
+				std::pair<std::map<int, MarkerInfo>::iterator, bool> inserted = allInfo.insert(*iter);
+				if(!inserted.second)
+				{
+					UWARN("Marker %d already added by another camera, ignoring detection from camera %d", iter->first, i);
+				}
+			}
+		}
+		else
+		{
+			allInfo.insert(subInfo.begin(), subInfo.end());
+		}
+		if(imageWithDetections)
+		{
+			if(i==0)
+			{
+				*imageWithDetections = image.clone();
+			}
+			if(!subImageWithDetections.empty())
+			{
+				subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows)));
+			}
+		}
+	}
+	return allInfo;
+}
+
 std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
                           const CameraModel & model,
                           const cv::Mat & depth,
                           const std::map<int, float> & markerLengths,
                           cv::Mat * imageWithDetections)
 {
+	if(!image.empty() && image.cols != model.imageWidth())
+	{
+		UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");
+		return std::map<int, MarkerInfo>();
+	}
+
    std::map<int, MarkerInfo> detections;

 #ifdef HAVE_OPENCV_ARUCO
@@ -257,7 +326,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 								 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
 				Transform pose = model.localTransform() * t;
 				detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
-				UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
+				UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str(), model.localTransform().prettyPrint().c_str());
 			}
 		}
 		if(markerLength_ == 0 && !scales.empty())
@@ -304,7 +373,11 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
                std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
                if(iter!=detections.end())
                {
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1)))
+                    cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
+#else
                    cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
+#endif
                }
 			}
 		}
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -101,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
+	_invertedReg(Parameters::defaultRGBDInvertedReg()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
@@ -122,14 +123,22 @@ Memory::Memory(const ParametersMap & parameters) :
 	_linksChanged(false),
 	_signaturesAdded(0),
 	_allNodesInWM(true),
-
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
 	_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
-	_parallelized(Parameters::defaultKpParallelized())
+	_parallelized(Parameters::defaultKpParallelized()),
+	_registrationVis(0)
 {
 	_feature2D = Feature2D::create(parameters);
 	_vwd = new VWDictionary(parameters);
 	_registrationPipeline = Registration::create(parameters);
+	if(!_registrationPipeline->isImageRequired())
+	{
+		// make sure feature matching is used instead of optical flow to compute the guess
+		ParametersMap tmp = parameters;
+		uInsert(tmp, ParametersPair(Parameters::kVisCorType(), "0"));
+		uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+		_registrationVis = new RegistrationVis(tmp);
+	}

 	// for local scan matching, correspondences ratio should be two times higher as we expect more matches
 	float corRatio = Parameters::defaultIcpCorrespondenceRatio();
@@ -531,6 +540,7 @@ Memory::~Memory()
 	delete _vwd;
 	delete _registrationPipeline;
 	delete _registrationIcpMulti;
+	delete _registrationVis;
 	delete _occupancy;
 }

@@ -569,6 +579,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
 	Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
+	Parameters::parse(params, Parameters::kRGBDInvertedReg(), _invertedReg);
+	if(_invertedReg && _localBundleOnLoopClosure)
+	{
+		UWARN("%s and %s cannot be used at the same time, disabling %s...",
+				Parameters::kRGBDLocalBundleOnLoopClosure().c_str(),
+				Parameters::kRGBDInvertedReg().c_str(),
+				Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
+		_localBundleOnLoopClosure = false;
+	}
 	Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
 	Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
@@ -647,12 +666,28 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
 	uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));

+	Registration::Type currentStrategy = Registration::kTypeUndef;
+	if(_registrationPipeline)
+	{
+		if(_registrationPipeline->isImageRequired() && _registrationPipeline->isScanRequired())
+		{
+			currentStrategy = Registration::kTypeVisIcp;
+		}
+		else if(_registrationPipeline->isImageRequired())
+		{
+			currentStrategy = Registration::kTypeVis;
+		}
+		else if(_registrationPipeline->isScanRequired())
+		{
+			currentStrategy = Registration::kTypeIcp;
+		}
+	}
 	Registration::Type regStrategy = Registration::kTypeUndef;
 	if((iter=params.find(Parameters::kRegStrategy())) != params.end())
 	{
 		regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
 	}
-	if(regStrategy!=Registration::kTypeUndef)
+	if(regStrategy!=Registration::kTypeUndef && regStrategy != currentStrategy)
 	{
 		UDEBUG("new registration strategy %d", int(regStrategy));
 		if(_registrationPipeline)
@@ -662,10 +697,29 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		}

 		_registrationPipeline = Registration::create(regStrategy, parameters_);
+
+		if(!_registrationPipeline->isImageRequired() && _registrationVis == 0)
+		{
+			ParametersMap tmp = params;
+			uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+			_registrationVis = new RegistrationVis(tmp);
+		}
+		else if(_registrationPipeline->isImageRequired() && _registrationVis)
+		{
+			delete _registrationVis;
+			_registrationVis = 0;
+		}
 	}
 	else if(_registrationPipeline)
 	{
 		_registrationPipeline->parseParameters(params);
+
+		if(_registrationVis)
+		{
+			ParametersMap tmp = params;
+			uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+			_registrationVis->parseParameters(tmp);
+		}
 	}

 	if(_registrationIcpMulti)
@@ -1768,7 +1822,7 @@ void Memory::clear()
 	_linksChanged = false;
 	_gpsOrigin = GPS();
 	_rectCameraModels.clear();
-	_rectStereoCameraModel = StereoCameraModel();
+	_rectStereoCameraModels.clear();
 	_odomMaxInf.clear();
 	_groundTruths.clear();
 	_labels.clear();
@@ -2811,8 +2865,21 @@ Transform Memory::computeTransform(
 		(fromS.getWords().size() && toS.getWords().size()) ||
 		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
 	{
-		Signature tmpFrom = fromS;
-		Signature tmpTo = toS;
+		Signature tmpFrom, tmpTo;
+		if(_invertedReg)
+		{
+			tmpFrom = toS;
+			tmpTo = fromS;
+			if(!guess.isNull())
+			{
+				guess = guess.inverse();
+			}
+		}
+		else
+		{
+			tmpFrom = fromS;
+			tmpTo = toS;
+		}

 		if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
 		{
@@ -2835,12 +2902,8 @@ Transform Memory::computeTransform(
 		{
 			UDEBUG("");
 			// no visual in the pipeline, make visual registration for guess
-			// make sure feature matching is used instead of optical flow to compute the guess
-			ParametersMap parameters = parameters_;
-			uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
-			uInsert(parameters, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
-			RegistrationVis regVis(parameters);
-			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			UASSERT(_registrationVis!=0);
+			guess = _registrationVis->computeTransformation(tmpFrom, tmpTo, guess, info);
 			if(!guess.isNull())
 			{
 				transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
@@ -2851,6 +2914,7 @@ Transform Memory::computeTransform(
 				_registrationPipeline->isImageRequired() &&
 			   !_registrationPipeline->isScanRequired() &&
 			   !_registrationPipeline->isUserDataRequired() &&
+			   !_invertedReg &&
 			   !tmpTo.getWordsDescriptors().empty() &&
 			   !tmpTo.getWords().empty() &&
 			   !tmpFrom.getWordsDescriptors().empty() &&
@@ -2923,7 +2987,7 @@ Transform Memory::computeTransform(
 				}
 				std::map<int, Transform> bundlePoses;
 				std::multimap<int, Link> bundleLinks;
-				std::map<int, CameraModel> bundleModels;
+				std::map<int, std::vector<CameraModel> > bundleModels;
 				std::map<int, std::map<int, FeatureBA> > wordReferences;

 				std::multimap<int, Link> links = fromS.getLinks();
@@ -2949,28 +3013,32 @@ Transform Memory::computeTransform(
 						}
 						if(s)
 						{
-							CameraModel model;
-							if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
+							std::vector<CameraModel> models;
+							if(s->sensorData().cameraModels().size() >= 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
 							{
-								model = s->sensorData().cameraModels()[0];
+								models = s->sensorData().cameraModels();
 							}
-							else if(s->sensorData().stereoCameraModel().isValidForProjection())
+							else if(s->sensorData().stereoCameraModels().size() >= 1 && s->sensorData().stereoCameraModels().at(0).isValidForProjection())
 							{
-								model = s->sensorData().stereoCameraModel().left();
-								// Set Tx for stereo BA
-								model = CameraModel(model.fx(),
-										model.fy(),
-										model.cx(),
-										model.cy(),
-										model.localTransform(),
-										-s->sensorData().stereoCameraModel().baseline()*model.fx());
+								for(size_t i=0; i<s->sensorData().stereoCameraModels().size(); ++i)
+								{
+									CameraModel model = s->sensorData().stereoCameraModels()[i].left();
+									// Set Tx for stereo BA
+									model = CameraModel(model.fx(),
+											model.fy(),
+											model.cx(),
+											model.cy(),
+											model.localTransform(),
+											-s->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
+											model.imageSize());
+									models.push_back(model);
+								}
 							}
 							else
 							{
 								UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
 							}
-							bundleModels.insert(std::make_pair(id, model));
-							Transform invLocalTransform = model.localTransform().inverse();
+							bundleModels.insert(std::make_pair(id, models));
 							UASSERT(iter->second.isValid() || iter->first == fromS.id());

 							if(iter->second.transform().isNull())
@@ -2990,16 +3058,27 @@ Transform Memory::computeTransform(
 								if(points3DMap.find(jter->first)!=points3DMap.end() &&
 										(id == tmpTo.id() || jter->first > 0)) // Since we added negative words of "from", only accept matches with current frame
 								{
+									cv::KeyPoint kpts = s->getWordsKpts()[jter->second];
+									int cameraIndex = 0;
+									if(models.size()>1)
+									{
+										UASSERT(models[0].imageWidth()>0);
+										float subImageWidth = models[0].imageWidth();
+										cameraIndex = int(kpts.pt.x / subImageWidth);
+										kpts.pt.x = kpts.pt.x - (subImageWidth*float(cameraIndex));
+									}
+
 									//get depth
 									float d = 0.0f;
 									if( !s->getWords3().empty() &&
 										util3d::isFinite(s->getWords3()[jter->second]))
 									{
 										//move back point in camera frame (to get depth along z)
+										Transform invLocalTransform = models[cameraIndex].localTransform().inverse();
 										d = util3d::transformPoint(s->getWords3()[jter->second], invLocalTransform).z;
 									}
 									wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
-									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(s->getWordsKpts()[jter->second], d)));
+									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(kpts, d, cv::Mat(), cameraIndex)));
 									++totalWordReferences;
 								}
 							}
@@ -3060,6 +3139,10 @@ Transform Memory::computeTransform(
 		{
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 		}
+		if(_invertedReg && !transform.isNull())
+		{
+			transform = transform.inverse();
+		}
 	}
 	return transform;
 }
@@ -4070,19 +4153,19 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,

 void Memory::getNodeCalibration(int nodeId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel) const
+		std::vector<StereoCameraModel> & stereoModels) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
 	if(s)
 	{
 		models = s->sensorData().cameraModels();
-		stereoModel = s->sensorData().stereoCameraModel();
+		stereoModels = s->sensorData().stereoCameraModels();
 	}
 	else if(_dbDriver)
 	{
 		// load from database
-		_dbDriver->getCalibration(nodeId, models, stereoModel);
+		_dbDriver->getCalibration(nodeId, models, stereoModels);
 	}
 }

@@ -4405,15 +4488,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						CV_16UC1, CV_32FC1, CV_8UC1).c_str());

 	if(!data.depthOrRightRaw().empty() &&
-		data.cameraModels().size() == 0 &&
-		!data.stereoCameraModel().isValidForProjection() &&
+		data.cameraModels().empty() &&
+		data.stereoCameraModels().empty() &&
 		!pose.isNull())
 	{
-		UERROR("Camera calibration not valid, calibrate your camera!");
-		if(data.cameraModels().empty())
-			std::cout << data.stereoCameraModel() << std::endl;
-		else
-			std::cout << data.cameraModels()[0] << std::endl;
+		UERROR("No camera calibration found, calibrate your camera!");
 		return 0;
 	}
 	UASSERT(_feature2D != 0);
@@ -4464,6 +4543,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		// we assume that once rtabmap is receiving data, the calibration won't change over time
 		if(data.cameraModels().size())
 		{
+			UDEBUG("Monocular rectification");
 			// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
 			UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
 			int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
@@ -4505,25 +4585,58 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			data.setRGBDImage(rectifiedImages, data.depthOrRightRaw(), data.cameraModels());
 		}
-		else if(data.stereoCameraModel().isValidForRectification())
+		else if(data.stereoCameraModels().size())
 		{
-			if(!_rectStereoCameraModel.isValidForRectification())
+			UDEBUG("Stereo rectification");
+			UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
+			int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
+			UASSERT(subImageWidth == data.rightRaw().cols/(int)data.stereoCameraModels().size());
+			cv::Mat rectifiedLefts(data.imageRaw().size(), data.imageRaw().type());
+			cv::Mat rectifiedRights(data.rightRaw().size(), data.rightRaw().type());
+			bool initRectMaps = _rectStereoCameraModels.empty();
+			if(initRectMaps)
 			{
-				_rectStereoCameraModel = data.stereoCameraModel();
-				if(!_rectStereoCameraModel.isRectificationMapInitialized())
+				_rectStereoCameraModels.resize(data.stereoCameraModels().size());
+			}
+
+			for(unsigned int i=0; i<data.stereoCameraModels().size(); ++i)
+			{
+				if(data.stereoCameraModels()[i].isValidForRectification())
 				{
-					UWARN("Initializing rectification maps (only done for the first image received)...");
-					_rectStereoCameraModel.initRectificationMap();
-					UWARN("Initializing rectification maps (only done for the first image received)...done!");
+					if(initRectMaps)
+					{
+						_rectStereoCameraModels[i] = data.stereoCameraModels()[i];
+						if(!_rectStereoCameraModels[i].isRectificationMapInitialized())
+						{
+							UWARN("Initializing rectification maps (only done for the first image received)...");
+							_rectStereoCameraModels[i].initRectificationMap();
+							UWARN("Initializing rectification maps (only done for the first image received)...done!");
+						}
+					}
+					UASSERT(_rectStereoCameraModels[i].left().imageWidth() == data.stereoCameraModels()[i].left().imageWidth());
+					UASSERT(_rectStereoCameraModels[i].left().imageHeight() == data.stereoCameraModels()[i].left().imageHeight());
+
+					cv::Mat rectifiedLeft = _rectStereoCameraModels[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					cv::Mat rectifiedRight = _rectStereoCameraModels[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
+					rectifiedLeft.copyTo(cv::Mat(rectifiedLefts, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					rectifiedRight.copyTo(cv::Mat(rectifiedRights, cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
+					imagesRectified = true;
+				}
+				else
+				{
+					UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
+						"full calibration. If images are already rectified, set %s parameter back to true.",
+						(int)i,
+						Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					std::cout << data.stereoCameraModels()[i] << std::endl;
+					return 0;
 				}
 			}
-			UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
-			UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
+
 			data.setStereoImage(
-					_rectStereoCameraModel.left().rectifyImage(data.imageRaw()),
-					_rectStereoCameraModel.right().rectifyImage(data.rightRaw()),
-					data.stereoCameraModel());
-			imagesRectified = true;
+					rectifiedLefts,
+					rectifiedRights,
+					data.stereoCameraModels());
 		}
 		else
 		{
@@ -4596,17 +4709,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 							util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
 							cameraModels);
 				}
-				else
+
+				std::vector<StereoCameraModel> stereoCameraModels = decimatedData.stereoCameraModels();
+				for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
+				{
+					stereoCameraModels[i].scale(1.0/double(_imagePreDecimation));
+				}
+				if(!stereoCameraModels.empty())
 				{
-					StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
-					if(stereoModel.isValidForProjection())
-					{
-						stereoModel.scale(1.0/double(_imagePreDecimation));
-					}
 					decimatedData.setStereoImage(
 							util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
 							util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation),
-							stereoModel);
+							stereoCameraModels);
 				}
 			}

@@ -4845,7 +4959,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					keypoints3D = data.keypoints3D();
 				}
 				else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
 				{
 					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
 					t = timer.ticks();
@@ -5050,7 +5164,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

 		if(keypoints3D.empty() &&
 			((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
-		   (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
+		   (!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
 		{
 			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
 		}
@@ -5223,40 +5337,37 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("Detecting markers...");
 		if(landmarks.empty())
 		{
-			std::map<int, MarkerInfo> markers;
-			if(!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection())
+			std::vector<CameraModel> models = data.cameraModels();
+			if(models.empty())
 			{
-				if(data.cameraModels().size() > 1)
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
 				{
-					static bool warned = false;
-					if(!warned)
+					models.push_back(data.stereoCameraModels()[i].left());
+				}
+			}
+
+			if(!models.empty() && models[0].isValidForProjection())
+			{
+				std::map<int, MarkerInfo> markers = _markerDetector->detect(data.imageRaw(), models, data.depthRaw(), _landmarksSize);
+
+				for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
+				{
+					if(iter->first <= 0)
 					{
-						UWARN("Detecting markers in multi-camera setup is not yet implemented, aborting marker detection. This message is only printed once.");
+						UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
+						continue;
 					}
-					warned = true;
-				}
-				else
-				{
-					markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0], data.depthRaw(), _landmarksSize);
+					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+					covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
+					covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+					landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
 				}
+				UDEBUG("Markers detected = %d", (int)markers.size());
 			}
-			else if(data.stereoCameraModel().isValidForProjection())
+			else
 			{
-				markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModel().left(), cv::Mat(), _landmarksSize);
+				UWARN("No valid camera calibration for marker detection");
 			}
-			for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
-			{
-				if(iter->first <= 0)
-				{
-					UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
-					continue;
-				}
-				cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-				covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
-				covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
-				landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
-			}
-			UDEBUG("Markers detected = %d", (int)markers.size());
 		}
 		else
 		{
@@ -5270,7 +5381,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	cv::Mat image = data.imageRaw();
 	cv::Mat depthOrRightImage = data.depthOrRightRaw();
 	std::vector<CameraModel> cameraModels = data.cameraModels();
-	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
+	std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();

 	// apply decimation?
 	if(_imagePostDecimation > 1 && !isIntermediateNode)
@@ -5280,7 +5391,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			image = decimatedData.imageRaw();
 			depthOrRightImage = decimatedData.depthOrRightRaw();
 			cameraModels = decimatedData.cameraModels();
-			stereoCameraModel = decimatedData.stereoCameraModel();
+			stereoCameraModels = decimatedData.stereoCameraModels();
 		}
 		else
 		{
@@ -5308,9 +5419,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
 			}
-			if(stereoCameraModel.isValidForProjection())
+			for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
 			{
-				stereoCameraModel.scale(1.0/double(_imagePostDecimation));
+				stereoCameraModels[i].scale(1.0/double(_imagePostDecimation));
 			}
 		}

@@ -5558,7 +5669,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			"",
 			pose,
 			data.groundTruth(),
-			stereoCameraModel.isValidForProjection()?
+			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
 							LaserScan(compressedScan,
@@ -5576,7 +5687,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.localTransform()),
 						compressedImage,
 						compressedDepth,
-						stereoCameraModel,
+						stereoCameraModels,
 						id,
 						0,
 						compressedUserData):
@@ -5642,7 +5753,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			"",
 			pose,
 			data.groundTruth(),
-			stereoCameraModel.isValidForProjection()?
+			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
 								LaserScan(compressedScan,
@@ -5660,7 +5771,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 									laserScan.localTransform()),
 						cv::Mat(),
 						cv::Mat(),
-						stereoCameraModel,
+						stereoCameraModels,
 						id,
 						0,
 						compressedUserData):
@@ -5698,7 +5809,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}
 	else
 	{
-		s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModel, false);
+		s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModels, false);
 	}
 	s->sensorData().setLaserScan(laserScan, false);
 	s->sensorData().setUserData(data.userDataRaw(), false);
@@ -5774,7 +5885,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				UINFO("Added GPS origin: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
 			}
 			cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
-			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
+			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*M_PI/180.0);
 			cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999

 			UDEBUG("Added GPS prior: x=%f y=%f z=%f yaw=%f", gpsPose.x(), gpsPose.y(), gpsPose.z(), gpsPose.theta());
--- a/corelib/src/OccupancyGrid.cpp
+++ b/corelib/src/OccupancyGrid.cpp
@@ -430,8 +430,25 @@ void OccupancyGrid::createLocalMap(
 			}
 			else
 			{
-				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
-				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+				// average of all local transforms
+				float sum = 0;
+				for(unsigned int i=0; i<node.sensorData().stereoCameraModels().size(); ++i)
+				{
+					const Transform & t = node.sensorData().stereoCameraModels()[i].localTransform();
+					if(!t.isNull())
+					{
+						viewPoint.x += t.x();
+						viewPoint.y += t.y();
+						viewPoint.z += t.z();
+						sum += 1.0f;
+					}
+				}
+				if(sum > 0.0f)
+				{
+					viewPoint.x /= sum;
+					viewPoint.y /= sum;
+					viewPoint.z /= sum;
+				}
 			}

 			cv::Mat scanGroundCells;
@@ -753,7 +770,7 @@ void OccupancyGrid::addToCache(
 		const cv::Mat & obstacles,
 		const cv::Mat & empty)
 {
-	UDEBUG("nodeId=%d", nodeId);
+	UDEBUG("nodeId=%d (ground=%d obstacles=%d empty=%d)", nodeId, ground.cols, obstacles.cols, empty.cols);
 	if(nodeId < 0)
 	{
 		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
@@ -1009,6 +1026,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)

 		if(!cache_.empty())
 		{
+			UDEBUG("Updating from cache");
 			for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 			{
 				if(uContains(cache_, iter->first))
@@ -1018,14 +1036,18 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);

 					//ground
+					cv::Mat ground;
+					if(pair.first.first.cols || pair.second.cols)
+					{
+						ground = cv::Mat(1, pair.first.first.cols+pair.second.cols, CV_32FC2);
+					}
 					if(pair.first.first.cols)
 					{
 						if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
 						{
 							UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
 						}
-						cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
-						for(int i=0; i<ground.cols; ++i)
+						for(int i=0; i<pair.first.first.cols; ++i)
 						{
 							const float * vi = pair.first.first.ptr<float>(0,i);
 							float * vo = ground.ptr<float>(0,i);
@@ -1050,7 +1072,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							else if(maxY < vo[1])
 								maxY = vo[1];
 						}
-						uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));

 						if(cloudAssembling_)
 						{
@@ -1066,11 +1087,10 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 						{
 							UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
 						}
-						cv::Mat ground(1, pair.second.cols, CV_32FC2);
-						for(int i=0; i<ground.cols; ++i)
+						for(int i=0; i<pair.second.cols; ++i)
 						{
 							const float * vi = pair.second.ptr<float>(0,i);
-							float * vo = ground.ptr<float>(0,i);
+							float * vo = ground.ptr<float>(0,i+pair.first.first.cols);
 							cv::Point3f vt;
 							if(pair.second.channels() != 2 && pair.second.channels() != 5)
 							{
@@ -1092,7 +1112,6 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							else if(maxY < vo[1])
 								maxY = vo[1];
 						}
-						uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));

 						if(cloudAssembling_)
 						{
@@ -1100,6 +1119,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							assembledEmptyCellsUpdated = true;
 						}
 					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));

 					//obstacles
 					if(pair.first.second.cols)
@@ -1229,205 +1249,208 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 				}
 				for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
 				{
-					if(kter->first > 0)
-					{
-						uInsert(addedNodes_, *kter);
-					}
 					std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
 					std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
-					std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
-					if(cter == cellCount_.end() && kter->first > 0)
+					if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
 					{
-						cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
-					}
-					if(iter!=emptyLocalMaps.end())
-					{
-						for(int i=0; i<iter->second.cols; ++i)
+						if(kter->first > 0)
 						{
-							float * ptf = iter->second.ptr<float>(0,i);
-							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-							UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
-									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
-											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
-							if(value != -2 && (!incrementalGraphUpdate || value==-1))
-							{
-								float * info = mapInfo.ptr<float>(pt.y, pt.x);
-								int nodeId = (int)info[0];
-								if(value != -1)
-								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
-									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId > 0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = ptf[0];
-									info[2] = ptf[1];
-									cter->second.first+=1;
-								}
-								value = 0; // free space
-
-								// update odds
-								if(nodeId != kter->first)
-								{
-									info[3] += probMiss_;
-									if (info[3] < probClampingMin_)
-									{
-										info[3] = probClampingMin_;
-									}
-									if (info[3] > probClampingMax_)
-									{
-										info[3] = probClampingMax_;
-									}
-								}
-							}
+							uInsert(addedNodes_, *kter);
 						}
-					}
-
-					if(footprintRadius_ >= cellSize_*1.5f)
-					{
-						// place free space under the footprint of the robot
-						cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
-						cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
-						if(ptBegin.x < 0)
-							ptBegin.x = 0;
-						if(ptEnd.x >= map.cols)
-							ptEnd.x = map.cols-1;
-
-						if(ptBegin.y < 0)
-							ptBegin.y = 0;
-						if(ptEnd.y >= map.rows)
-							ptEnd.y = map.rows-1;
-
-						for(int i=ptBegin.x; i<ptEnd.x; ++i)
+						std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
+						if(cter == cellCount_.end() && kter->first > 0)
 						{
-							for(int j=ptBegin.y; j<ptEnd.y; ++j)
-							{
-								UASSERT(j < map.rows && i < map.cols);
-								char & value = map.at<char>(j, i);
-								float * info = mapInfo.ptr<float>(j, i);
-								int nodeId = (int)info[0];
-								if(value != -1)
-								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
-									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
-										{
-											eter->second.first -= 1;
-										}
-										else if(value == 100)
-										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.first += 1;
-										}
-									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = float(i) * cellSize_ + xMin;
-									info[2] = float(j) * cellSize_ + yMin;
-									info[3] = probClampingMin_;
-									cter->second.first+=1;
-								}
-								value = -2; // free space (footprint)
-							}
+							cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
 						}
-					}
-
-					if(jter!=occupiedLocalMaps.end())
-					{
-						for(int i=0; i<jter->second.cols; ++i)
+						if(iter!=emptyLocalMaps.end())
 						{
-							float * ptf = jter->second.ptr<float>(0,i);
-							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-							UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+							for(int i=0; i<iter->second.cols; ++i)
+							{
+								float * ptf = iter->second.ptr<float>(0,i);
+								cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+								UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
 										uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
-												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
-							char & value = map.at<char>(pt.y, pt.x);
-							if(value != -2)
+												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
+								char & value = map.at<char>(pt.y, pt.x);
+								if(value != -2 && (!incrementalGraphUpdate || value==-1))
+								{
+									float * info = mapInfo.ptr<float>(pt.y, pt.x);
+									int nodeId = (int)info[0];
+									if(value != -1)
+									{
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+										{
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
+										}
+										if(nodeId > 0)
+										{
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.first += 1;
+											}
+										}
+									}
+									if(kter->first > 0)
+									{
+										info[0] = (float)kter->first;
+										info[1] = ptf[0];
+										info[2] = ptf[1];
+										cter->second.first+=1;
+									}
+									value = 0; // free space
+
+									// update odds
+									if(nodeId != kter->first)
+									{
+										info[3] += probMiss_;
+										if (info[3] < probClampingMin_)
+										{
+											info[3] = probClampingMin_;
+										}
+										if (info[3] > probClampingMax_)
+										{
+											info[3] = probClampingMax_;
+										}
+									}
+								}
+							}
+						}
+
+						if(footprintRadius_ >= cellSize_*1.5f)
+						{
+							// place free space under the footprint of the robot
+							cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
+							cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
+							if(ptBegin.x < 0)
+								ptBegin.x = 0;
+							if(ptEnd.x >= map.cols)
+								ptEnd.x = map.cols-1;
+
+							if(ptBegin.y < 0)
+								ptBegin.y = 0;
+							if(ptEnd.y >= map.rows)
+								ptEnd.y = map.rows-1;
+
+							for(int i=ptBegin.x; i<ptEnd.x; ++i)
 							{
-								float * info = mapInfo.ptr<float>(pt.y, pt.x);
-								int nodeId = (int)info[0];
-								if(value != -1)
+								for(int j=ptBegin.y; j<ptEnd.y; ++j)
 								{
-									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+									UASSERT(j < map.rows && i < map.cols);
+									char & value = map.at<char>(j, i);
+									float * info = mapInfo.ptr<float>(j, i);
+									int nodeId = (int)info[0];
+									if(value != -1)
 									{
-										// cannot rewrite on cells referred by more recent nodes
-										continue;
-									}
-									if(nodeId>0)
-									{
-										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
-										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
-										if(value == 0)
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
 										{
-											eter->second.first -= 1;
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
 										}
-										else if(value == 100)
+										if(nodeId>0)
 										{
-											eter->second.second -= 1;
-										}
-										if(kter->first < 0)
-										{
-											eter->second.second += 1;
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.first += 1;
+											}
 										}
 									}
-								}
-								if(kter->first > 0)
-								{
-									info[0] = (float)kter->first;
-									info[1] = ptf[0];
-									info[2] = ptf[1];
-									cter->second.second+=1;
-								}
-
-								// update odds
-								if(nodeId != kter->first || value!=100)
-								{
-									info[3] += probHit_;
-									if (info[3] < probClampingMin_)
+									if(kter->first > 0)
 									{
+										info[0] = (float)kter->first;
+										info[1] = float(i) * cellSize_ + xMin;
+										info[2] = float(j) * cellSize_ + yMin;
 										info[3] = probClampingMin_;
+										cter->second.first+=1;
 									}
-									if (info[3] > probClampingMax_)
-									{
-										info[3] = probClampingMax_;
-									}
+									value = -2; // free space (footprint)
 								}
+							}
+						}

-								value = 100; // obstacles
+						if(jter!=occupiedLocalMaps.end())
+						{
+							for(int i=0; i<jter->second.cols; ++i)
+							{
+								float * ptf = jter->second.ptr<float>(0,i);
+								cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+								UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+											uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
+													kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
+								char & value = map.at<char>(pt.y, pt.x);
+								if(value != -2)
+								{
+									float * info = mapInfo.ptr<float>(pt.y, pt.x);
+									int nodeId = (int)info[0];
+									if(value != -1)
+									{
+										if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+										{
+											// cannot rewrite on cells referred by more recent nodes
+											continue;
+										}
+										if(nodeId>0)
+										{
+											std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+											UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+											if(value == 0)
+											{
+												eter->second.first -= 1;
+											}
+											else if(value == 100)
+											{
+												eter->second.second -= 1;
+											}
+											if(kter->first < 0)
+											{
+												eter->second.second += 1;
+											}
+										}
+									}
+									if(kter->first > 0)
+									{
+										info[0] = (float)kter->first;
+										info[1] = ptf[0];
+										info[2] = ptf[1];
+										cter->second.second+=1;
+									}
+
+									// update odds
+									if(nodeId != kter->first || value!=100)
+									{
+										info[3] += probHit_;
+										if (info[3] < probClampingMin_)
+										{
+											info[3] = probClampingMin_;
+										}
+										if (info[3] > probClampingMax_)
+										{
+											info[3] = probClampingMax_;
+										}
+									}
+
+									value = 100; // obstacles
+								}
 							}
 						}
 					}
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -202,6 +202,7 @@ Odometry::~Odometry()

 void Odometry::reset(const Transform & initialPose)
 {
+	UDEBUG("");
 	UASSERT(!initialPose.isNull());
 	previousVelocities_.clear();
 	velocityGuess_.setNull();
@@ -328,35 +329,73 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet

 	if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
 	{
-		if(data.stereoCameraModel().isValidForRectification())
+		if(!data.stereoCameraModels().empty())
 		{
-			if(!stereoModel_.isRectificationMapInitialized() ||
-				stereoModel_.left().imageSize() != data.stereoCameraModel().left().imageSize())
+			bool valid = true;
+			if(data.stereoCameraModels().size() != stereoModels_.size())
 			{
-				stereoModel_ = data.stereoCameraModel();
-				stereoModel_.initRectificationMap();
-				if(stereoModel_.isRectificationMapInitialized())
+				stereoModels_.clear();
+				valid = false;
+			}
+			else
+			{
+				for(size_t i=0; i<data.stereoCameraModels().size() && valid; ++i)
+				{
+					valid = stereoModels_[i].isRectificationMapInitialized() &&
+							stereoModels_[i].left().imageSize() == data.stereoCameraModels()[i].left().imageSize();
+				}
+			}
+
+			if(!valid)
+			{
+				stereoModels_ = data.stereoCameraModels();
+				valid = true;
+				for(size_t i=0; i<stereoModels_.size() && valid; ++i)
+				{
+					stereoModels_[i].initRectificationMap();
+					valid = stereoModels_[i].isRectificationMapInitialized();
+				}
+				if(valid)
 				{
 					UWARN("%s parameter is set to false but the selected odometry approach cannot "
-							"process raw images. We will rectify them for convenience.",
+							"process raw stereo images. We will rectify them for convenience.",
 							Parameters::kRtabmapImagesAlreadyRectified().c_str());
 				}
 				else
 				{
-					UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
+					UERROR("Odometry approach chosen cannot process raw stereo images (not rectified images) "
 							"and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
-							"Make sure images are rectified and set %s parameter back to true, or make sure "
-							"calibration is valid for rectification.",
+							"Make sure images are rectified and set %s parameter back to true, or "
+							"make sure calibration is valid for rectification",
 							Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					stereoModels_.clear();
 				}
 			}
-			if(stereoModel_.isRectificationMapInitialized())
+			if(valid)
 			{
-				data.setStereoImage(
-						stereoModel_.left().rectifyImage(data.imageRaw()),
-						stereoModel_.right().rectifyImage(data.rightRaw()),
-						stereoModel_,
-						false);
+				if(stereoModels_.size()==1)
+				{
+					data.setStereoImage(
+							stereoModels_[0].left().rectifyImage(data.imageRaw()),
+							stereoModels_[0].right().rectifyImage(data.rightRaw()),
+							stereoModels_,
+							false);
+				}
+				else
+				{
+					UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
+					int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
+					cv::Mat rectifiedLeftImages = data.imageRaw().clone();
+					cv::Mat rectifiedRightImages = data.imageRaw().clone();
+					for(size_t i=0; i<stereoModels_.size() && valid; ++i)
+					{
+						cv::Mat rectifiedLeft = stereoModels_[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+						cv::Mat rectifiedRight = stereoModels_[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
+						rectifiedLeft.copyTo(cv::Mat(rectifiedLeftImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+						rectifiedRight.copyTo(cv::Mat(rectifiedRightImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					}
+					data.setStereoImage(rectifiedLeftImages, rectifiedRightImages, stereoModels_, false);
+				}
 			}
 		}
 		else if(!data.cameraModels().empty())
@@ -599,12 +638,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 		else
 		{
-			StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
-			if(stereoModel.isValidForProjection())
+			std::vector<StereoCameraModel> stereoModels = decimatedData.stereoCameraModels();
+			for(unsigned int i=0; i<stereoModels.size(); ++i)
 			{
-				stereoModel.scale(1.0/double(_imageDecimation));
+				stereoModels[i].scale(1.0/double(_imageDecimation));
+			}
+			if(!stereoModels.empty())
+			{
+				decimatedData.setStereoImage(rgbLeft, depthRight, stereoModels);
 			}
-			decimatedData.setStereoImage(rgbLeft, depthRight, stereoModel);
 		}


--- a/corelib/src/OdometryThread.cpp
+++ b/corelib/src/OdometryThread.cpp
@@ -134,7 +134,7 @@ void OdometryThread::addData(const SensorData & data)
 	{
 		if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 		{
-			if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection())) &&
+			if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
 					data.laserScanRaw().empty())
 			{
 				ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
@@ -144,7 +144,7 @@ void OdometryThread::addData(const SensorData & data)
 		else
 		{
 			// Mono can accept RGB only
-			if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
+			if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
 			{
 				ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
 				return;
--- a/corelib/src/Optimizer.cpp
+++ b/corelib/src/Optimizer.cpp
@@ -438,7 +438,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		const std::map<int, CameraModel> & models,
+		const std::map<int, std::vector<CameraModel> > & models,
 		std::map<int, cv::Point3f> & points3DMap,
 		const std::map<int, std::map<int, FeatureBA> > & wordReferences,
 		std::set<int> * outliers)
@@ -457,37 +457,35 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		bool rematchFeatures)
 {
 	UDEBUG("");
-	std::map<int, CameraModel> models;
+	std::map<int, std::vector<CameraModel> > multiModels;
 	std::map<int, Transform> poses;
 	for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
 	{
 		// Get camera model
-		CameraModel model;
+		std::vector<CameraModel> models;
 		if(uContains(signatures, iter->first))
 		{
-			if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
+			const SensorData & s = signatures.at(iter->first).sensorData();
+			if(s.cameraModels().size() >= 1 && s.cameraModels().at(0).isValidForProjection())
 			{
-				model = signatures.at(iter->first).sensorData().cameraModels()[0];
+				models = s.cameraModels();
 			}
-			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
+			else if(!s.stereoCameraModels().empty() && s.stereoCameraModels()[0].isValidForProjection())
 			{
-				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+				for(size_t i=0; i<s.stereoCameraModels().size(); ++i)
+				{
+					CameraModel model = s.stereoCameraModels()[i].left();

-				// Set Tx = -baseline*fx for stereo BA
-				model = CameraModel(
-						model.fx(),
-						model.fy(),
-						model.cx(),
-						model.cy(),
-						model.localTransform(),
-						-signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
-			}
-			else if(signatures.at(iter->first).sensorData().cameraModels().size() > 1)
-			{
-				UERROR("Multi-cameras (%d) is not supported (id=%d).",
-						signatures.at(iter->first).sensorData().cameraModels().size(),
-						iter->first);
-				return std::map<int, Transform>();
+					// Set Tx = -baseline*fx for stereo BA
+					models.push_back(CameraModel(
+							model.fx(),
+							model.fy(),
+							model.cx(),
+							model.cy(),
+							model.localTransform(),
+							-s.stereoCameraModels()[i].baseline()*model.fx(),
+							model.imageSize()));
+				}
 			}
 			else
 			{
@@ -501,16 +499,14 @@ std::map<int, Transform> Optimizer::optimizeBA(
 			return std::map<int, Transform>();
 		}

-		UASSERT(model.isValidForProjection());
-
-		models.insert(std::make_pair(iter->first, model));
+		multiModels.insert(std::make_pair(iter->first, models));
 		poses.insert(*iter);
 	}

 	// compute correspondences
 	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);

-	return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
+	return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
 }

 std::map<int, Transform> Optimizer::optimizeBA(
@@ -537,9 +533,11 @@ Transform Optimizer::optimizeBA(
 	poses.insert(std::make_pair(link.to(), link.transform()));
 	std::multimap<int, Link> links;
 	links.insert(std::make_pair(link.from(), link));
-	std::map<int, CameraModel> models;
-	models.insert(std::make_pair(link.from(), model));
-	models.insert(std::make_pair(link.to(), model));
+	std::map<int, std::vector<CameraModel> > models;
+	std::vector<CameraModel> tmp;
+	tmp.push_back(model);
+	models.insert(std::make_pair(link.from(), tmp));
+	models.insert(std::make_pair(link.to(), tmp));
 	poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
 	if(poses.size() == 2)
 	{
@@ -567,7 +565,7 @@ void Optimizer::computeBACorrespondences(
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
 		bool rematchFeatures)
 {
-	UDEBUG("");
+	UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
 	std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
@@ -587,6 +585,14 @@ void Optimizer::computeBACorrespondences(
 			if(sFrom.getWeight() >= 0) // ignore intermediate links
 			{
 				Signature sTo = signatures.at(link.to());
+
+				if((sFrom.sensorData().cameraModels().empty() && sFrom.sensorData().stereoCameraModels().empty()) ||
+				   (sTo.sensorData().cameraModels().empty() && sTo.sensorData().stereoCameraModels().empty()))
+				{
+					UERROR("No camera models found");
+					continue;
+				}
+
 				if(sTo.getWeight() < 0)
 				{
 					for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
@@ -675,8 +681,7 @@ void Optimizer::computeBACorrespondences(
 									wordId = ++wordCount;
 									wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));

-									p = util3d::transformPoint(p, pose);
-									points3DMap.insert(std::make_pair(wordId, p));
+									points3DMap.insert(std::make_pair(wordId, util3d::transformPoint(p, pose)));
 								}
 								else
 								{
@@ -692,7 +697,27 @@ void Optimizer::computeBACorrespondences(
 										UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
 										descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
 									}
-									wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
+									int cameraIndex = 0;
+									if(sFrom.sensorData().cameraModels().size()>1 || sFrom.sensorData().stereoCameraModels().size()>1)
+									{
+										float subImageWidth = sFrom.sensorData().cameraModels().size()>1?sFrom.sensorData().cameraModels()[0].imageWidth():sFrom.sensorData().stereoCameraModels()[0].left().imageWidth();
+										cameraIndex = int(ptFrom.pt.x / subImageWidth);
+										ptFrom.pt.x = ptFrom.pt.x - (subImageWidth*float(cameraIndex));
+									}
+									
+									float depth = 0.0f;
+									if(!sFrom.sensorData().cameraModels().empty())
+									{
+										depth = util3d::transformPoint(p, sFrom.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
+									}
+									else
+									{
+										UASSERT(!sFrom.sensorData().stereoCameraModels().empty());
+										depth = util3d::transformPoint(p, sFrom.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
+									}
+
+
+									wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, depth, descriptorFrom, cameraIndex)));
 									frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
 									frameToWordMap.at(sFrom.id()).insert(std::make_pair(ptFrom, wordId));
 								}
@@ -705,17 +730,32 @@ void Optimizer::computeBACorrespondences(
 										UASSERT(indexTo < sTo.getWordsDescriptors().rows);
 										descriptorTo = sTo.getWordsDescriptors().row(indexTo);
 									}
+
+									int cameraIndex = 0;
+									if(sTo.sensorData().cameraModels().size()>1 || sTo.sensorData().stereoCameraModels().size()>1)
+									{
+										float subImageWidth = sTo.sensorData().cameraModels().size()>1?sTo.sensorData().cameraModels()[0].imageWidth():sTo.sensorData().stereoCameraModels()[0].left().imageWidth();
+										cameraIndex = int(ptTo.pt.x / subImageWidth);
+										ptTo.pt.x = ptTo.pt.x - (subImageWidth*float(cameraIndex));
+									}
+
 									float depth = 0.0f;
 									if(!sTo.getWords3().empty())
 									{
 										UASSERT(indexTo < (int)sTo.getWords3().size());
 										const cv::Point3f & pt = sTo.getWords3()[indexTo];
-										if( pt.x > 0)
+										if(!sTo.sensorData().cameraModels().empty())
 										{
-											depth = pt.x;
+											depth = util3d::transformPoint(pt, sTo.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
+										}
+										else
+										{
+											UASSERT(!sTo.sensorData().stereoCameraModels().empty());
+											depth = util3d::transformPoint(pt, sTo.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
 										}
 									}
-									wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
+
+									wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo, cameraIndex)));
 									frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
 									frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId));
 								}
@@ -732,6 +772,14 @@ void Optimizer::computeBACorrespondences(
 		}
 	}
 	UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
+	if(links.empty())
+	{
+		UERROR("No links found for BA?!");
+	}
+	else if(wordCount == 0)
+	{
+		UERROR("No words added for BA?!");
+	}
 }

 } /* namespace rtabmap */
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -188,7 +188,8 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 			group.compare("GTSAM") == 0 ||
 			(vis && (group.compare("Vis") == 0 || group.compare("PyMatcher") == 0 || group.compare("GMS") == 0)) ||
 			iter->first.compare(kRtabmapPublishRAMUsage())==0 ||
-			iter->first.compare(kRtabmapImagesAlreadyRectified())==0)
+			iter->first.compare(kRtabmapImagesAlreadyRectified())==0 ||
+			iter->first.compare(kKpByteToFloat())==0)
 		{
 			odomParameters.insert(*iter);
 		}
@@ -660,6 +661,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OpenGV:";
+#ifdef RTABMAP_OPENGV
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With Madgwick:";
 #ifdef RTABMAP_MADGWICK
--- a/corelib/src/Recovery.cpp
+++ b/corelib/src/Recovery.cpp
@@ -52,7 +52,6 @@ bool databaseRecovery(
 		return false;
 	}

-	std::string backupPath;
 	if(UFile::getExtension(databasePath).compare("db") != 0)
 	{
 		if(errorMsg)
@@ -61,12 +60,17 @@ bool databaseRecovery(
 	}
 	std::list<std::string> strList = uSplit(databasePath, '.');
 	strList.pop_back();
-	backupPath = uJoin(strList, ".") + ".backup.db";
-	if(UFile::exists(backupPath))
+
+	std::string recoveryPath;
+	recoveryPath = uJoin(strList, ".") + ".recovery.db";
+	if(UFile::exists(recoveryPath))
 	{
-		if(errorMsg)
-			*errorMsg = uFormat("Backup file \"%s\" already exists!", backupPath.c_str());
-		return false;
+		if(UFile::erase(recoveryPath) != 0)
+		{
+			if(errorMsg)
+				*errorMsg = uFormat("Failed to remove temporary recovery database \"%s\", is it opened by another app?", recoveryPath.c_str());
+			return false;
+		}
 	}

 	DBDriver * dbDriver = DBDriver::create();
@@ -125,41 +129,42 @@ bool databaseRecovery(
 	dbDriver->closeConnection(false);
 	delete dbDriver;

-	if(progressState)
-		progressState->callback(uFormat("Renaming \"%s\" to \"%s\"...", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str()));
-	if(UFile::rename(databasePath, backupPath) != 0)
-	{
-		if(errorMsg)
-			*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str());
-		return false;
-	}
-
 	bool incrementalMemory = true;
+	bool dbInMemory = false;
 	Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
+	Parameters::parse(parameters, Parameters::kDbSqlite3InMemory(), dbInMemory);
 	if(!incrementalMemory)
 	{
 		if(progressState)
 		{
-			progressState->callback("Database is in localization mode, setting it to mapping mode to recover...");
+			progressState->callback("Database is in localization mode, setting it to mapping mode to recover.");
 		}
 		uInsert(parameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true"));
 	}
+	if(dbInMemory)
+	{
+		if(progressState)
+		{
+			progressState->callback(uFormat("Database has %s=true, setting it to false to avoid RAM problems during recovery.", Parameters::kDbSqlite3InMemory().c_str()));
+		}
+		uInsert(parameters, ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+	}

 	Rtabmap rtabmap;
-	rtabmap.init(parameters, databasePath);
+	rtabmap.init(parameters, recoveryPath);

 	bool rgbdEnabled = Parameters::defaultRGBDEnabled();
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
 	{
-		DBReader dbReader(backupPath, 0, odometryIgnored);
+		DBReader dbReader(databasePath, 0, odometryIgnored);
 		dbReader.init();

 		CameraInfo info;
 		SensorData data = dbReader.takeImage(&info);
 		int processed = 0;
 		if (progressState)
-			progressState->callback(uFormat("Recovering data of \"%s\"...", backupPath.c_str()));
+			progressState->callback(uFormat("Recovering data of \"%s\"...", databasePath.c_str()));
 		while (data.isValid() && (progressState == 0 || !progressState->isCanceled()))
 		{
 			std::string status;
@@ -198,25 +203,60 @@ bool databaseRecovery(
 		{
 			rtabmap.close(false);
 			if(errorMsg)
-				*errorMsg = uFormat("Recovery canceled, renaming back \"%s\" to \"%s\".", backupPath.c_str(), databasePath.c_str());
+				*errorMsg = uFormat("Recovery canceled, removing temporary recovery database \"%s\".", recoveryPath.c_str());

-			// put back the file as before
-			UFile::erase(databasePath);
-			UFile::rename(backupPath, databasePath);
+			UFile::erase(recoveryPath);
 			return false;
 		}
 	}

 	if(progressState)
-		progressState->callback(uFormat("Closing database \"%s\"...", databasePath.c_str()));
+		progressState->callback(uFormat("Closing database \"%s\"...", recoveryPath.c_str()));
 	rtabmap.close(true);
 	if(progressState)
-		progressState->callback(uFormat("Closing database \"%s\"... done!", databasePath.c_str()));
+		progressState->callback(uFormat("Closing database \"%s\"... done!", recoveryPath.c_str()));

-	if(!keepCorruptedDatabase)
+	if(keepCorruptedDatabase)
 	{
-		UFile::erase(backupPath);
+		std::string backupPath;
+		backupPath = uJoin(strList, ".") + ".backup.db";
+
+		if(!UFile::exists(backupPath))
+		{
+			if(progressState)
+				progressState->callback(uFormat("Renaming \"%s\" to \"%s\"... (keep corrupted database backup option is enabled).", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str()));
+			if(UFile::rename(databasePath, backupPath) != 0)
+			{
+				if(errorMsg)
+					*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str());
+				return false;
+			}
+			if(progressState)
+				progressState->callback(uFormat("Renaming \"%s\" to \"%s\"... done!", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str()));
+		}
+		else
+		{
+			if(progressState)
+				progressState->callback(uFormat("Backup \"%s\" already exists, won't copy again.", UFile::getName(backupPath).c_str()));
+		}
 	}
+	else if(UFile::erase(databasePath) != 0)
+	{
+		if(errorMsg)
+			*errorMsg = uFormat("Failed remove original database file \"%s\". Is it opened by another app? The recovered database cannot be copied back to original name.", UFile::getName(databasePath).c_str(), UFile::getName(recoveryPath).c_str());
+		return false;
+	}
+
+	if(progressState)
+		progressState->callback(uFormat("Renaming \"%s\" to \"%s\"...", UFile::getName(recoveryPath).c_str(), UFile::getName(databasePath).c_str()));
+	if(UFile::rename(recoveryPath, databasePath) != 0)
+	{
+		if(errorMsg)
+			*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(recoveryPath).c_str(), UFile::getName(databasePath).c_str());
+		return false;
+	}
+	if(progressState)
+		progressState->callback(uFormat("Renaming \"%s\" to \"%s\"... done!", UFile::getName(recoveryPath).c_str(), UFile::getName(databasePath).c_str()));

 	return true;
 }
--- a/corelib/src/Registration.cpp
+++ b/corelib/src/Registration.cpp
@@ -33,7 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

-double Registration::COVARIANCE_EPSILON = 0.000000001;
+double Registration::COVARIANCE_LINEAR_EPSILON = 0.00000001;   // 0.1 mm
+double Registration::COVARIANCE_ANGULAR_EPSILON = 0.00000003; // 0.01 deg

 Registration * Registration::create(const ParametersMap & parameters)
 {
@@ -237,18 +238,18 @@ Transform Registration::computeTransformationMod(
 		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}

-	if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON)
-		info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON)
-		info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON)
-		info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON)
-		info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON)
-		info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
-		info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
+	if(info.covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
+		info.covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+	if(info.covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
+		info.covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+	if(info.covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
+		info.covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+	if(info.covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
+		info.covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+	if(info.covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
+		info.covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+	if(info.covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
+		info.covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform


 	if(infoOut)
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -69,6 +69,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
+		_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
 		_correspondencesApproach(Parameters::defaultVisCorType()),
 		_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -124,6 +125,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
 	Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
+	Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
 	Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
 	Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -287,6 +289,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
 	UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
 	UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
+	UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
 	UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -310,7 +313,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			fromSignature.sensorData().imageRaw().cols,
 			fromSignature.sensorData().imageRaw().rows,
 			(int)fromSignature.sensorData().cameraModels().size(),
-			fromSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
+			(int)fromSignature.sensorData().stereoCameraModels().size());

 	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			toSignature.id(),
@@ -323,7 +326,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			toSignature.sensorData().imageRaw().cols,
 			toSignature.sensorData().imageRaw().rows,
 			(int)toSignature.sensorData().cameraModels().size(),
-			toSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
+			(int)toSignature.sensorData().stereoCameraModels().size());

 	std::string msg;
 	info.projectedIDs.clear();
@@ -487,17 +490,24 @@ Transform RegistrationVis::computeTransformationImpl(
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
 				if(guessSet)
 				{
-					Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
-					Transform guessCameraRef = (guess * localTransform).inverse();
-					cv::Mat R = (cv::Mat_<double>(3,3) <<
-							(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
-							(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
-							(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
-					cv::Mat rvec(1,3, CV_64FC1);
-					cv::Rodrigues(R, rvec);
-					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
-					cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
-					cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
+					if(toSignature.sensorData().cameraModels().size() == 1 || toSignature.sensorData().stereoCameraModels().size() == 1)
+					{
+						Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModels()[0].left().localTransform();
+						Transform guessCameraRef = (guess * localTransform).inverse();
+						cv::Mat R = (cv::Mat_<double>(3,3) <<
+								(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
+								(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
+								(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
+						cv::Mat rvec(1,3, CV_64FC1);
+						cv::Rodrigues(R, rvec);
+						cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
+						cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModels()[0].left().K();
+						cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
+					}
+					else
+					{
+						UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored...");
+					}
 				}

 				// Find features in the new left image
@@ -735,7 +745,7 @@ Transform RegistrationVis::computeTransformationImpl(

 			if(!kptsFrom3D.empty() &&
 			   (_detectorFrom->getMinDepth() > 0.0f || _detectorFrom->getMaxDepth() > 0.0f) &&
-			   (!fromSignature.sensorData().cameraModels().empty() || fromSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
+			   (!fromSignature.sensorData().cameraModels().empty() || !fromSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
 			{
 				_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
 			}
@@ -770,7 +780,7 @@ Transform RegistrationVis::computeTransformationImpl(

 			if(kptsTo3D.size() &&
 		       (_detectorTo->getMinDepth() > 0.0f || _detectorTo->getMaxDepth() > 0.0f) &&
-			   (!toSignature.sensorData().cameraModels().empty() || toSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
+			   (!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
 			{
 				_detectorTo->filterKeypointsByDepth(kptsTo, descriptorsTo, kptsTo3D, _detectorTo->getMinDepth(), _detectorTo->getMaxDepth());
 			}
@@ -787,15 +797,37 @@ Transform RegistrationVis::computeTransformationImpl(
 			// We have all data we need here, so match!
 			if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
 			{
-				cv::Size imageSize = imageTo.size();
-				bool isCalibrated = false; // multiple cameras not supported.
-				if(imageSize.height == 0 || imageSize.width == 0)
+				std::vector<CameraModel> models;
+				if(!toSignature.sensorData().stereoCameraModels().empty())
 				{
-					imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
+					for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
+					{
+						models.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
+					}
+				}
+				else
+				{
+					models = toSignature.sensorData().cameraModels();
 				}

-				isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
-						(toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
+				bool isCalibrated = !models.empty();
+				for(size_t i=0; i<models.size() && isCalibrated; ++i)
+				{
+					isCalibrated = models[i].isValidForProjection();
+
+					// For old database formats
+					if(isCalibrated && (models[i].imageWidth()==0 || models[i].imageHeight()==0))
+					{
+						if(!toSignature.sensorData().imageRaw().empty())
+						{
+							models[i].setImageSize(cv::Size(toSignature.sensorData().imageRaw().cols/models.size(), toSignature.sensorData().imageRaw().rows));
+						}
+						else
+						{
+							isCalibrated = false;
+						}
+					}
+				}

 				// If guess is set, limit the search of matches using optical flow window size
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
@@ -803,52 +835,62 @@ Transform RegistrationVis::computeTransformationImpl(
 						isCalibrated &&  // needed for projection
 						_estimationType != 2) // To make sure we match all features for 2D->2D
 				{
+					// Use guess to project 3D "from" keypoints into "to" image
 					UDEBUG("");
 					UASSERT((int)kptsTo.size() == descriptorsTo.rows);
 					UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);

-					// Use guess to project 3D "from" keypoints into "to" image
-					if(toSignature.sensorData().cameraModels().size() > 1)
+					std::vector<cv::Point2f> cornersProjected;
+					std::vector<int> projectedIndexToDescIndex;
+					float subImageWidth = models[0].imageWidth();
+					std::set<int> added;
+					int duplicates=0;
+					for(size_t m=0; m<models.size(); ++m)
 					{
-						UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
-					}
+						Transform guessCameraRef = (guess * models[m].localTransform()).inverse();
+						cv::Mat R = (cv::Mat_<double>(3,3) <<
+								(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
+								(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
+								(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
+						cv::Mat rvec(1,3, CV_64FC1);
+						cv::Rodrigues(R, rvec);
+						cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
+						cv::Mat K = models[m].K();
+						std::vector<cv::Point2f> projected;
+						cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
+						UDEBUG("Projected points=%d", (int)projected.size());

-					Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModel().left().localTransform();
-					Transform guessCameraRef = (guess * localTransform).inverse();
-					cv::Mat R = (cv::Mat_<double>(3,3) <<
-							(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
-							(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
-							(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
-					cv::Mat rvec(1,3, CV_64FC1);
-					cv::Rodrigues(R, rvec);
-					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
-					cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
-					std::vector<cv::Point2f> projected;
-					cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
-					UDEBUG("Projected points=%d", (int)projected.size());
-					//remove projected points outside of the image
-					UASSERT((int)projected.size() == descriptorsFrom.rows);
-					std::vector<cv::Point2f> cornersProjected(projected.size());
-					std::vector<int> projectedIndexToDescIndex(projected.size());
-					int oi=0;
-					for(unsigned int i=0; i<projected.size(); ++i)
-					{
-						if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
-						   uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
-						   util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
+						//remove projected points outside of the image
+						UASSERT((int)projected.size() == descriptorsFrom.rows);
+						int cornersInFrame = 0;
+						for(unsigned int i=0; i<projected.size(); ++i)
 						{
-							projectedIndexToDescIndex[oi] = i;
-							cornersProjected[oi++] = projected[i];
+							if(uIsInBounds(projected[i].x, 0.0f, float(models[m].imageWidth()-1)) &&
+							   uIsInBounds(projected[i].y, 0.0f, float(models[m].imageHeight()-1)) &&
+							   util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
+							{
+								if(added.find(i) != added.end())
+								{
+									++duplicates;
+									continue;
+								}
+
+								projectedIndexToDescIndex.push_back(i);
+								projected[i].x += subImageWidth*float(m); // Convert in multicam stitched image
+								cornersProjected.push_back(projected[i]);
+								++cornersInFrame;
+								added.insert(i);
+							}
 						}
+						UDEBUG("corners in frame=%d (camera index=%ld)", cornersInFrame, m);
 					}
-					projectedIndexToDescIndex.resize(oi);
-					cornersProjected.resize(oi);
-					UDEBUG("corners in frame=%d", (int)cornersProjected.size());

 					// For each projected feature guess of "from" in "to", find its matching feature in
 					// the radius around the projected guess.
 					// TODO: do cross-check?
-					UDEBUG("guessMatchToProjection=%d, cornersProjected=%d", _guessMatchToProjection?1:0, (int)cornersProjected.size());
+					UDEBUG("guessMatchToProjection=%d, cornersProjected=%d orignalWordsFromIds=%d (added=%ld, duplicates=%d)",
+							_guessMatchToProjection?1:0, (int)cornersProjected.size(), (int)orignalWordsFromIds.size(),
+							added.size(), duplicates);
 					if(cornersProjected.size())
 					{
 						if(_guessMatchToProjection)
@@ -1147,19 +1189,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
 					{
-						if(fromSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().cameraModels().size() > 1)
-						{
-							UWARN("Finding correspondences with the guess cannot "
-								  "be done with multiple cameras, global matching is "
-								   "done instead. Please set \"%s\" to 0 to avoid this warning.",
-								   Parameters::kVisCorGuessWinSize().c_str());
-						}
-						else
-						{
-							UWARN("Calibration not found! Finding correspondences "
-								   "with the guess cannot be done, global matching is "
-								   "done instead.");
-						}
+						UWARN("Calibration not found! Finding correspondences "
+							   "with the guess cannot be done, global matching is "
+							   "done instead.");
 					}

 					UDEBUG("");
@@ -1194,16 +1226,16 @@ Transform RegistrationVis::computeTransformationImpl(
 								descriptorsTo.type() == CV_32F &&
 								descriptorsFrom.type() == CV_32F &&
 								descriptorsFrom.rows == (int)kptsFrom.size() &&
-								imageSize.width > 0 && imageSize.height > 0)
+								models.size() == 1)
 							{
 								UDEBUG("Python matching");
-								matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, imageSize);
+								matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, models[0].imageSize());
 							}
 							else
 							{
 								if(_nnType == 6 && _pyMatcher)
 								{
-									UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported), doing bruteforce matching instead.", descriptorsFrom.type());
+									UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported, multicam not supported), doing bruteforce matching instead.", descriptorsFrom.type());
 								}
 #else
 							{
@@ -1215,11 +1247,11 @@ Transform RegistrationVis::computeTransformationImpl(
 								if(_nnType == 7)
 								{
 									imageSizeFrom = imageFrom.size();
-									if(imageSizeFrom.height == 0 || imageSizeFrom.width == 0)
+									if((imageSizeFrom.height == 0 || imageSizeFrom.width == 0) && (fromSignature.sensorData().cameraModels().size() || fromSignature.sensorData().stereoCameraModels().size()))
 									{
-										imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
+										imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModels()[0].left().imageSize();
 									}
-									if(imageSize.height > 0 && imageSize.width > 0 &&
+									if(!models.empty() && models[0].imageSize().height > 0 && models[0].imageSize().width > 0 &&
 									   imageSizeFrom.height > 0 && imageSizeFrom.width > 0)
 									{
 										doCrossCheck = false;
@@ -1239,8 +1271,9 @@ Transform RegistrationVis::computeTransformationImpl(
 #if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
 								if(!doCrossCheck)
 								{
+									UASSERT(!models.empty());
 									std::vector<cv::DMatch> matchesGMS;
-									cv::xfeatures2d::matchGMS(imageSize, imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
+									cv::xfeatures2d::matchGMS(models[0].imageSize(), imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
 									matches = matchesGMS;
 								}
 #endif
@@ -1385,7 +1418,8 @@ Transform RegistrationVis::computeTransformationImpl(
 			if(_estimationType == 2) // Epipolar Geometry
 			{
 				UDEBUG("");
-				if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
+				if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
+					!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
 				   (signatureB->sensorData().cameraModels().size() != 1 ||
 					!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
 				{
@@ -1394,8 +1428,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				else if((int)signatureA->getWords().size() >= _minInliers &&
 						(int)signatureB->getWords().size() >= _minInliers)
 				{
-					UASSERT(signatureA->sensorData().stereoCameraModel().isValidForProjection() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
-					const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValidForProjection()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
+					UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
+					const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];

 					// we only need the camera transform, send guess words3 for scale estimation
 					Transform cameraTransform;
@@ -1479,16 +1513,22 @@ Transform RegistrationVis::computeTransformationImpl(
 			else if(_estimationType == 1) // PnP
 			{
 				UDEBUG("");
-				if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
-				   (signatureB->sensorData().cameraModels().size() != 1 ||
-					!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
+				if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
+				   (signatureB->sensorData().cameraModels().empty() || !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
 				{
-					UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
+					UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
 							signatureB->id(),
 							(int)signatureB->sensorData().cameraModels().size(),
-							signatureB->sensorData().stereoCameraModel().isValidForProjection()?1:0,
+							signatureB->sensorData().stereoCameraModels().size(),
 							signatureB->getWeight());
 				}
+#ifndef RTABMAP_OPENGV
+				else if(signatureB->sensorData().cameraModels().size() > 1)
+				{
+					UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
+							"with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
+				}
+#endif
 				else
 				{
 					UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
@@ -1496,9 +1536,6 @@ Transform RegistrationVis::computeTransformationImpl(
 					if((int)signatureA->getWords3().size() >= _minInliers &&
 					   (int)signatureB->getWords().size() >= _minInliers)
 					{
-						UASSERT(signatureB->sensorData().stereoCameraModel().isValidForProjection() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValidForProjection()));
-						const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValidForProjection()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
-
 						std::vector<int> inliersV;
 						std::vector<int> matchesV;
 						std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
@@ -1518,22 +1555,65 @@ Transform RegistrationVis::computeTransformationImpl(
 								words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
 							}
 						}
-						transforms[dir] = util3d::estimateMotion3DTo2D(
-								words3A,
-								wordsB,
-								cameraModel,
-								_minInliers,
-								_iterations,
-								_PnPReprojError,
-								_PnPFlags,
-								_PnPRefineIterations,
-								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
-								words3B,
-								&covariances[dir],
-								&matchesV,
-								&inliersV);
-						inliers[dir] = inliersV;
-						matches[dir] = matchesV;
+
+						std::vector<CameraModel> models;
+						if(signatureB->sensorData().stereoCameraModels().size())
+						{
+							for(size_t i=0; i<signatureB->sensorData().stereoCameraModels().size(); ++i)
+							{
+								models.push_back(signatureB->sensorData().stereoCameraModels()[i].left());
+							}
+						}
+						else
+						{
+							models = signatureB->sensorData().cameraModels();
+						}
+
+						if(models.size()>1)
+						{
+							// Multi-Camera
+							UASSERT(models[0].isValidForProjection());
+
+							transforms[dir] = util3d::estimateMotion3DTo2D(
+									words3A,
+									wordsB,
+									models,
+									_minInliers,
+									_iterations,
+									_PnPReprojError,
+									_PnPFlags,
+									_PnPRefineIterations,
+									_PnPMaxVar,
+									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
+									words3B,
+									&covariances[dir],
+									&matchesV,
+									&inliersV);
+							inliers[dir] = inliersV;
+							matches[dir] = matchesV;
+						}
+						else
+						{
+							UASSERT(models.size() == 1 && models[0].isValidForProjection());
+
+							transforms[dir] = util3d::estimateMotion3DTo2D(
+									words3A,
+									wordsB,
+									models[0],
+									_minInliers,
+									_iterations,
+									_PnPReprojError,
+									_PnPFlags,
+									_PnPRefineIterations,
+									_PnPMaxVar,
+									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
+									words3B,
+									&covariances[dir],
+									&matchesV,
+									&inliersV);
+							inliers[dir] = inliersV;
+							matches[dir] = matchesV;
+						}
 						UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
 						if(transforms[dir].isNull())
 						{
@@ -1652,8 +1732,8 @@ Transform RegistrationVis::computeTransformationImpl(
 			allInliers.size() &&
 			fromSignature.getWords3().size() &&
 			toSignature.getWords().size() &&
-			fromSignature.sensorData().cameraModels().size() <= 1 &&
-			toSignature.sensorData().cameraModels().size() <= 1)
+			(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
+			(toSignature.sensorData().stereoCameraModels().size() >= 1 || toSignature.sensorData().cameraModels().size() >= 1))
 		{
 			UDEBUG("Refine with bundle adjustment");
 			Optimizer * sba = Optimizer::create(_bundleAdjustment==3?Optimizer::kTypeCeres:_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
@@ -1668,18 +1748,18 @@ Transform RegistrationVis::computeTransformationImpl(
 			for(int i=0;i<2;++i)
 			{
 				UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
-				if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
-					covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
-					covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
-					covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
-					covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
-					covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
-					covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
+					covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
+					covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
+					covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
+					covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
+					covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
+					covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
 			}

 			cv::Mat cov = covariances[0].clone();
@@ -1693,60 +1773,61 @@ Transform RegistrationVis::computeTransformationImpl(

 			std::map<int, Transform> optimizedPoses;

-			UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
-					(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
+			UASSERT((toSignature.sensorData().stereoCameraModels().size()  >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
+					(toSignature.sensorData().cameraModels().size() >= 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));

-			std::map<int, CameraModel> models;
+			std::map<int, std::vector<CameraModel> > models;

-			Transform invLocalTransformFrom;
-			CameraModel cameraModelFrom;
-			if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
+			std::vector<CameraModel> cameraModelsFrom;
+			if(fromSignature.sensorData().stereoCameraModels().size())
 			{
-				cameraModelFrom = fromSignature.sensorData().stereoCameraModel().left();
-				// Set Tx=-baseline*fx for Stereo BA
-				cameraModelFrom = CameraModel(cameraModelFrom.fx(),
-						cameraModelFrom.fy(),
-						cameraModelFrom.cx(),
-						cameraModelFrom.cy(),
-						cameraModelFrom.localTransform(),
-						-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
-				invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
+				for(size_t i=0; i<fromSignature.sensorData().stereoCameraModels().size(); ++i)
+				{
+					CameraModel cameraModel = fromSignature.sensorData().stereoCameraModels()[i].left();
+					// Set Tx=-baseline*fx for Stereo BA
+					cameraModel = CameraModel(cameraModel.fx(),
+							cameraModel.fy(),
+							cameraModel.cx(),
+							cameraModel.cy(),
+							cameraModel.localTransform(),
+							-fromSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
+							cameraModel.imageSize());
+					cameraModelsFrom.push_back(cameraModel);
+				}
 			}
-			else if(fromSignature.sensorData().cameraModels().size() == 1)
+			else
 			{
-				cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
-				invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
+				cameraModelsFrom = fromSignature.sensorData().cameraModels();
 			}

-			Transform invLocalTransformTo = Transform::getIdentity();
-			CameraModel cameraModelTo;
-			if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
+			std::vector<CameraModel> cameraModelsTo;
+			if(toSignature.sensorData().stereoCameraModels().size())
 			{
-				cameraModelTo = toSignature.sensorData().stereoCameraModel().left();
-				// Set Tx=-baseline*fx for Stereo BA
-				cameraModelTo = CameraModel(cameraModelTo.fx(),
-						cameraModelTo.fy(),
-						cameraModelTo.cx(),
-						cameraModelTo.cy(),
-						cameraModelTo.localTransform(),
-						-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
-				invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
+				for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
+				{
+					CameraModel cameraModel = toSignature.sensorData().stereoCameraModels()[i].left();
+					// Set Tx=-baseline*fx for Stereo BA
+					cameraModel = CameraModel(cameraModel.fx(),
+							cameraModel.fy(),
+							cameraModel.cx(),
+							cameraModel.cy(),
+							cameraModel.localTransform(),
+							-toSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
+							cameraModel.imageSize());
+					cameraModelsTo.push_back(cameraModel);
+				}
 			}
-			else if(toSignature.sensorData().cameraModels().size() == 1)
+			else
 			{
-				cameraModelTo = toSignature.sensorData().cameraModels()[0];
-				invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
-			}
-			if(invLocalTransformFrom.isNull())
-			{
-				invLocalTransformFrom = invLocalTransformTo;
+				cameraModelsTo = toSignature.sensorData().cameraModels();
 			}

-			models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
-			models.insert(std::make_pair(2, cameraModelTo));
+			models.insert(std::make_pair(1, cameraModelsFrom));
+			models.insert(std::make_pair(2, cameraModelsTo));

 			std::map<int, std::map<int, FeatureBA> > wordReferences;
 			std::set<int> sbaOutliers;
+			UDEBUG("");
 			for(unsigned int i=0; i<allInliers.size(); ++i)
 			{
 				int wordId = allInliers[i];
@@ -1762,22 +1843,50 @@ Transform RegistrationVis::computeTransformationImpl(
 				points3DMap.insert(std::make_pair(wordId, pt3D));

 				std::map<int, FeatureBA> ptMap;
-				if(!fromSignature.getWordsKpts().empty() && cameraModelFrom.isValidForProjection())
+				if(!fromSignature.getWordsKpts().empty())
 				{
-					float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
-					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[indexFrom];
-					ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
+					cv::KeyPoint kpt = fromSignature.getWordsKpts()[indexFrom];
+
+					int cameraIndex = 0;
+					const std::vector<CameraModel> & cam = models.at(1);
+					if(cam.size()>1)
+					{
+						UASSERT(cam[0].imageWidth()>0);
+						float subImageWidth = cam[0].imageWidth();
+						cameraIndex = int(kpt.pt.x / subImageWidth);
+						kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
+					}
+
+					UASSERT(cam[cameraIndex].isValidForProjection());
+
+					float depthFrom = util3d::transformPoint(pt3D, cam[cameraIndex].localTransform().inverse()).z;
+					ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom, cv::Mat(), cameraIndex)));
 				}
-				if(!toSignature.getWordsKpts().empty() && cameraModelTo.isValidForProjection())
+
+				if(!toSignature.getWordsKpts().empty())
 				{
 					int indexTo = toSignature.getWords().find(wordId)->second;
+					cv::KeyPoint kpt = toSignature.getWordsKpts()[indexTo];
+
+					int cameraIndex = 0;
+					const std::vector<CameraModel> & cam = models.at(2);
+					if(cam.size()>1)
+					{
+						UASSERT(cam[0].imageWidth()>0);
+						float subImageWidth = cam[0].imageWidth();
+						cameraIndex = int(kpt.pt.x / subImageWidth);
+						kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
+					}
+
+					UASSERT(cam[cameraIndex].isValidForProjection());
+
 					float depthTo = 0.0f;
 					if(!toSignature.getWords3().empty())
 					{
-						depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], invLocalTransformTo).z;
+						depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], cam[cameraIndex].localTransform().inverse()).z;
 					}
-					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[indexTo];
-					ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
+
+					ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo, cv::Mat(), cameraIndex)));
 				}

 				wordReferences.insert(std::make_pair(wordId, ptMap));
@@ -1873,31 +1982,31 @@ Transform RegistrationVis::computeTransformationImpl(
 		if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
 		{
 			cv::Mat pcaData;
-			float cx=0, cy=0, w=0, h=0;
+			std::vector<CameraModel> cameraModelsTo;
+			if(toSignature.sensorData().stereoCameraModels().size())
+			{
+				for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
+				{
+					cameraModelsTo.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
+				}
+			}
+			else
+			{
+				cameraModelsTo = toSignature.sensorData().cameraModels();
+			}
 			if(_minInliersDistributionThr > 0)
 			{
-				if(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
-					(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
+				if(cameraModelsTo.size() >= 1 && cameraModelsTo[0].isValidForReprojection())
 				{
-					const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
-					cx = cameraModel.cx();
-					cy = cameraModel.cy();
-					w = cameraModel.imageWidth();
-					h = cameraModel.imageHeight();
-
-					if(w>0 && h>0)
+					if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
 					{
 						pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
 					}
 					else
 					{
-						UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
+						UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", cameraModelsTo[0].imageWidth(), cameraModelsTo[0].imageHeight(), Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
 					}
 				}
-				else if(toSignature.sensorData().cameraModels().size() > 1)
-				{
-					UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
-				}
 				else
 				{
 					UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
@@ -1927,12 +2036,14 @@ Transform RegistrationVis::computeTransformationImpl(

 				if(!pcaData.empty())
 				{
-					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
-					UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
+					std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
+					UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty());
 					float * ptr = pcaData.ptr<float>(i, 0);
-					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
-					ptr[0] = (kpt.pt.x-cx) / w;
-					ptr[1] = (kpt.pt.y-cy) / h;
+					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];
+					int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
+					UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
+					ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
+					ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
 				}
 			}

--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -53,6 +53,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UProcessInfo.h>

+#ifdef RTABMAP_PYTHON
+#include "rtabmap/core/PythonInterface.h"
+#endif
+
 #include <pcl/search/kdtree.h>
 #include <pcl/filters/crop_box.h>
 #include <pcl/io/pcd_io.h>
@@ -97,6 +101,7 @@ Rtabmap::Rtabmap() :
 	_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
 	_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
+	_maxRepublished(Parameters::defaultRtabmapMaxRepublished()),
 	_rawDataKept(Parameters::defaultMemImageKept()),
 	_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
 	_statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
@@ -137,6 +142,8 @@ Rtabmap::Rtabmap() :
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
 	_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
+	_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
+	_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -161,6 +168,9 @@ Rtabmap::Rtabmap() :
 	_pathTransformToGoal(Transform::getIdentity()),
 	_pathStuckCount(0),
 	_pathStuckDistance(0.0f)
+#ifdef RTABMAP_PYTHON
+	,_python(new PythonInterface())
+#endif
 {
 }

@@ -346,6 +356,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	_globalScanMapPoses.clear();
 	_odomCachePoses.clear();
 	_odomCacheConstraints.clear();
+	_nodesToRepublish.clear();

 	// Parse all parameters
 	this->parseParameters(allParameters);
@@ -464,6 +475,8 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_globalScanMap.clear();
 	_globalScanMapPoses.clear();

+	_nodesToRepublish.clear();
+
 	flushStatisticLogs();
 	if(_foutFloat)
 	{
@@ -550,6 +563,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
 	Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
 	Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
+	Parameters::parse(parameters, Parameters::kRtabmapMaxRepublished(), _maxRepublished);
+	if(_maxRepublished == 0 || !_publishLastSignatureData)
+	{
+		_nodesToRepublish.clear();
+	}
 	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
@@ -584,13 +602,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_optimizeFromGraphEndChanged = true;
 	}
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
-	if(_optimizationMaxError > 0.0 && _optimizationMaxError < 1.0)
-	{
-		UWARN("RGBD/OptimizeMaxError (value=%f) is smaller than 1.0, setting to default %f "
-			  "instead (for backward compatibility issues when this parameter was previously "
-			  "an absolute error value).", _optimizationMaxError, Parameters::defaultRGBDOptimizeMaxError());
-		_optimizationMaxError = Parameters::defaultRGBDOptimizeMaxError();
-	}
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
@@ -604,6 +615,44 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
 	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);

+	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
+	UASSERT(_markerPriorsLinearVariance>0.0f);
+	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), _markerPriorsAngularVariance);
+	UASSERT(_markerPriorsAngularVariance>0.0f);
+	std::string markerPriorsStr;
+	if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
+	{
+		_markerPriors.clear();
+		std::list<std::string> strList = uSplit(markerPriorsStr, '|');
+		for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
+		{
+			std::string markerStr = *iter;
+			while(!markerStr.empty() && !uIsDigit(markerStr[0]))
+			{
+				markerStr.erase(markerStr.begin());
+			}
+			if(!markerStr.empty())
+			{
+				std::string idStr = uSplitNumChar(markerStr).front();
+				int id = uStr2Int(idStr);
+				Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
+				if(!prior.isNull() && id>0)
+				{
+					_markerPriors.insert(std::make_pair(-id, prior));
+					UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
+				}
+				else
+				{
+					UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
+				}
+			}
+			else if(!iter->empty())
+			{
+				UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
+			}
+		}
+	}
+
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
 	UASSERT(_rgbdLinearSpeedUpdate >= 0.0f);
@@ -972,7 +1021,7 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 		}

 		std::map<int, double> stamps;
-		if(format == 1)
+		if(format == 1 || format == 10 || format == 11)
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -1013,6 +1062,7 @@ void Rtabmap::resetMemory()
 	_optimizeFromGraphEndChanged = false;
 	_globalScanMap.clear();
 	_globalScanMapPoses.clear();
+	_nodesToRepublish.clear();
 	this->clearPath(0);

 	if(_memory)
@@ -1038,23 +1088,32 @@ void Rtabmap::resetMemory()
 class NearestPathKey
 {
 public:
-	NearestPathKey(float l, int i) :
+	NearestPathKey(float l, int i, float d) :
 		likelihood(l),
-		id(i){}
+		id(i),
+		distance(d){}
 	bool operator<(const NearestPathKey & k) const
 	{
 		if(likelihood < k.likelihood)
 		{
 			return true;
 		}
-		else if(likelihood == k.likelihood && id < k.id)
+		else if(likelihood == k.likelihood)
 		{
-			return true;
+			if(distance > k.distance)
+			{
+				return true;
+			}
+			else if(distance == k.distance && id < k.id)
+			{
+				return true;
+			}
 		}
 		return false;
 	}
 	float likelihood;
 	int id;
+	float distance;
 };

 //============================================================
@@ -1366,6 +1425,7 @@ bool Rtabmap::process(
 	bool tooFastMovement = false;
 	std::list<int> signaturesRemoved;
 	bool neighborLinkRefined = false;
+	bool addedNewLandmark = false;
 	if(_rgbdSlamMode)
 	{
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
@@ -1384,32 +1444,45 @@ bool Rtabmap::process(
 				//============================================================
 				// Minimum displacement required to add to Memory
 				//============================================================
-				const std::multimap<int, Link> & links = signature->getLinks();
-				if(links.size() && links.begin()->second.type() == Link::kNeighbor)
+				Transform t;
+
+				if(_memory->isIncremental())
 				{
-					const Signature * s = _memory->getSignature(links.begin()->second.to());
-					UASSERT(s!=0);
-					// don't filter if the new node is not intermediate but previous one is
-					if(signature->getWeight() < 0 || s->getWeight() >= 0)
+					const std::multimap<int, Link> & links = signature->getLinks();
+					if(links.size() && links.begin()->second.type() == Link::kNeighbor)
 					{
-						float x,y,z, roll,pitch,yaw;
-						links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-						bool isMoving = fabs(x) > _rgbdLinearUpdate ||
-										fabs(y) > _rgbdLinearUpdate ||
-										fabs(z) > _rgbdLinearUpdate ||
-									    (_rgbdAngularUpdate>0.0f && (
-											fabs(roll) > _rgbdAngularUpdate ||
-											fabs(pitch) > _rgbdAngularUpdate ||
-											fabs(yaw) > _rgbdAngularUpdate));
-						if(!isMoving)
+						const Signature * s = _memory->getSignature(links.begin()->second.to());
+						UASSERT(s!=0);
+						// don't filter if the new node is not intermediate but previous one is
+						if(signature->getWeight() < 0 || s->getWeight() >= 0)
 						{
-							// This will disable global loop closure detection, only retrieval will be done.
-							// The location will also be deleted at the end.
-							smallDisplacement = true;
-							UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
+							t = links.begin()->second.transform();
 						}
 					}
 				}
+				else if(!_odomCachePoses.empty())
+				{
+					t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
+				}
+				if(!t.isNull())
+				{
+					float x,y,z, roll,pitch,yaw;
+					t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+					bool isMoving = fabs(x) > _rgbdLinearUpdate ||
+									fabs(y) > _rgbdLinearUpdate ||
+									fabs(z) > _rgbdLinearUpdate ||
+									(_rgbdAngularUpdate>0.0f && (
+										fabs(roll) > _rgbdAngularUpdate ||
+										fabs(pitch) > _rgbdAngularUpdate ||
+										fabs(yaw) > _rgbdAngularUpdate));
+					if(!isMoving)
+					{
+						// This will disable global loop closure detection, only retrieval will be done.
+						// The location will also be deleted at the end.
+						smallDisplacement = true;
+						UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
+					}
+				}
 			}
 			if(odomVelocity.size() == 6)
 			{
@@ -1428,7 +1501,8 @@ bool Rtabmap::process(
 			signature->getLinks().size() &&
 			signature->getLinks().begin()->second.type() == Link::kNeighbor &&
 		   _memory->isIncremental() && // ignore pose matching in localization mode
-		   rehearsedId == 0) // don't do it if rehearsal happened
+		   rehearsedId == 0 && // don't do it if rehearsal happened
+		   !tooFastMovement) // ignore if too fast movement has been detected
 		{
 			int oldId = signature->getLinks().begin()->first;
 			const Signature * oldS = _memory->getSignature(oldId);
@@ -1543,6 +1617,8 @@ bool Rtabmap::process(
 				if(_optimizedPoses.find(iter->first) == _optimizedPoses.end())
 				{
 					_optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
+					UDEBUG("Added landmark %d : %s", iter->first, (newPose*iter->second.transform()).prettyPrint().c_str());
+					addedNewLandmark = true;
 				}
 				_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
 			}
@@ -1952,6 +2028,11 @@ bool Rtabmap::process(
 	else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement))
 	{
 		_highestHypothesis = lastHighestHypothesis;
+		UDEBUG("smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0);
+	}
+	else
+	{
+		UDEBUG("Ignoring likelihood and loop closure hypotheses as current signature doesn't have enough visual features.");
 	}

 	//============================================================
@@ -2348,23 +2429,6 @@ bool Rtabmap::process(
 		ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
 	}

-	//============================================================
-	// Landmark
-	//============================================================
-	std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
-	if(!signature->getLandmarks().empty())
-	{
-		for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
-		{
-			if(uContains(_memory->getLandmarksIndex(), iter->first) &&
-					_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
-			{
-				UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
-				landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
-			}
-		}
-	}
-
 	//============================================================
 	// Proximity detections
 	//============================================================
@@ -2446,21 +2510,25 @@ bool Rtabmap::process(
 				UDEBUG("got %d paths", (int)nearestPathsNotSorted.size());
 				// sort nearest paths by highest likelihood (if two have same likelihood, sort by id)
 				std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
+				Transform currentPoseInv = _optimizedPoses.at(signature->id());
 				for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
 				{
 					const std::map<int, Transform> & path = iter->second;
 					float highestLikelihood = 0.0f;
 					int highestLikelihoodId = iter->first;
+					float smallestDistanceSqr = -1;
 					for(std::map<int, Transform>::const_iterator jter=path.begin(); jter!=path.end(); ++jter)
 					{
 						float v = uValue(likelihood, jter->first, 0.0f);
-						if(v > highestLikelihood)
+						float distance = (currentPoseInv * jter->second).getNormSquared();
+						if(v > highestLikelihood || (v == highestLikelihood && (smallestDistanceSqr < 0 || distance < smallestDistanceSqr)))
 						{
 							highestLikelihood = v;
 							highestLikelihoodId = jter->first;
+							smallestDistanceSqr = distance;
 						}
 					}
-					nearestPaths.insert(std::make_pair(NearestPathKey(highestLikelihood, highestLikelihoodId), path));
+					nearestPaths.insert(std::make_pair(NearestPathKey(highestLikelihood, highestLikelihoodId, smallestDistanceSqr), path));
 				}
 				UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);

@@ -2538,17 +2606,20 @@ bool Rtabmap::process(
 									if(_loopClosureHypothesis.first>0 &&
 										nearestIds.find(_loopClosureHypothesis.first)!=nearestIds.end())
 									{
+										// Avoid transform computation on the global loop closure if a visual proximity
+										// one has been detected close (inside proximity radius) to that hypothesis.
 										UDEBUG("Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...",
 												nearestId, _loopClosureHypothesis.first);
+
 										if(nearestId == _loopClosureHypothesis.first)
 										{
 											type = Link::kGlobalClosure;
+											loopIdSuppressedByProximity = nearestId;
+										}
+										else if(loopIdSuppressedByProximity == 0)
+										{
+											loopIdSuppressedByProximity = nearestId;
 										}
-										// In localization mode, avoid transform
-										// computation on the global loop closure if a visual proximity
-										// one has been detected close (inside proximity radius) to that hypothesis.
-										loopIdSuppressedByProximity = _loopClosureHypothesis.first;
-										_loopClosureHypothesis.first = 0;
 									}

 									_memory->addLink(Link(signature->id(), nearestId, type, transform, information));
@@ -2764,56 +2835,63 @@ bool Rtabmap::process(
 	//=============================================================
 	if(_loopClosureHypothesis.first>0)
 	{
-		//Compute transform if metric data are present
-		Transform transform;
-		RegistrationInfo info;
-		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
-		if(_rgbdSlamMode)
+		if(loopIdSuppressedByProximity==0)
 		{
-			transform = _memory->computeTransform(
-					_loopClosureHypothesis.first,
-					signature->id(),
-					_loopClosureIdentityGuess?Transform::getIdentity():Transform(),
-					&info);
+			//Compute transform if metric data are present
+			Transform transform;
+			RegistrationInfo info;
+			info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+			if(_rgbdSlamMode)
+			{
+				transform = _memory->computeTransform(
+						_loopClosureHypothesis.first,
+						signature->id(),
+						_loopClosureIdentityGuess?Transform::getIdentity():Transform(),
+						&info);

-			loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
-			loopClosureVisualInliersDistribution = info.inliersDistribution;
+				loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
+				loopClosureVisualInliersDistribution = info.inliersDistribution;

-			loopClosureVisualInliers = info.inliers;
-			loopClosureVisualInliersRatio = info.inliersRatio;
-			loopClosureVisualMatches = info.matches;
-			rejectedGlobalLoopClosure = transform.isNull();
-			if(rejectedGlobalLoopClosure)
-			{
-				UWARN("Rejected loop closure %d -> %d: %s",
-						_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
+				loopClosureVisualInliers = info.inliers;
+				loopClosureVisualInliersRatio = info.inliersRatio;
+				loopClosureVisualMatches = info.matches;
+				rejectedGlobalLoopClosure = transform.isNull();
+				if(rejectedGlobalLoopClosure)
+				{
+					UWARN("Rejected loop closure %d -> %d: %s",
+							_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
+				}
+				else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
+				{
+					rejectedGlobalLoopClosure = true;
+					UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
+							_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
+				}
+				else
+				{
+					transform = transform.inverse();
+				}
 			}
-			else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
-			{
-				rejectedGlobalLoopClosure = true;
-				UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
-						_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
-			}
-			else
-			{
-				transform = transform.inverse();
-			}
-		}
-		if(!rejectedGlobalLoopClosure)
-		{
-			// Make the new one the parent of the old one
-			UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
-			cv::Mat information = getInformation(info.covariance);
-			loopClosureLinearVariance = 1.0/information.at<double>(0,0);
-			loopClosureAngularVariance = 1.0/information.at<double>(5,5);
-			rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
 			if(!rejectedGlobalLoopClosure)
 			{
-				loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
+				// Make the new one the parent of the old one
+				UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+				cv::Mat information = getInformation(info.covariance);
+				loopClosureLinearVariance = 1.0/information.at<double>(0,0);
+				loopClosureAngularVariance = 1.0/information.at<double>(5,5);
+				rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
+				if(!rejectedGlobalLoopClosure)
+				{
+					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
+				}
+			}
+
+			if(rejectedGlobalLoopClosure)
+			{
+				_loopClosureHypothesis.first = 0;
 			}
 		}
-
-		if(rejectedGlobalLoopClosure)
+		else if(loopIdSuppressedByProximity != _loopClosureHypothesis.first)
 		{
 			_loopClosureHypothesis.first = 0;
 		}
@@ -2822,6 +2900,42 @@ bool Rtabmap::process(
 	timeAddLoopClosureLink = timer.ticks();
 	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);

+	//============================================================
+	// Landmark
+	//============================================================
+	std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
+	if(!signature->getLandmarks().empty())
+	{
+		bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
+		UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
+		for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
+		{
+			if(uContains(_memory->getLandmarksIndex(), iter->first) &&
+					_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
+			{
+				if(!_memory->isIncremental() &&          // In localization mode
+					!hasGlobalLoopClosuresInOdomCache && // If there are global loop closures in odom cache, we can keep far landmarks
+					_localRadius>0.0 &&
+					iter->second.transform().getNormSquared() > _localRadius*_localRadius)
+				{
+					// Ignore landmark detections over local radius
+					UWARN("Ignoring landmark %d for localization as it is too far (%fm > %s=%f) "
+							"and odom cache doesn't contain global loop closure(s).",
+							iter->first,
+							iter->second.transform().getNorm(),
+							Parameters::kRGBDLocalRadius().c_str(),
+							_localRadius);
+				}
+				else
+				{
+					UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
+					landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
+					rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
+				}
+			}
+		}
+	}
+
 	//============================================================
 	// Add virtual links if a path is activated
 	//============================================================
@@ -2860,6 +2974,7 @@ bool Rtabmap::process(
 	cv::Mat localizationCovariance;
 	Transform previousMapCorrection;
 	bool rejectedLandmark = false;
+	bool delayedLocalization = false;
 	UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
 	UDEBUG("Incremental: %d", _memory->isIncremental());
 	UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -2906,6 +3021,16 @@ bool Rtabmap::process(
 			}
 		}

+		bool allLocalizationLinksInGraph = !localizationLinks.empty();
+		for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
+		{
+			if(!uContains(_optimizedPoses, iter->first))
+			{
+				allLocalizationLinksInGraph = false;
+				break;
+			}
+		}
+
 		// Note that in localization mode, we don't re-optimize the graph
 		// if:
 		//  1- there are no signatures retrieved,
@@ -2913,7 +3038,7 @@ bool Rtabmap::process(
 		if(!_memory->isIncremental() &&
 		   signaturesRetrieved.empty() &&
 		   !localizationLinks.empty() &&
-		   uContains(_optimizedPoses, localizationLinks.rbegin()->first))
+		   allLocalizationLinksInGraph)
 		{
 			bool rejectLocalization = _odomCachePoses.empty();
 			if(!_odomCachePoses.empty())
@@ -3054,7 +3179,7 @@ bool Rtabmap::process(
 				}

 				bool hasGlobalLoopClosuresOrLandmarks = false;
-				if(rejectLocalization)
+				if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
 				{
 					// Let's try again without local loop closures
 					localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
@@ -3215,8 +3340,13 @@ bool Rtabmap::process(
 						UDEBUG("  to %s", newT.prettyPrint().c_str());
 						iter->second.setTransform(newT);

+						// Update link in the referred signatures
+						if(iter->first > 0)
+							_memory->updateLink(iter->second, false);
+
 						_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
 					}
+
 					_odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end());

 					// At least 2 localizations at 2 different time required
@@ -3246,7 +3376,7 @@ bool Rtabmap::process(
                                            !landmarksDetected.at(landmarkId).empty());
                                    loopId = *landmarksDetected.at(landmarkId).begin();
                                }
-                                
+
                                const Signature * loopS = _memory->getSignature(loopId);
                                UASSERT(loopS !=0);
                                std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
@@ -3335,6 +3465,7 @@ bool Rtabmap::process(
 					else //delayed localization (wait for more than 1 link)
 					{
 						UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
+						delayedLocalization = true;
 						rejectLocalization = true;
 					}
 				}
@@ -3546,6 +3677,7 @@ bool Rtabmap::process(

 	// Posterior is empty if a bad signature is detected
 	float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
+	int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;

 	// prepare statistics
 	if(_loopClosureHypothesis.first || _publishStats)
@@ -3560,6 +3692,7 @@ bool Rtabmap::process(
 			statistics_.setLoopClosureMapId(_memory->getMapId(_loopClosureHypothesis.first));
 			ULOGGER_INFO("Loop closure detected! With id=%d", _loopClosureHypothesis.first);
 		}
+
 		if(_publishStats)
 		{
 			ULOGGER_INFO("send all stats...");
@@ -3601,7 +3734,6 @@ bool Rtabmap::process(
 			statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
 			statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));

-			int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
 			statistics_.addStatistic(Statistics::kLoopId(), loopId);
 			statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);

@@ -3726,6 +3858,8 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
 			statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
 			statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
+			statistics_.addStatistic(Statistics::kMemoryNew_landmark(), addedNewLandmark?1.0f:0);
+
 			if(_publishRAMUsage)
 			{
 				UTimer ramTimer;
@@ -3784,6 +3918,13 @@ bool Rtabmap::process(
 		_memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
 	}

+	// Localization mode and saving localization data: save odometry covariance in a prior link
+	// so that DBReader can republish the covariance of localization data
+	if(!_memory->isIncremental() && _memory->isLocalizationDataSaved() && !odomCovariance.empty())
+	{
+		_memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
+	}
+
 	// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
 	int signatureRemoved = _memory->cleanup();
 	if(signatureRemoved)
@@ -3817,7 +3958,11 @@ bool Rtabmap::process(
 			signaturesRemoved.push_back(signature->id());
 			_memory->deleteLocation(signature->id());
 		}
-		else if((smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
+		else if((smallDisplacement || tooFastMovement) &&
+				_loopClosureHypothesis.first == 0 &&
+				lastProximitySpaceClosureId == 0 &&
+				(rejectedLandmark || landmarksDetected.empty()) &&
+				!addedNewLandmark)
 		{
 			// Don't delete the location if a loop closure is detected
 			UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
@@ -3834,10 +3979,22 @@ bool Rtabmap::process(
 	else if(!_memory->isIncremental() &&
 			(smallDisplacement || tooFastMovement) &&
 			_loopClosureHypothesis.first == 0 &&
-			lastProximitySpaceClosureId == 0)
+			lastProximitySpaceClosureId == 0 &&
+			!delayedLocalization &&
+			(rejectedLandmark || landmarksDetected.empty()))
 	{
 		_odomCachePoses.erase(signatureRemoved);
-		_odomCacheConstraints.erase(signatureRemoved);
+		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
+		{
+			if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved)
+			{
+				_odomCacheConstraints.erase(iter++);
+			}
+			else
+			{
+				++iter;
+			}
+		}
 	}

 	// Pass this point signature should not be used, since it could have been transferred...
@@ -4041,7 +4198,71 @@ bool Rtabmap::process(
 		if(_publishLastSignatureData)
 		{
 			UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
-			statistics_.setLastSignatureData(lastSignatureData);
+			statistics_.addSignatureData(lastSignatureData);
+
+			if(_nodesToRepublish.size())
+			{
+				std::multimap<int, int> missingIds;
+
+				// priority to loopId
+				int tmpId = loopId>0?loopId:_highestHypothesis.first;
+				if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end())
+				{
+					missingIds.insert(std::make_pair(-1, tmpId));
+				}
+
+				if(!_lastLocalizationPose.isNull())
+				{
+					// Republish data from closest nodes of the current localization
+					std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
+					int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose);
+					if(id>0)
+					{
+						std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true, false, true);
+						for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+						{
+							if(iter->first != loopId &&
+									_nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
+							{
+								missingIds.insert(std::make_pair(iter->second, iter->first));
+							}
+						}
+
+						if(_nodesToRepublish.size() != missingIds.size())
+						{
+							// remove requested nodes not anymore in the graph
+							for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
+							{
+								if(ids.find(*iter) == ids.end())
+								{
+									iter = _nodesToRepublish.erase(iter);
+								}
+								else
+								{
+									++iter;
+								}
+							}
+						}
+					}
+				}
+
+				int loaded = 0;
+				std::stringstream stream;
+				for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter)
+				{
+					statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true));
+					_nodesToRepublish.erase(iter->second);
+					++loaded;
+					stream << iter->second << " ";
+				}
+				if(loaded)
+				{
+					UWARN("Republishing data of requested node(s) %s(%s=%d)",
+							stream.str().c_str(),
+							Parameters::kRtabmapMaxRepublished().c_str(),
+							_maxRepublished);
+				}
+			}
 		}
 		else
 		{
@@ -4061,7 +4282,7 @@ bool Rtabmap::process(
 			}
 			nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
 			nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
-			statistics_.setLastSignatureData(nodeInfo);
+			statistics_.addSignatureData(nodeInfo);
 		}
 		UDEBUG("");
 		localGraphSize = (int)poses.size();
@@ -4671,6 +4892,20 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
 	UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());

+	// add landmark priors if there are some
+	for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter)
+	{
+		if(_markerPriors.find(iter->first) != _markerPriors.end())
+		{
+			cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
+			infMatrix(cv::Range(0,3), cv::Range(0,3)) /= _markerPriorsLinearVariance;
+			infMatrix(cv::Range(3,6), cv::Range(3,6)) /= _markerPriorsAngularVariance;
+			edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPosePrior, _markerPriors.at(iter->first), infMatrix)));
+			UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", iter->first, _markerPriors.at(iter->first).prettyPrint().c_str(),
+					_markerPriorsLinearVariance, _markerPriorsAngularVariance);
+		}
+	}
+
 	if(_graphOptimizer->iterations() > 0)
 	{
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -4697,7 +4932,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	}
 	else
 	{
-		bool hasLandmarks = edgeConstraints.begin()->first < 0;
+		bool hasLandmarks = !edgeConstraints.empty() && edgeConstraints.begin()->first < 0;
 		if(poses.size() != guessPoses.size() || hasLandmarks)
 		{
 			UDEBUG("recompute poses using only links (robust to multi-session)");
@@ -4885,10 +5120,10 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 		if(!images && withWords)
 		{
 			std::vector<CameraModel> models;
-			StereoCameraModel stereoModel;
-			_memory->getNodeCalibration(id, models, stereoModel);
+			std::vector<StereoCameraModel> stereoModels;
+			_memory->getNodeCalibration(id, models, stereoModels);
 			data.setCameraModels(models);
-			data.setStereoCameraModel(stereoModel);
+			data.setStereoCameraModels(stereoModels);
 		}

 		s=Signature(id,
@@ -5180,7 +5415,7 @@ int Rtabmap::detectMoreLoopClosures(
 							UASSERT(signatures.find(to) != signatures.end());

 							Transform guess;
-							if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
+							if(_proximityBySpace && uContains(poses, from) && uContains(poses, to))
 							{
 								guess = poses.at(from).inverse() * poses.at(to);
 							}
@@ -5520,7 +5755,7 @@ bool Rtabmap::addLink(const Link & link)
 	}
 	if(t.isNull())
 	{
-		UERROR("Link's transform is null!");
+		UERROR("Link's transform is null! (%d->%d type=%s)", link.from(), link.to(), link.typeName().c_str());
 		return false;
 	}
 	if(_memory->isIncremental())
@@ -5856,6 +6091,26 @@ cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
 	return information;
 }

+void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
+{
+	if(ids.empty())
+	{
+		_nodesToRepublish.clear();
+	}
+	else if(_maxRepublished > 0 && _publishLastSignatureData)
+	{
+		_nodesToRepublish.insert(ids.begin(), ids.end());
+	}
+	else if(_maxRepublished == 0)
+	{
+		UWARN("%s=0, so cannot republish the %d requested nodes.", Parameters::kRtabmapMaxRepublished().c_str(), (int)ids.size());
+	}
+	else //_publishLastSignatureData=false
+	{
+		UWARN("%s=false, so cannot republish the %d requested nodes.", Parameters::kRtabmapPublishLastSignature().c_str(), (int)ids.size());
+	}
+}
+
 void Rtabmap::clearPath(int status)
 {
 	UINFO("status=%d", status);
--- a/corelib/src/RtabmapThread.cpp
+++ b/corelib/src/RtabmapThread.cpp
@@ -66,14 +66,14 @@ RtabmapThread::~RtabmapThread()
 	delete _frameRateTimer;
 }

-void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
+void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
 {
 	ULOGGER_DEBUG("to %d", newState);

 	_stateMutex.lock();
 	{
 		_state.push(newState);
-		_stateParam.push(parameters);
+		_stateParam.push(cmdEvent);
 	}
 	_stateMutex.unlock();

@@ -180,7 +180,7 @@ void RtabmapThread::mainLoopKill()
 void RtabmapThread::mainLoop()
 {
 	State state = kStateDetecting;
-	ParametersMap parameters;
+	RtabmapEventCmd cmdEvent(RtabmapEventCmd::kCmdUndef);

 	_stateMutex.lock();
 	{
@@ -188,7 +188,7 @@ void RtabmapThread::mainLoop()
 		{
 			state = _state.front();
 			_state.pop();
-			parameters = _stateParam.front();
+			cmdEvent = _stateParam.front();
 			_stateParam.pop();
 		}
 	}
@@ -198,110 +198,161 @@ void RtabmapThread::mainLoop()
 	cv::Mat userData;
 	UTimer timer;
 	std::string str;
+	RtabmapEventCmd::Cmd cmd = cmdEvent.getCmd();
 	switch(state)
 	{
 	case kStateDetecting:
 		this->process();
 		break;
-	case kStateInit:
-		UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
-		str = parameters.at("RtabmapThread/DatabasePath");
-		parameters.erase("RtabmapThread/DatabasePath");
-		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
-		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
-		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_rate >= 0.0f);
-		_rtabmap->init(parameters, str);
-		break;
-	case kStateChangingParameters:
-		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
-		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
-		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_rate >= 0.0f);
-		_rtabmap->parseParameters(parameters);
-		break;
-	case kStateReseting:
-		_rtabmap->resetMemory();
-		this->clearBufferedData();
-		break;
-	case kStateClose:
-		if(_dataBuffer.size())
+	case kStateProcessCommand:
+		if(cmd == RtabmapEventCmd::kCmdInit)
 		{
-			UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
+			ULOGGER_DEBUG("CMD_INIT");
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
+			UASSERT(_rate >= 0.0f);
+			_rtabmap->init(cmdEvent.getParameters(), cmdEvent.value1().toStr());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdClose)
+		{
+			ULOGGER_DEBUG("CMD_CLOSE");
+			if(_dataBuffer.size())
+			{
+				UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
+				this->clearBufferedData();
+			}
+			_rtabmap->close(cmdEvent.value1().toBool(), cmdEvent.value2().toStr());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdUpdateParams)
+		{
+			ULOGGER_DEBUG("CMD_UPDATE_PARAMS");
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
+			Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
+			UASSERT(_rate >= 0.0f);
+			_rtabmap->parseParameters(cmdEvent.getParameters());
+			break;
+		}
+		else if(cmd == RtabmapEventCmd::kCmdResetMemory)
+		{
+			ULOGGER_DEBUG("CMD_RESET_MEMORY");
+			_rtabmap->resetMemory();
 			this->clearBufferedData();
 		}
-		_rtabmap->close(uStr2Bool(parameters.at("saved")), parameters.at("outputPath"));
-		break;
-	case kStateDumpingMemory:
-		_rtabmap->dumpData();
-		break;
-	case kStateDumpingPrediction:
-		_rtabmap->dumpPrediction();
-		break;
-	case kStateExportingDOTGraph:
-		_rtabmap->generateDOTGraph(
-				parameters.at("path"),
-				atoi(parameters.at("id").c_str()),
-				atoi(parameters.at("margin").c_str()));
-		break;
-	case kStateExportingPoses:
-		_rtabmap->exportPoses(
-				parameters.at("path"),
-				uStr2Bool(parameters.at("optimized")),
-				uStr2Bool(parameters.at("global")),
-				atoi(parameters.at("type").c_str()));
-		break;
-	case kStateCleanDataBuffer:
-		this->clearBufferedData();
-		break;
-	case kStatePublishingMap:
-		this->publishMap(
-				uStr2Bool(parameters.at("optimized")),
-				uStr2Bool(parameters.at("global")),
-				uStr2Bool(parameters.at("graph_only")));
-		break;
-	case kStateTriggeringMap:
-		_rtabmap->triggerNewMap();
-		break;
-	case kStateSettingGoal:
-		id = atoi(parameters.at("id").c_str());
-		if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
+		else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
 		{
-			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
-			if(id <= 0)
+			ULOGGER_DEBUG("CMD_DUMP_MEMORY");
+			_rtabmap->dumpData();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
+		{
+			ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
+			_rtabmap->dumpPrediction();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
+		{
+			ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
+			_rtabmap->generateDOTGraph(
+					cmdEvent.value2().toStr(),
+					cmdEvent.value1().toBool()?0:cmdEvent.value3().toInt(),
+					cmdEvent.value1().toBool()?0:cmdEvent.value4().toInt());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdExportPoses)
+		{
+			ULOGGER_DEBUG("CMD_EXPORT_POSES");
+			_rtabmap->exportPoses(
+					cmdEvent.value3().toStr(),
+					cmdEvent.value2().toBool(),
+					cmdEvent.value1().toBool(),
+					cmdEvent.value4().toInt());
+
+		}
+		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
+		{
+			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
+			this->clearBufferedData();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
+		{
+			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
+			this->publishMap(
+					cmdEvent.value2().toBool(),
+					cmdEvent.value1().toBool(),
+					cmdEvent.value3().toBool());
+		}
+		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
+		{
+			ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
+			_rtabmap->triggerNewMap();
+		}
+		else if(cmd == RtabmapEventCmd::kCmdPause)
+		{
+			ULOGGER_DEBUG("CMD_PAUSE");
+			_paused = !_paused;
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGoal)
+		{
+			ULOGGER_DEBUG("CMD_GOAL");
+			if(cmdEvent.value1().isStr() && !cmdEvent.value1().toStr().empty() && _rtabmap->getMemory())
 			{
-				UERROR("Failed to find a node with label \"%s\".", parameters.at("label").c_str());
+				id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr());
+				if(id <= 0)
+				{
+					UERROR("Failed to find a node with label \"%s\".", cmdEvent.value1().toStr().c_str());
+				}
+			}
+			else if(cmdEvent.value1().isInt() || cmdEvent.value1().isUInt())
+			{
+				id = cmdEvent.value1().toInt();
+			}
+
+			if(id < 0)
+			{
+				UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
+			}
+			timer.start();
+			if(id > 0 && !_rtabmap->computePath(id, true))
+			{
+				UERROR("Failed to compute a path to goal %d.", id);
+			}
+			this->post(new RtabmapGlobalPathEvent(
+					id,
+					cmdEvent.value1().isStr()?cmdEvent.value1().toStr():"",
+					_rtabmap->getPath(),
+					timer.elapsed()));
+			break;
+		}
+		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
+		{
+			ULOGGER_DEBUG("CMD_CANCEL_GOAL");
+			_rtabmap->clearPath(0);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdLabel)
+		{
+			ULOGGER_DEBUG("CMD_LABEL");
+			if(!_rtabmap->labelLocation(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()))
+			{
+				this->post(new RtabmapLabelErrorEvent(cmdEvent.value2().toInt(), cmdEvent.value1().toStr()));
 			}
 		}
-		else if(id < 0)
+		else if(cmd == RtabmapEventCmd::kCmdRemoveLabel)
 		{
-			UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
+			ULOGGER_DEBUG("CMD_REMOVE_LABEL");
+			id = _rtabmap->getMemory()->getSignatureIdByLabel(cmdEvent.value1().toStr(), true);
+			if(id <= 0 || !_rtabmap->labelLocation(id, ""))
+			{
+				this->post(new RtabmapLabelErrorEvent(id, cmdEvent.value1().toStr()));
+			}
 		}
-		timer.start();
-		if(id > 0 && !_rtabmap->computePath(id, true))
+		else if(cmd == RtabmapEventCmd::kCmdRepublishData)
 		{
-			UERROR("Failed to compute a path to goal %d.", id);
+			ULOGGER_DEBUG("CMD_REPUBLISH_DATA");
+			_rtabmap->addNodesToRepublish(cmdEvent.value1().toIntArray());
 		}
-		this->post(new RtabmapGlobalPathEvent(
-				id,
-				parameters.at("label"),
-				_rtabmap->getPath(),
-				timer.elapsed()));
-		break;
-	case kStateCancellingGoal:
-		_rtabmap->clearPath(0);
-		break;
-	case kStateLabelling:
-		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label")))
+		else
 		{
-			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label")));
-		}
-		break;
-	case kStateRemovingLabel:
-		id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"), true);
-		if(!_rtabmap->labelLocation(id, ""))
-		{
-			this->post(new RtabmapLabelErrorEvent(id, parameters.at("label")));
+			UWARN("Cmd %d unknown!", cmd);
 		}
 		break;
 	default:
@@ -397,127 +448,12 @@ bool RtabmapThread::handleEvent(UEvent* event)
 		else if(event->getClassName().compare("RtabmapEventCmd") == 0)
 		{
 			RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
-			RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
-			if(cmd == RtabmapEventCmd::kCmdInit)
-			{
-				ULOGGER_DEBUG("CMD_INIT");
-				ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
-				UASSERT(rtabmapEvent->value1().isStr());
-				UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
-				pushNewState(kStateInit, parameters);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdClose)
-			{
-				ULOGGER_DEBUG("CMD_CLOSE");
-				UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
-				ParametersMap param;
-				param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
-				param.insert(ParametersPair("outputPath", rtabmapEvent->value2().toStr()));
-				pushNewState(kStateClose, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdResetMemory)
-			{
-				ULOGGER_DEBUG("CMD_RESET_MEMORY");
-				pushNewState(kStateReseting);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
-			{
-				ULOGGER_DEBUG("CMD_DUMP_MEMORY");
-				pushNewState(kStateDumpingMemory);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
-			{
-				ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
-				pushNewState(kStateDumpingPrediction);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
-			{
-				ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
-				UASSERT(rtabmapEvent->value1().isBool());
-				UASSERT(rtabmapEvent->value2().isStr());
-				UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
-				UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
-				param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
-				param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
-				pushNewState(kStateExportingDOTGraph, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdExportPoses)
-			{
-				ULOGGER_DEBUG("CMD_EXPORT_POSES");
-				UASSERT(rtabmapEvent->value1().isBool());
-				UASSERT(rtabmapEvent->value2().isBool());
-				UASSERT(rtabmapEvent->value3().isStr());
-				UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
-				param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
-				pushNewState(kStateExportingPoses, param);
-
-			}
-			else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
-			{
-				ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
-				pushNewState(kStateCleanDataBuffer);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
-			{
-				ULOGGER_DEBUG("CMD_PUBLISH_MAP");
-				UASSERT(rtabmapEvent->value1().isBool());
-				UASSERT(rtabmapEvent->value2().isBool());
-				UASSERT(rtabmapEvent->value3().isBool());
-				ParametersMap param;
-				param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
-				param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
-				pushNewState(kStatePublishingMap, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
-			{
-				ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
-				pushNewState(kStateTriggeringMap);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdPause)
-			{
-				ULOGGER_DEBUG("CMD_PAUSE");
-				_paused = !_paused;
-			}
-			else if(cmd == RtabmapEventCmd::kCmdGoal)
-			{
-				ULOGGER_DEBUG("CMD_GOAL");
-				UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
-				param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
-				pushNewState(kStateSettingGoal, param);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
-			{
-				ULOGGER_DEBUG("CMD_CANCEL_GOAL");
-				pushNewState(kStateCancellingGoal);
-			}
-			else if(cmd == RtabmapEventCmd::kCmdLabel)
-			{
-				ULOGGER_DEBUG("CMD_LABEL");
-				UASSERT(rtabmapEvent->value1().isStr());
-				UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
-				ParametersMap param;
-				param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
-				param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
-				pushNewState(kStateLabelling, param);
-			}
-			else
-			{
-				UWARN("Cmd %d unknown!", cmd);
-			}
+			pushNewState(kStateProcessCommand, *rtabmapEvent);
 		}
 		else if(event->getClassName().compare("ParamEvent") == 0)
 		{
 			ULOGGER_DEBUG("changing parameters");
-			pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
+			pushNewState(kStateProcessCommand, RtabmapEventCmd(RtabmapEventCmd::kCmdUpdateParams, ((ParamEvent*)event)->getParameters()));
 		}
 	}
 	return false;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	95e6a9f039	bump 0.20.23	2023-01-21 16:09:41 -08:00
matlabbe	b5eef4b86a	DBViewer: ignore marker constraints in refine, add marker priors when optimizing the graph	2023-01-21 15:52:01 -08:00
reito	40ab33031b	fix bigobj under MSVC, fix yaml-cpp link. (#957 )	2023-01-21 15:50:02 -08:00
reito	0e908206d0	OpenCV 4.7 moved aruco back to opencv repo, and makes some breaking changes. (#955 )	2023-01-16 16:41:48 -08:00
matlabbe	6b9f7de782	Fixed a -pedantic warning	2023-01-16 14:14:25 -08:00
matlabbe	22fede9335	Reprocess: added --odom option to recompute odometry.	2023-01-15 15:07:24 -08:00
matlabbe	22917cc1c3	VTK9: Fixed vtkGUISupportQt -> VTK::GUISupportQt	2023-01-14 20:12:33 -08:00
matlabbe	98bf3fb184	workflows: removed galactic build (EOF)	2023-01-08 16:59:24 -08:00
matlabbe	8d301afe6c	reprocess: added --default option	2023-01-08 16:29:44 -08:00
matlabbe	e3ceb8a572	VINS-Fusion: Fixed error wrongly logged when only imu is processed by vins.	2023-01-08 15:22:46 -08:00
matlabbe	f12cc83fc2	RegVis: using toSignature camera model for guess reprojection on optical flow mode (#947 )	2022-12-20 20:51:56 -08:00
matlabbe	4a6a765cd1	Fixed #944	2022-12-14 19:42:56 -08:00
matlabbe	a4761ad397	Fixed #940	2022-12-11 01:12:21 -08:00
matlabbe	aa380de2a7	Update install_deps.sh Removed vtk default iOS simulator build	2022-12-07 17:59:11 -08:00
matlabbe	467dc738c7	Fixed deprecated warning not set to right function	2022-12-05 20:32:53 -08:00
matlabbe	64cb7fb6fe	export: when --images and --cam_projection are used, we export depth image from lidar projection to RGB frames	2022-12-04 17:23:33 -08:00
matlabbe	8a8c360f89	fixed compilation warning	2022-11-27 16:20:56 -08:00
matlabbe	83693ed1b9	detectMoreLoopClosures: fixed guess not set for lidar-only registration (https://github.com/introlab/rtabmap_ros/issues/727 )	2022-11-26 20:24:43 -08:00
matlabbe	860c14e0ec	rtabmap: fixed export poses in format 10 and 11 (#883 )	2022-11-26 12:42:04 -08:00
matlabbe	85bc1ee27f	GTSAM: return empty optimized poses in case of exception or error. g2o: don't assert on marginal not computed, log error and reutn empty optimized poses.	2022-11-22 12:26:02 -08:00
matlabbe	05676d1577	ios: Moved install_deps.sh in upper directory to easy cleanup Libraries. Fixed uNumber2Str(double) link error.	2022-11-19 12:41:01 -08:00
matlabbe	6d713c2ccb	uNumberToStr(double): added "fixed" param (default false) to fix ros param double/int conversion issue.	2022-11-11 16:20:59 -08:00
matlabbe	16e3314e64	💄	2022-11-11 14:45:33 -08:00
matlabbe	d59a306ae3	Export: fixed depth image wrongly set for stereo data	2022-11-10 17:18:57 -08:00
matlabbe	bdfab4e0be	reprocess: added "-a" option (append mode)	2022-11-02 15:06:13 -07:00
matlabbe	c9d50ef445	Export GPS poses: set 8 decimals (1 mm accuracy) https://github.com/introlab/rtabmap_ros/issues/833	2022-11-02 11:35:54 -07:00
matlabbe	2889cf8048	GUI: fixed latest data requested on localization mode	2022-10-31 13:21:13 -07:00
matlabbe	def876983d	docker/android: fixed docker not detecting compilation failure. ios: added exit flag for install_deps.sh script	2022-10-30 15:07:55 -07:00
matlabbe	69214b927a	fixed android build	2022-10-30 14:47:34 -07:00
matlabbe	67b2cbe5ca	docker: set dummy build-args for build not required them	2022-10-30 14:39:33 -07:00
matlabbe	86f66cc8b8	Added missing signature info in republished data	2022-10-30 13:59:08 -07:00
matlabbe	d70924a3a4	bump package version 0.20.22	2022-10-28 12:30:01 -07:00
matlabbe	1297714271	Bump version 0.20.22. Refactored RtabmapThread commands handling. MainWindow/RtabmapThread/Statistics/UVariant: added new option to republish missing data on GUI side. MainWindow: fixed 1 sec lag when waypoints are used. Parameters: added Rtabmap/MaxRepublished (moved from rtabmap_ros). OccupancyGrid: fixed ground cells ignored if there are empty cells, also don't add pose in addedNodes if corresponding node was not in cache.	2022-10-28 12:25:48 -07:00
matlabbe	8e30c1c812	GraphView: added user link on inter/intra loop closure color scheme option	2022-10-24 17:45:16 -07:00
matlabbe	df888962f4	depthai: fixed app freezing if oakd device (oak-d lite) doesn't have imu and imu is requested.	2022-10-24 11:13:27 -07:00
matlabbe	a8cdf1f940	reprocess: disable Mem/UseOdomFeatures by default when there are more than 1 input database	2022-10-24 10:34:37 -07:00
matlabbe	769ace86ce	Added oak-d lite menu option (depthai: imu is now optionnal)	2022-10-23 18:15:23 -07:00
matlabbe	97a150b180	DbViewer: added option to regenerate optimized 2d map from OctoMap (https://github.com/introlab/rtabmap_ros/issues/213 )	2022-10-23 12:16:35 -07:00
matlabbe	6e8f43916c	floam: republish input scan with features	2022-10-16 21:31:43 -07:00
matlabbe	5b0047efad	docker: fixed AliceVision build (bionic/focal)	2022-10-15 14:06:20 -07:00
matlabbe	fdb78d33b0	DBViewer: added optimzed and prior poses in node details. Reprocess: add start_s, stop_s and pub_loops options.	2022-10-12 13:05:01 -07:00
matlabbe	2cbd43dfb6	Update cmake-ros.yml Added ros2 distros	2022-10-09 12:47:03 -07:00
matlabbe	1f06386f37	Update README.md	2022-10-09 12:22:09 -07:00
matlabbe	1e9b090624	Gui: Added re-localization colors on odom cache graph. Added frame labels (map, odom).	2022-10-04 16:27:33 -07:00
matlabbe	7e3e5ec50b	DBViewer: fixed matching lines when two images don't have the same number of cameras. Fixed nodes with w=-9 can be used to add constraint (in case localization data is saved in database). Add constraint: compute transform in order of the sliders.	2022-10-04 14:23:12 -07:00
matlabbe	8d23f19f6c	bump ros package version	2022-10-01 18:30:51 -07:00
matlabbe	9afbcf2d06	detectMoreLoopClosures tool: regenerate global occupancy grid for convenience if there was one before	2022-09-30 15:48:09 -07:00
matlabbe	30bf5895ca	Fixed #909	2022-09-30 14:17:23 -07:00
matlabbe	e0a2adcb45	Updated latest jfr2018 docker	2022-09-27 16:37:25 -07:00
matlabbe	87faea7a85	Fixed ceres linker issue with floam dependency (if WITH_CERES is OFF)	2022-09-27 23:35:13 +00:00
matlabbe	74c6e9cfec	Fixed compilation error about stereoCameraModel() not found (#905 ) with OpenVINS odometry	2022-09-27 13:48:57 -07:00
matlabbe	675dad5f76	Fixed compilation error about stereoCameraModel() not found (#905 ) with odometry approaches: Fovis, MSCKF, Okvis, VINS, Viso2.	2022-09-27 18:56:05 +00:00
matlabbe	55546132a0	Fixed build on 16.04	2022-09-27 17:22:52 +00:00
matlabbe	fa31affea0	Pnp multicam refactoring (#902 ) * gui: fixed wrongly showing landmark rejected when it was not (because a loop closure was rejected at the same time) * Added Vis/PnPMaxVariance and RGBD/InvertedReg parameters. Implemented inlier distribution computation for multicam. * On loc/small displacement: don't remove from odom cache if loop is rejected (maybe first loc) * Loc: don't prune odom cache on small movement if delayed loc is enabled * loc/small movement: cleanup bidirectional links * Cov/PnP: fixed objPt transform to estimate depth Co-authored-by: mathieu86 <mathieu@robust.ai>	2022-09-24 12:29:42 -07:00
matlabbe	95a76cb696	gui: fixed wrongly showing landmark rejected when it was not (because a loop closure was rejected at the same time)	2022-09-22 15:16:08 -07:00
matlabbe	737675c6f1	fixed dereference end iterator assert (#900 )	2022-09-19 07:34:48 -07:00
matlabbe	4d6bc78e3d	implement multi camera marker detection #898 (#899 ) * Added multicamera marker detection support * Fixed marker detection on camera index> 0 Co-authored-by: mathieu86 <mathieu@robust.ai>	2022-09-14 17:13:59 -07:00
matlabbe	adfb250d4e	2022-IlluminationInvariant: set RGBD/OptimizeMaxError to 0 to make it easier to reproduce results of the paper	2022-09-13 19:58:55 -07:00
matlabbe	a14b39b953	fixed android docker file not found on CI	2022-09-13 00:45:39 -07:00
matlabbe	3dd0965a15	Updated archive/2022-IlluminationInvariant/README.md	2022-09-13 00:29:44 -07:00
matlabbe	e059ead7f5	Merge branch 'master' of https://github.com/introlab/rtabmap	2022-09-12 23:22:59 -07:00
matlabbe	749cd096ff	Fixed map::at() seg fault when localizing on a database with multiple disjoint sessions	2022-09-13 06:19:23 +00:00
matlabbe	37b920396c	Added docker image for frontiers2022 (updated README in archive/2022-IlluminationInvariant)	2022-09-11 13:03:32 -07:00
matlabbe	0dc5fbdd70	fixed #873	2022-09-06 16:19:20 -07:00
matlabbe	f1c987a0ce	Fixed android build (res tool not found)	2022-09-04 14:29:59 -04:00
matlabbe	69b0caed6f	Update README.md	2022-09-04 13:39:40 -04:00
Windel Bouwman	8826f136a9	Use imported target for res_tool during cross compilation. (#865 )	2022-09-04 13:18:14 -04:00
matlabbe	8cd4a6feff	OdomF2M: fixed lidar-only broken from commit `9797918d52`	2022-07-28 11:48:55 -04:00
matlabbe	33e54430a1	bump version 0.20.20 (api changes on CameraModel)	2022-07-28 10:51:13 -04:00
matlabbe	9797918d52	fixed https://github.com/introlab/rtabmap_ros/issues/790	2022-07-27 10:15:21 -04:00
matlabbe	aa3b71dbf6	docker: added missing wget dependency for arm64 (opengv dpendency)	2022-07-20 22:39:41 -04:00
matlabbe	34ed9d79c7	export: added --texture_blur filtering option	2022-07-20 22:08:45 -04:00
matlabbe	5943a8b065	Added stereo multi-camera support (#884 ) * Integrated OpenGV * Fixed build without opengv * Cmake: moved OpenGV dependency status under solvers group * Added multi-stereocamera models support * Fixed OpenGV 0 sample error when one of the camera doesn't have features. Fixed g2o BA id offset with multi-camera. * Fixed multicam 3d points generated from stereo correspondences * db: Fixed multi stereo models not loaded correctly * gui: fixed stereo rectification option, RegVis: fixed projection error with old databases (image size not set in calibration) * OdomF2M: Fixed map.at error when bundle adjustment is not used * depthai: added imu firmware update option for convenience * Fixed various refactor errors * Moved "large number stereo correspondences rejected" warning outside computeCorrespondences function for multicam * Added error log if ba correspondences are computed with empty signatures * fixed compilation errors with latest opencv Co-authored-by: mathieu86 <mathieu@robust.ai>	2022-07-20 15:20:14 -04:00
matlabbe	71a28bb570	Update README.md	2022-06-29 22:23:56 -04:00
matlabbe	89f56642b7	Update README.md	2022-06-26 21:30:14 -07:00
matlabbe	fb6770d70f	Update README.md	2022-06-26 20:59:20 -07:00
matlabbe	a10eb062e5	Update README.md	2022-06-26 20:58:15 -07:00
matlabbe	4d502c9e0d	Create README.md	2022-06-26 20:54:34 -07:00
matlabbe	83c1adfd1e	Updated illumination invariant paper documentation	2022-06-26 20:46:08 -07:00
mathieu86	bc42bc3520	Added Kp/ByteToFloat parameter to odom parameters	2022-06-09 10:51:08 -07:00
matlabbe	8f8256c1dd	would fix: #618 #790 #795 (#871 )	2022-05-30 07:24:04 -04:00
matlabbe	744c737da1	Update README.md	2022-05-28 17:22:50 -04:00
matlabbe	4cfd3ba496	Updated cmake workflow name	2022-05-28 17:20:49 -04:00
matlabbe	2da333895c	workflow: added minimal build for Ubuntu 22.04	2022-05-28 17:14:55 -04:00
matlabbe	601e4015fb	fixed android docker script path	2022-05-28 16:15:51 -04:00
matlabbe	47c94a4474	workflows docker: use current context instead of cloning the repo inside dockerfile	2022-05-28 16:11:52 -04:00
matlabbe	cf64b20e1f	Fixed error: ‘drawAxis’ is not a member of ‘cv::aruco’ (opencv 4.5.5)	2022-05-10 22:47:15 -04:00
matlabbe	5d200a0799	Fixed _markerPriorsLinearVariance not intitialized	2022-05-05 11:08:44 -04:00
matlabbe	dab7aa6e58	export: added min_range option	2022-05-03 23:12:09 -04:00
matlabbe	b646c5e1db	Marker priors (#859 ) * Added MarkerPriors parameter * Fixed Marker/Priors format to use '\|' instead ';'. Fixed landmark priors not used. * Marker: added priors variance parameters * g2o: refactored backward compatibility includes * fixed build with old g2o	2022-04-28 09:18:30 -04:00
matlabbe	190071678f	Statistics: Added Memory/New_landmark. Rtabmap: don't remove node on small movements when a new landmark has been just detected. DbViewer: fixed missing statistics when ignored nodes are not saved in database.	2022-04-25 12:19:22 -04:00
matlabbe	dc58266eda	ORBSLAM2: fixed build	2022-04-10 19:42:15 -04:00
matlabbe	e92dfd50e1	GUI: set default working dir if it is empty in the loaded config	2022-04-10 15:59:38 -04:00
matlabbe	1857111d7d	Reprocess: added option to stream only one camera (in case of multi-camera database)	2022-04-06 14:07:23 -04:00
matlabbe	3e630e0250	UPlot: fixed number of digits after the decimal points to 6 when exporting data to text	2022-04-04 11:42:36 -04:00
matlabbe	d35193721b	report tool: option --loc can be used without a number.	2022-04-01 13:53:32 -04:00
matlabbe	e46af2c3cd	DbViewer: Added GroundTruth pose in node info labels	2022-03-31 12:00:52 -04:00
matlabbe	6cefc6d00a	DBViewer: added camera frustums in Constraints View	2022-03-25 11:24:34 -04:00
matlabbe	ff739a98a5	Removed opencv's optflow module from required dependency (https://github.com/introlab/rtabmap/issues/427#issuecomment-1058767133 )	2022-03-07 00:12:56 -05:00
matlabbe	4321c3040a	CameraDepthAI: fixed imu transform and depth mode with latest depthai-core version	2022-03-06 22:25:01 -05:00
matlabbe	1e82fd3110	ImageView: added option to set maximum range for depth colormap	2022-03-03 18:01:04 -05:00
matlabbe	b9c7182a08	Update package.xml	2022-03-03 14:06:15 -05:00
matlabbe	656da152b3	multiband: fixed error when intermediate nodes without data are in memory	2022-03-01 18:42:43 -05:00
matlabbe	f140e99881	DBViewer: fixed kml export disabled for poses when there are gps values. Fixed commas used instead of dots for float in kml export on french machine.	2022-02-27 15:20:02 -05:00
matlabbe	53e0099dd8	ios: tmp database is now public, so it can be copied from file explorer / Finder in case recovery fails.	2022-02-27 14:32:19 -05:00
matlabbe	51f779628f	Recovery: original database is erased/replaced only after successful recovery (in case there is an seg fault during recovery).	2022-02-27 13:50:40 -05:00
matlabbe	e7ee025127	Export: fixed texturing assert when there are intermediate nodes	2022-02-25 02:08:08 -05:00
matlabbe	e6a5fe9c26	Don't do neighbor refining if fast movement has been detected	2022-02-20 18:09:39 -05:00
matlabbe	e297320dd5	Removed backward compatibility with RGBD/OptimizeMaxError to be able to use ratio lower than 1.	2022-02-20 13:44:53 -05:00
matlabbe	4d895785a6	GraphViewer: added Ensure Frame Visible menu option. MainWindow: fixed loop signature failed to load image warning when camera is not used.	2022-02-19 16:16:29 -05:00
matlabbe	c2c68c0caf	Updated exported poses in TUM format to avoid confusion with header when importing in CloudCompare	2022-02-18 12:17:47 -05:00
matlabbe	9bd758a62c	SuperPoint: return at minimum 2 features or 0 (fixed crash when only one feature). Python: added main python instance from Rtabmap object (to get python instancied once for ROS). MultiSessionWidget: Fixed layout warning. Memory::computeTransform() avoid re-instanciating RegistrationVis when Reg/Strategy=1 for guess transform (to fix PyMatcher re-initialized every time). CMake: added video module to opencv required components.	2022-02-15 01:28:38 -05:00
matlabbe	578c19cc38	Export CLI: for --poses_camera option with intermediate nodes in db, re-use previous camera models of last valid node with data.	2022-02-13 14:45:08 -05:00
matlabbe	4776de7931	rtabmap: updated proximity paths check order when likehood is not available (use closest node). MainWindow: don't pause when error happens in monitoring mode / ROS. DbViewer: show local transform of scan.	2022-02-12 16:46:10 -05:00
matlabbe	191e165a28	GUI: Added Multi-Session Localization view. CameraModel: set localTransform to optical rotation in default constructor (fixed matrix invertion errors with code ignoring setting local transform), save/read local transform in/from camera calibration yaml file (so that local transform is also exported when extracting rgb/depth images). Rtabmap: in localization mode, ignore landmarks farther than RGBD/LocalRadius if no global loop closures are already in odometry cache.	2022-02-12 13:35:23 -05:00
matlabbe	f584f42ea4	rtabmap/dbreader: save odometry covariance in pose prior link when in localization mode and saving localization data.	2022-02-06 20:17:46 -05:00
matlabbe	b44b212218	Bump 0.20.19. OdomF2M: scale x0.1 covariance when local bundle adjustment is done. Rtabmap: fixed GT localization error when RGBD/MaxOdomCacheSize is used.	2022-02-06 17:41:53 -05:00
matlabbe	9ad6b626e4	Updated how covariance is computed for 2d-3d estimation without query's depth image available (instead of computed from reprojection error)	2022-02-05 16:41:43 -05:00
matlabbe	32ad92e2d2	GraphView: added optional overlay to better see odometry cache constraints. rtabmap: keep global loop closure id in statistics when proximity is detected on same node.	2022-02-03 21:58:36 -05:00
matlabbe	6f8f6d4d8e	android: reverted export zip cleanup (keep zip files in export subdir for easy usb transfer)	2022-02-02 17:05:49 -05:00
matlabbe	c99203bbed	CMake: added explicit opencv modules to avoid VTK6-VTK7 issues on Focal (vtk6 dependency coming with opencv_viz 4.2 module and vtk7 coming from PCL)	2022-02-01 13:40:02 -05:00
matlabbe	20d873c29e	android: fixed backward compatibility target folder for android <30. Open: files are sorted by last modified date.	2022-01-30 15:47:08 -05:00