latest update

added fast normal estimation on obstacle segmentation
2026-09-08 12:30:20 +08:00 · 2023-06-07 17:14:48 -07:00 · 2023-06-02 16:05:05 -07:00
179 changed files with 8258 additions and 14992 deletions
@@ -19,7 +19,7 @@ init:
 install:
  # To download from google drive
  - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH%
-  - ps: py -m pip --disable-pip-version-check install gdown>=5.1.0 
+  - ps: py -m pip --disable-pip-version-check install gdown
  # Qt
  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
@@ -1,8 +0,0 @@
-{
-    "image": "introlab3it/rtabmap:20.04",
-    "customizations": {
-        "vscode": {
-          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
-        }
-      }
-}
@@ -21,23 +21,28 @@ jobs:
    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
    runs-on: ${{ matrix.os }}
    strategy:
-      fail-fast: false
      matrix:
-        ros_distribution: [ noetic, humble, iron]
+        ros_distribution: [ noetic, foxy, humble, rolling]
        include:
        - ros_distribution: 'noetic'
          os: ubuntu-20.04
+        - ros_distribution: 'foxy'
+          os: ubuntu-20.04
        - ros_distribution: 'humble'
          os: ubuntu-22.04
-        - ros_distribution: 'iron'
+        - ros_distribution: 'rolling'
          os: ubuntu-22.04

    steps:
-    - uses: ros-tooling/setup-ros@v0.6
+    - name: Workaround dpkg grub-efi-amd64-signed error
+      run: |
+        sudo apt-mark hold grub-efi-amd64-signed
+    
+    - uses: ros-tooling/setup-ros@v0.5
      with:
        required-ros-distributions: ${{ matrix.ros_distribution }}
    
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v2

    - name: Install dependencies
      run: |
@@ -16,7 +16,6 @@ jobs:
    name: ${{ matrix.os }}
    runs-on: ${{ matrix.os }}
    strategy:
-      fail-fast: false
      matrix:
        os: [ubuntu-22.04, ubuntu-20.04]

@@ -27,7 +26,7 @@ jobs:
        sudo apt-get update
        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev

-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v2

    - name: Configure CMake
      run: |
@@ -6,74 +6,22 @@ on:
      - 'master'

 jobs:
-  docker_deps:
-    runs-on: ubuntu-latest
-    
-    strategy:
-      fail-fast: false
-      matrix:
-        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps]
-        include:
-        - docker_tag: focal-deps
-          docker_tags: |
-            introlab3it/rtabmap:focal-deps
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-          docker_path: 'focal/deps'
-        - docker_tag: jammy-deps
-          docker_tags: |
-            introlab3it/rtabmap:jammy-deps
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-          docker_path: 'jammy/deps'
-        - docker_tag: jammy-iron-deps
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron-deps
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron/deps'
-    
-    steps:
-      -
-        name: Checkout
-        uses: actions/checkout@v2
-      - 
-        name: Set up QEMU
-        uses: docker/setup-qemu-action@v1
-        with:
-          platforms: all
-      -
-        name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v1
-      -
-        name: Login to DockerHub
-        uses: docker/login-action@v1
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
-      -
-        name: Build and push
-        uses: docker/build-push-action@v2
-        with:
-          context: .
-          push: true
-          platforms: ${{ matrix.docker_platforms }}
-          file: ./docker/${{ matrix.docker_path }}/Dockerfile
-          tags: ${{ matrix.docker_tags }}
-          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
-          cache-to: type=inline
-  
  docker:
-    needs: docker_deps
    runs-on: ubuntu-latest
    
    strategy:
-      fail-fast: false
      matrix:
-        docker_tag: [bionic, focal, jammy, jammy-iron, android23, android24, android26, android30]
+        docker_tag: [xenial, bionic, focal, focal-foxy, jammy, android23, android24, android26, android30]
        include:
+        - docker_tag: xenial
+          docker_tags: |
+            introlab3it/rtabmap:xenial
+            introlab3it/rtabmap:16.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'xenial'
        - docker_tag: bionic
          docker_tags: |
            introlab3it/rtabmap:bionic
@@ -95,6 +43,16 @@ jobs:
            linux/amd64
            linux/arm64
          docker_path: 'focal'
+        - docker_tag: focal-foxy
+          docker_tags: |
+            introlab3it/rtabmap:focal-foxy
+            introlab3it/rtabmap:20.04-foxy
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal-foxy'
        - docker_tag: jammy
          docker_tags: |
            introlab3it/rtabmap:jammy
@@ -105,14 +63,6 @@ jobs:
            linux/amd64
            linux/arm64
          docker_path: 'jammy'
-        - docker_tag: jammy-iron
-          docker_tags: |
-            introlab3it/rtabmap:jammy-iron
-          docker_args: |
-            NOT_USED=0
-          docker_platforms: |
-            linux/amd64
-          docker_path: 'jammy-iron'
        - docker_tag: android23
          docker_tags: |
            introlab3it/rtabmap:android23
@@ -1,5 +1,5 @@
 # Top-Level CmakeLists.txt
-cmake_minimum_required(VERSION 3.14)
+cmake_minimum_required(VERSION 3.5)
 PROJECT( RTABMap )
 SET(PROJECT_PREFIX rtabmap)

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -203,12 +203,11 @@ option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
-option(WITH_OCTOMAP       "Include OctoMap support"              ON)
-option(WITH_GRIDMAP       "Include GridMap support"              ON)
+option(WITH_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
 option(WITH_OPENCHISEL    "Include open_chisel support"          OFF)
 option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)
-option(WITH_FOVIS         "Include FOVIS supp++ort"                OFF)
+option(WITH_FOVIS         "Include FOVIS support"                OFF)
 option(WITH_VISO2         "Include VISO2 support"                OFF)
 option(WITH_DVO           "Include DVO support"                  OFF)
 option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" OFF)
@@ -252,10 +251,10 @@ endif()

 FIND_PACKAGE(ZLIB REQUIRED QUIET)

-FIND_PACKAGE(SQLite3 QUIET)
-IF(SQLite3_FOUND)
-   MESSAGE(STATUS "Found SQLite3: ${SQLite3_INCLUDE_DIRS} ${SQLite3_LIBRARIES}")
-ENDIF(SQLite3_FOUND)
+FIND_PACKAGE(Sqlite3 QUIET)
+IF(Sqlite3_FOUND)
+   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
+ENDIF(Sqlite3_FOUND)

 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
   # fix libproj.so not found on Xenial
@@ -294,20 +293,14 @@ SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF FALSE)
 IF(WITH_QT)
    FIND_PACKAGE(VTK)
    IF(NOT VTK_FOUND)
-      MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.")
+        MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.")
    ENDIF(NOT VTK_FOUND)

    # If Qt is here, the GUI will be built
    IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
-      IF(NOT VTK_QT_VERSION)
-        MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.")
-      ENDIF()
-
-      option(VTK_GLOBAL_WARNING_DISPLAY  "Show VTK warning display on runtime" OFF)
-      IF(NOT VTK_GLOBAL_WARNING_DISPLAY)
-        ADD_DEFINITIONS(-DVTK_GLOBAL_WARNING_DISPLAY_OFF)
-      ENDIF()
-
+	  IF(NOT VTK_QT_VERSION)
+	    MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.")
+	  ENDIF()
      MESSAGE(STATUS "VTK>=9 detected, will use VTK_QT_VERSION=${VTK_QT_VERSION} for Qt version.")
      IF(${VTK_QT_VERSION} EQUAL 6)
        FIND_PACKAGE(Qt6 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
@@ -332,11 +325,6 @@ IF(WITH_QT)
    ENDIF()
        
    IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
-        # For VCPKG build, set those global variables to off, 
-        # we will enable them for jsut specific targets
-        set(CMAKE_AUTOMOC OFF)
-        set(CMAKE_AUTORCC OFF)
-        set(CMAKE_AUTOUIC OFF)
        IF("${VTK_MAJOR_VERSION}" EQUAL 5)
            FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
        ELSE()
@@ -401,7 +389,6 @@ IF(WITH_PYTHON)
    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy)
    IF(Python3_FOUND)
        MESSAGE(STATUS "Found Python3")
-        FIND_PACKAGE(pybind11 REQUIRED)
    ENDIF(Python3_FOUND)
 ENDIF(WITH_PYTHON)

@@ -531,14 +518,7 @@ ENDIF(WITH_CVSBA)
 IF(WITH_POINTMATCHER)
    find_package(libpointmatcher QUIET)
    IF(libpointmatcher_FOUND)
-        MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
-        string(FIND "${libpointmatcher_LIBRARIES}" "libnabo" value)
-        IF(value EQUAL -1)
-            # Find libnabo (Issue #1117):
-            find_package(libnabo REQUIRED PATHS ${LIBNABO_INSTALL_DIR})
-            message(STATUS "libnabo found, version ${libnabo_VERSION} (Config mode)")
-            SET(libpointmatcher_LIBRARIES "${libpointmatcher_LIBRARIES};libnabo::nabo")
-        ENDIF(value EQUAL -1)
+       MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
    ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)

@@ -667,13 +647,6 @@ IF(WITH_OCTOMAP)
    ENDIF(octomap_FOUND)
 ENDIF(WITH_OCTOMAP)

-IF(WITH_GRIDMAP)
-    FIND_PACKAGE(grid_map_core QUIET)
-    IF(grid_map_core_FOUND)
-       MESSAGE(STATUS "Found grid_map_core ${grid_map_core_VERSION}: ${grid_map_core_INCLUDE_DIRS}")
-    ENDIF(grid_map_core_FOUND)
-ENDIF(WITH_GRIDMAP)
-
 IF(WITH_CPUTSDF)
    FIND_PACKAGE(CPUTSDF QUIET)
    IF(CPUTSDF_FOUND)
@@ -793,7 +766,7 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)

 IF(NOT MSVC)
-    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND)
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1))
      # Qt6 requires c++17
      include(CheckCXXCompilerFlag)
      CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
@@ -804,8 +777,8 @@ IF(NOT MSVC)
        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler if you want to use Qt6.")
      ENDIF()
    ENDIF()
-    IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND (msckf_vio_FOUND OR loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND))
-     #MSCKF_VIO, LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
+    IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND ((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)))
+     #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
     include(CheckCXXCompilerFlag)
      CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
      IF(COMPILER_SUPPORTS_CXX14)
@@ -816,7 +789,22 @@ IF(NOT MSVC)
      ENDIF()
    ENDIF()

-    IF(NOT ("${CMAKE_CXX_STANDARD}" STREQUAL "17") AND NOT ("${CMAKE_CXX_STANDARD}" STREQUAL "14"))
+    IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "17") AND NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
+           G2O_FOUND OR
+           GTSAM_FOUND OR
+           CERES_FOUND OR
+           ZED_FOUND OR
+           ZEDOC_FOUND OR
+           ANDROID OR
+           RealSense_FOUND OR
+           realsense2_FOUND OR
+           ORB_SLAM_FOUND OR
+           okvis_FOUND OR
+           open_chisel_FOUND OR
+           msckf_vio_FOUND OR
+           vins_FOUND OR
+           ov_msckf_FOUND OR
+           libpointmatcher_FOUND))
     #Newest versions require std11
     include(CheckCXXCompilerFlag)
      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
@@ -831,7 +819,6 @@ IF(NOT MSVC)
    ENDIF()
 ENDIF()

-
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
  IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
@@ -892,9 +879,9 @@ ENDIF()
 IF(NOT MRPT_FOUND)
   SET(MRPT "//")
 ENDIF(NOT MRPT_FOUND)
-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)
@@ -916,9 +903,9 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
   SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
-IF(NOT opengv_FOUND OR NOT WITH_OPENGV)
+IF(NOT opengv_FOUND)
   SET(OPENGV "//")
-ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
+ENDIF(NOT opengv_FOUND)
 IF(NOT PDAL_FOUND)
   SET(PDAL "//")
 ENDIF(NOT PDAL_FOUND)
@@ -1001,12 +988,6 @@ IF(NOT octomap_FOUND)
 ELSE()
   SET(CONF_WITH_OCTOMAP 1)
 ENDIF()
-IF(NOT grid_map_core_FOUND)
-   SET(GRIDMAP "//")
-   SET(CONF_WITH_GRIDMAP 0)
-ELSE()
-   SET(CONF_WITH_GRIDMAP 1)
-ENDIF()
 IF(NOT CPUTSDF_FOUND)
   SET(CPUTSDF "//")
 ENDIF()
@@ -1048,9 +1029,6 @@ IF(NOT TORCH_FOUND)
 ENDIF()
 IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
   SET(PYTHON "//")
-   SET(CONF_WITH_PYTHON 0)
-ELSE()
-   SET(CONF_WITH_PYTHON 1)
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
   SET(CONF_VTK_QT true)
@@ -1089,7 +1067,6 @@ ENDIF(BUILD_EXAMPLES)
 #######################
 # Uninstall target, for "make uninstall"
 #######################
-IF (NOT TARGET uninstall)
 CONFIGURE_FILE(
  "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in"
  "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
@@ -1097,7 +1074,6 @@ CONFIGURE_FILE(

 ADD_CUSTOM_TARGET(uninstall
  "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
-ENDIF()

 ####
 # Global Export Target
@@ -1332,10 +1308,10 @@ ELSE()
 MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
 ENDIF()

-IF(SQLite3_FOUND)
+IF(Sqlite3_FOUND)
 MESSAGE(STATUS "  With external SQLite3     = YES (License: Public Domain)")
 ELSE()
-MESSAGE(STATUS "  With external SQLite3     = NO (SQLite3 not found, internal version is used for convenience)")
+MESSAGE(STATUS "  With external SQLite3     = NO (sqlite3 not found, internal version is used for convenience)")
 ENDIF()

 IF(WITH_ORB_OCTREE)
@@ -1406,8 +1382,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()
@@ -1466,7 +1446,7 @@ ELSE()
 MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()

-IF(opengv_FOUND AND WITH_OPENGV)
+IF(opengv_FOUND)
 MESSAGE(STATUS "  With OpenGV ${opengv_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")
@@ -1477,19 +1457,11 @@ ENDIF()
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)
-MESSAGE(STATUS "  With OctoMap ${octomap_VERSION}        = YES (License: BSD)")
+MESSAGE(STATUS "  With OCTOMAP ${octomap_VERSION}        = YES (License: BSD)")
 ELSEIF(NOT WITH_OCTOMAP)
-MESSAGE(STATUS "  With OctoMap              = NO (WITH_OCTOMAP=OFF)")
+MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
 ELSE()
-MESSAGE(STATUS "  With OctoMap              = NO (octomap not found)")
-ENDIF()
-
-IF(grid_map_core_FOUND)
-MESSAGE(STATUS "  With GridMap ${grid_map_core_VERSION}        = YES (License: BSD)")
-ELSEIF(NOT WITH_OCTOMAP)
-MESSAGE(STATUS "  With GridMap              = NO (WITH_GRIDMAP=OFF)")
-ELSE()
-MESSAGE(STATUS "  With GridMap              = NO (grid_map_core not found)")
+MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
 ENDIF()

 IF(CPUTSDF_FOUND)
@@ -7,7 +7,7 @@ rtabmap
 [![Downloads][downloads-image]][downloads]
 [![License][license-image]][license]

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

 [downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
@@ -54,18 +54,22 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
 <table>
    <tbody>
        <tr>
-           <td rowspan="1">ROS 1</td>
+           <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>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>
+            <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>Iron</td>
-            <td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+            <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>
@@ -50,14 +50,6 @@ IF(@CONF_WITH_OCTOMAP@)
  find_dependency(octomap)
 ENDIF()

-IF(@CONF_WITH_GRIDMAP@)
-  find_dependency(grid_map_core)
-ENDIF()
-
-IF(@CONF_WITH_PYTHON@)
-  find_dependency(Python3 COMPONENTS Interpreter Development NumPy)
-ENDIF()
-
 # Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies)
 set(RTABMap_INCLUDE_DIRS "")
 set(RTABMap_LIBRARIES "")
@@ -69,7 +69,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@MYNTEYE@#define RTABMAP_MYNTEYE
@DEPTHAI@#define RTABMAP_DEPTHAI
@OCTOMAP@#define RTABMAP_OCTOMAP
-@GRIDMAP@#define RTABMAP_GRIDMAP
@CPUTSDF@#define RTABMAP_CPUTSDF
@ALICE_VISION@#define RTABMAP_ALICE_VISION
@OPENCHISEL@#define RTABMAP_OPENCHISEL
@@ -649,9 +649,9 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 								UWARN("Cloud %d is empty", id);
 							}
 						}
-						else if(!data.depthOrRightCompressed().empty() || !data.laserScanCompressed().isEmpty())
+						else
 						{
-							UERROR("Failed to uncompress data! (rgb=%d, depth=%d, scan=%d)", data.imageCompressed().cols, data.depthOrRightCompressed().cols, data.laserScanCompressed().size());
+							UERROR("Failed to uncompress data!");
 							status=-2;
 						}
 					}
@@ -122,7 +122,7 @@ git clone https://github.com/PointCloudLibrary/pcl.git
 cd pcl
 git checkout tags/pcl-1.11.1
 # patch
-curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
+curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/4a66ebb9faa1dfe997a0860d733bc5473cff20ee/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
 git apply pcl_1_11_1_vtk_ios_support.patch
 mkdir build
 cd build
@@ -65,17 +65,14 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
       
 IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
  SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
-  SET(plugin_dest_dir bin/plugins)
+  SET(plugin_dest_dir bin)
  SET(qtconf_dest_dir bin)
-  SET(thirdparty_dest_dir bin)
+  SET(openni2_dest_dir bin)
 
  IF(APPLE)
-    SET(plugin_dest_dir MacOS/plugins)
-    IF(Qt6_FOUND)
-       SET(plugin_dest_dir PlugIns)
-    ENDIF()
+    SET(plugin_dest_dir MacOS)
    SET(qtconf_dest_dir Resources)
-    SET(thirdparty_dest_dir MacOS)
+    SET(openni2_dest_dir MacOS)
    SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/MacOS/${CMAKE_BUNDLE_NAME}")
  ENDIF(APPLE)

@@ -92,11 +89,11 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
    ENDIF()
    
    INSTALL(DIRECTORY "${OpenNI2_BIN_DIR}/OpenNI2" 
-          DESTINATION ${thirdparty_dest_dir} 
+          DESTINATION ${openni2_dest_dir} 
          COMPONENT runtime 
          REGEX ".*pdb" EXCLUDE)
    INSTALL(FILES "${OpenNI2_BIN_DIR}/OpenNI.ini" 
-          DESTINATION ${thirdparty_dest_dir} 
+          DESTINATION ${openni2_dest_dir} 
          COMPONENT runtime)
  ENDIF(OpenNI2_FOUND)
  
@@ -105,7 +102,7 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
    IF(WIN32)
 		file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR)
 		INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" 
-          DESTINATION ${thirdparty_dest_dir} 
+          DESTINATION ${plugin_dest_dir} 
          COMPONENT runtime)
 	ENDIF(WIN32)
  ENDIF(k4a_FOUND)
@@ -120,7 +117,7 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 			MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}")
 			MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}")
 			INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL}
-			  DESTINATION ${thirdparty_dest_dir} 
+			  DESTINATION ${plugin_dest_dir} 
 			  COMPONENT runtime)
 		ELSE()
 			MESSAGE(AUTHOR_WARNING "Using Torch with CUDA, but cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll are not found on the PATH, so it won't be added to package.")
@@ -138,87 +135,75 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 	include(\"${QT_DEPLOY_SUPPORT}\")
 	qt_deploy_runtime_dependencies(
 		EXECUTABLE \"${APPS}\"
-		PLUGINS_DIR ${plugin_dest_dir}
-		GENERATE_QT_CONF
-		NO_TRANSLATIONS
 		VERBOSE
+		PLUGINS_DIR ${plugin_dest_dir}/plugins
 	)")
-    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "Debug")
 	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appDebug.cmake"
-            CONFIGURATIONS Debug
-            COMPONENT runtime)
-    	ENDIF()
-    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "Release")
-        install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake"
-            CONFIGURATIONS Release
-            COMPONENT runtime)
-    	ENDIF()
-    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
-        install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake"
-            CONFIGURATIONS RelWithDebInfo
-            COMPONENT runtime)
-    	ENDIF()
-    	IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
-        install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake"
-            CONFIGURATIONS MinSizeRel
-            COMPONENT runtime)
-    	ENDIF()
+		CONFIGURATIONS Debug
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake"
+		CONFIGURATIONS Release
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake"
+		CONFIGURATIONS RelWithDebInfo
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake"
+		CONFIGURATIONS MinSizeRel
+		COMPONENT runtime)
  ELSEIF(Qt5_FOUND)
-    #Qt5
-    foreach(plugin ${Qt5Gui_PLUGINS})
-        get_target_property(plugin_loc ${plugin} LOCATION)
-        get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
-        string(REPLACE "plugins" ";" loc_list ${plugin_dir})
-        list(GET loc_list 1 plugin_type)
-        IF(NOT plugin_root)
-            get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
-        ENDIF(NOT plugin_root)
-        #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}${plugin_type}\"")
-        INSTALL(FILES ${plugin_loc}
-          DESTINATION ${plugin_dest_dir}${plugin_type}
-          COMPONENT runtime)
-    endforeach()
-    IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
-        IF(WIN32)
-            SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
-        ELSEIF(APPLE)
-            SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
-        ENDIF()
-        IF(EXISTS ${plugin_loc})
-            get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
-            string(REPLACE "plugins" ";" loc_list ${plugin_dir})
-            list(GET loc_list 1 plugin_type)
-            INSTALL(FILES ${plugin_loc}
-                DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
-                COMPONENT runtime)
-            #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}${plugin_type}\"")
-        ENDIF(EXISTS ${plugin_loc})
-    ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+	#Qt5
+	foreach(plugin ${Qt5Gui_PLUGINS})
+		get_target_property(plugin_loc ${plugin} LOCATION)
+		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+		list(GET loc_list 1 plugin_type)
+		IF(NOT plugin_root)
+			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
+		ENDIF(NOT plugin_root)
+		#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		INSTALL(FILES ${plugin_loc}
+		  DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+		  COMPONENT runtime)
+	endforeach()
+	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+		IF(WIN32)
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+		ELSEIF(APPLE)
+			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+		ENDIF()
+		IF(EXISTS ${plugin_loc})
+			get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+			string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+			list(GET loc_list 1 plugin_type)
+			INSTALL(FILES ${plugin_loc}
+				DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+				COMPONENT runtime)
+			#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		ENDIF(EXISTS ${plugin_loc})
+	ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
  ELSEIF(QT_PLUGINS_DIR) # Qt4
    # Install needed Qt plugins by copying directories from the qt installation
-    # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
-    # Exclude debug libraries
-    INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
-        DESTINATION ${plugin_dest_dir}
-        COMPONENT runtime 
-        REGEX ".*d4.dll" EXCLUDE
-        REGEX ".*d4.a" EXCLUDE)
+	# One can cull what gets copied by using 'REGEX "..." EXCLUDE'
+	# Exclude debug libraries
+	INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
+		DESTINATION ${plugin_dest_dir}/plugins 
+		COMPONENT runtime 
+		REGEX ".*d4.dll" EXCLUDE
+		REGEX ".*d4.a" EXCLUDE)
  ENDIF()
 
-  IF(Qt5_FOUND OR QT4_FOUND)
-      # install a qt.conf file
-      # this inserts some cmake code into the install script to write the file
-      SET(QT_CONF_FILE [Paths]\nPlugins=plugins)
-      IF(APPLE)
-        SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
-      ENDIF(APPLE)
-      INSTALL(CODE "
-        file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
-        " COMPONENT runtime)
-  ENDIF()
+  # install a qt.conf file
+  # this inserts some cmake code into the install script to write the file
+  SET(QT_CONF_FILE [Paths]\nPlugins=plugins)
+  IF(APPLE)
+	SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
+  ENDIF(APPLE)
+  INSTALL(CODE "
+	file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
+	" COMPONENT runtime)
 
  # directories to look for dependencies
-  SET(DIRS "${QT_LIBRARY_DIRS}" "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/lib")
+  SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_BINARY_DIR}/bin)
  IF(APPLE)
    SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current)
  ENDIF(APPLE)
@@ -230,11 +215,10 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
  # over.
  # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them).
  install(CODE "
-   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
    set(BU_CHMOD_BUNDLE_ITEMS ON)
    include(\"BundleUtilities\")
    fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
  " COMPONENT runtime)
-
 ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
  
@@ -55,6 +55,9 @@ int main(int argc, char* argv[])
 	CoInitialize(nullptr);
 #endif

+#if VTK_MAJOR_VERSION >= 8 && defined(BUILD_AS_BUNDLE)
+	vtkObject::GlobalWarningDisplayOff();
+#endif
 #if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
    // needed to ensure appropriate OpenGL context is created for VTK rendering.
    QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());
@@ -25,13 +25,13 @@ The following image shows when we do the same localization experiment at differe

 We provide two formats: the first one is more general and the second one is used to produce the results in this paper with RTAB-Map. Please open issue if the links are outdated.

-* [Images](https://drive.google.com/file/d/1fUm1m8oW6q8qlThx7BjrBH2vrVbNQ9bQ/view?usp=drive_link):
+* [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://drive.google.com/file/d/1TklUcTKFSrcg8b0t0U80G_IpFRMVRlY5/view?usp=drive_link)
+* [RTAB-Map Databases](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EU5fb0jEKzlGhPK3OWjMGLUBnDo1BRAoZwtB2czyeVLE_A?e=Y0JyXY)



@@ -12,7 +12,6 @@ find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/incl
 find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
 find_library(ORB_SLAM3_LIBRARY NAMES ORB_SLAM3 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
 find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
-find_path(sophus_INCLUDE_DIR NAMES sophus/se3.hpp PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/Sophus NO_DEFAULT_PATH)
 find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
 find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)

@@ -22,9 +21,6 @@ IF(ORB_SLAM2_LIBRARY)
 ELSEIF(ORB_SLAM3_LIBRARY)
   SET(ORB_SLAM_VERSION 3)
   SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY})
-   IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR) # ORB_SLAM3 v1
-      SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR})
-   ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR)
 ENDIF()

 IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
@@ -0,0 +1,30 @@
+# - Find Sqlite3
+# This module finds an installed Sqlite3 package.
+#
+# It sets the following variables:
+#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
+#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
+#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.
+
+FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})
+
+FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
+
+IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
+   SET(Sqlite3_FOUND TRUE)
+   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
+   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
+ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
+
+IF (Sqlite3_FOUND)
+   # show which Sqlite3 was found only if not quiet
+   IF (NOT Sqlite3_FIND_QUIETLY)
+      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
+   ENDIF (NOT Sqlite3_FIND_QUIETLY)
+ELSE (Sqlite3_FOUND)
+   # fatal error if Sqlite3 is required but not found
+   IF (Sqlite3_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find Sqlite3")
+   ENDIF (Sqlite3_FIND_REQUIRED)
+ENDIF (Sqlite3_FOUND)
+
@@ -45,7 +45,6 @@ public:
 		timeMirroring(0.0f),
 		timeStereoExposureCompensation(0.0f),
 		timeImageDecimation(0.0f),
-		timeHistogramEqualization(0.0f),
 		timeScanFromDepth(0.0f),
 		timeUndistortDepth(0.0f),
 		timeBilateralFiltering(0.0f),
@@ -63,7 +62,6 @@ public:
 	float timeMirroring;
 	float timeStereoExposureCompensation;
 	float timeImageDecimation;
-	float timeHistogramEqualization;
 	float timeScanFromDepth;
 	float timeUndistortDepth;
 	float timeBilateralFiltering;
@@ -47,7 +47,6 @@ class CameraInfo;
 class SensorData;
 class StereoDense;
 class IMUFilter;
-class Feature2D;

 /**
 * Class CameraThread
@@ -81,7 +80,6 @@ public:
 	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
-	void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
 	void setImageRate(float imageRate);
 	void setDistortionModel(const std::string & path);
@@ -89,8 +87,6 @@ public:
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
 	void disableIMUFiltering();
-	void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
-	void disableFeatureDetection();

 	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
 	RTABMAP_DEPRECATED void setScanParameters(
@@ -138,7 +134,6 @@ private:
 	bool _stereoExposureCompensation;
 	bool _colorOnly;
 	int _imageDecimation;
-	int _histogramMethod;
 	bool _stereoToDepth;
 	bool _scanFromDepth;
 	int _scanDownsampleStep;
@@ -155,8 +150,6 @@ private:
 	float _bilateralSigmaR;
 	IMUFilter * _imuFilter;
 	bool _imuBaseFrameConversion;
-	Feature2D * _featureDetector;
-	bool _depthAsMask;
 };

 } // namespace rtabmap
@@ -96,10 +96,6 @@ public:
 				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint);
-	void updateCalibration(
-		int nodeId,
-		const std::vector<CameraModel> & models,
-		const std::vector<StereoCameraModel> & stereoModels);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
 	void updateLaserScan(int nodeId, const LaserScan & scan);

@@ -235,11 +231,6 @@ protected:
 			float cellSize,
 			const cv::Point3f & viewpoint) const = 0;

-	virtual void updateCalibrationQuery(
-			int nodeId,
-			const std::vector<CameraModel> & models,
-			const std::vector<StereoCameraModel> & stereoModels) const = 0;
-
 	virtual void updateDepthImageQuery(
 			int nodeId,
 			const cv::Mat & image) const = 0;
@@ -96,11 +96,6 @@ protected:
 			float cellSize,
 			const cv::Point3f & viewpoint) const;

-	virtual void updateCalibrationQuery(
-			int nodeId,
-			const std::vector<CameraModel> & models,
-			const std::vector<StereoCameraModel> & stereoModels) const;
-
 	virtual void updateDepthImageQuery(
 			int nodeId,
 			const cv::Mat & image) const;
@@ -158,7 +153,6 @@ private:
 	std::string queryStepNode() const;
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
-	std::string queryStepCalibrationUpdate() const;
 	std::string queryStepDepthUpdate() const;
 	std::string queryStepScanUpdate() const;
 	std::string queryStepSensorData() const;
@@ -171,7 +165,6 @@ private:
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
-	void stepCalibrationUpdate(sqlite3_stmt * ppStmt, int nodeId, const std::vector<CameraModel> & models, const std::vector<StereoCameraModel> & stereoModels) const;
 	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
 	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
@@ -1,102 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef SRC_MAP_H_
-#define SRC_MAP_H_
-
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <rtabmap/core/LocalGrid.h>
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/Transform.h>
-#include <list>
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT GlobalMap
-{
-public:
-	inline static float logodds(double probability)
-	{
-		return (float) log(probability/(1-probability));
-	}
-
-	inline static double probability(double logodds)
-	{
-		return 1. - ( 1. / (1. + exp(logodds)));
-	}
-
-public:
-	virtual ~GlobalMap();
-
-	bool update(const std::map<int, Transform> & poses); // return true if map has changed
-
-	virtual void clear();
-
-	float getCellSize() const {return cellSize_;}
-	float getUpdateError() const {return updateError_;}
-	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
-
-	void getGridMin(double & x, double & y) const {x=minValues_[0];y=minValues_[1];}
-	void getGridMax(double & x, double & y) const {x=maxValues_[0];y=maxValues_[1];}
-	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
-	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
-
-	virtual unsigned long getMemoryUsed() const;
-
-protected:
-	GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
-
-	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses) = 0;
-
-	const std::map<int, LocalGrid> & cache() const {return cache_->localGrids();}
-
-	const std::map<int, Transform> & assembledNodes() const {return addedNodes_;}
-	bool isNodeAssembled(int id) {return addedNodes_.find(id) != addedNodes_.end();}
-	void addAssembledNode(int id, const Transform & pose);
-
-protected:
-	float cellSize_;
-	float updateError_;
-
-	float occupancyThr_;
-	float logOddsHit_;
-	float logOddsMiss_;
-	float logOddsClampingMin_;
-	float logOddsClampingMax_;
-
-	double minValues_[3];
-	double maxValues_[3];
-
-private:
-	const LocalGridCache * cache_;
-	std::map<int, Transform> addedNodes_;
-};
-
-} /* namespace rtabmap */
-
-#endif /* SRC_MAP_H_ */
@@ -136,12 +136,6 @@ std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
-std::multimap<int, std::pair<int, Link::Type> >::iterator RTABMAP_CORE_EXPORT findLink(
-		std::multimap<int, std::pair<int, Link::Type> > & links,
-		int from,
-		int to,
-		bool checkBothWays = true,
-		Link::Type type = Link::kUndef);
 std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, int> & links,
 		int from,
@@ -153,12 +147,6 @@ std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
-std::multimap<int, std::pair<int, Link::Type> >::const_iterator RTABMAP_CORE_EXPORT findLink(
-		const std::multimap<int, std::pair<int, Link::Type> > & links,
-		int from,
-		int to,
-		bool checkBothWays = true,
-		Link::Type type = Link::kUndef);
 std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, int> & links,
 		int from,
@@ -1,90 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef SRC_LOCALGRID_H_
-#define SRC_LOCALGRID_H_
-
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <opencv2/core.hpp>
-#include <map>
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT LocalGrid
-{
-public:
-	LocalGrid(const cv::Mat & ground,
-		 const cv::Mat & obstacles,
-		 const cv::Mat & empty,
-		 float cellSize,
-		 const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
-	virtual ~LocalGrid() {}
-	bool is3D() const;
-public:
-	cv::Mat groundCells;
-	cv::Mat obstacleCells;
-	cv::Mat emptyCells;
-	float cellSize;
-	cv::Point3f viewPoint;
-};
-
-class RTABMAP_CORE_EXPORT LocalGridCache
-{
-public:
-	LocalGridCache() {}
-	virtual ~LocalGridCache() {}
-
-	void add(int nodeId,
-			const cv::Mat & ground,
-			const cv::Mat & obstacles,
-			const cv::Mat & empty,
-			float cellSize,
-			const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
-
-	void add(int nodeId, const LocalGrid & localGrid);
-
-	bool shareTo(int nodeId, LocalGridCache & anotherCache) const;
-
-	unsigned long getMemoryUsed() const;
-	void clear(bool temporaryOnly = false);
-
-	size_t size() const {return localGrids_.size();}
-	bool empty() const {return localGrids_.empty();}
-	const std::map<int, LocalGrid> & localGrids() const {return localGrids_;}
-
-	std::map<int, LocalGrid>::const_iterator find(int nodeId) const {return localGrids_.find(nodeId);}
-	std::map<int, LocalGrid>::const_iterator begin() const {return localGrids_.begin();}
-	std::map<int, LocalGrid>::const_iterator end() const {return localGrids_.end();}
-
-private:
-	std::map<int, LocalGrid> localGrids_;
-};
-
-} /* namespace rtabmap */
-
-#endif /* SRC_LOCALGRID_H_ */
@@ -1,115 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef SRC_LOCAL_MAP_H_
-#define SRC_LOCAL_MAP_H_
-
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <pcl/pcl_base.h>
-#include <pcl/point_types.h>
-
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/Signature.h>
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT LocalGridMaker
-{
-public:
-	LocalGridMaker(const ParametersMap & parameters = ParametersMap());
-	virtual ~LocalGridMaker();
-	virtual void parseParameters(const ParametersMap & parameters);
-
-	float getCellSize() const {return cellSize_;}
-	bool isGridFromDepth() const {return occupancySensor_;}
-	bool isMapFrameProjection() const {return projMapFrame_;}
-
-	template<typename PointT>
-	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
-			const typename pcl::PointCloud<PointT>::Ptr & cloud,
-			const pcl::IndicesPtr & indices,
-			const Transform & pose,
-			const cv::Point3f & viewPoint,
-			pcl::IndicesPtr & groundIndices,        // output cloud indices
-			pcl::IndicesPtr & obstaclesIndices,     // output cloud indices
-			pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
-
-	void createLocalMap(
-			const Signature & node,
-			cv::Mat & groundCells,
-			cv::Mat & obstacleCells,
-			cv::Mat & emptyCells,
-			cv::Point3f & viewPoint);
-
-	void createLocalMap(
-			const LaserScan & cloud,
-			const Transform & pose,
-			cv::Mat & groundCells,
-			cv::Mat & obstacleCells,
-			cv::Mat & emptyCells,
-			cv::Point3f & viewPointInOut) const;
-
-protected:
-	ParametersMap parameters_;
-
-	unsigned int cloudDecimation_;
-	float rangeMax_;
-	float rangeMin_;
-	std::vector<float> roiRatios_;
-	float footprintLength_;
-	float footprintWidth_;
-	float footprintHeight_;
-	int scanDecimation_;
-	float cellSize_;
-	bool preVoxelFiltering_;
-	int occupancySensor_;
-	bool projMapFrame_;
-	float maxObstacleHeight_;
-	int normalKSearch_;
-	float groundNormalsUp_;
-	float maxGroundAngle_;
-	float clusterRadius_;
-	int minClusterSize_;
-	bool flatObstaclesDetected_;
-	float minGroundHeight_;
-	float maxGroundHeight_;
-	bool normalsSegmentation_;
-	bool grid3D_;
-	bool groundIsObstacle_;
-	float noiseFilteringRadius_;
-	int noiseFilteringMinNeighbors_;
-	bool scan2dUnknownSpaceFilled_;
-	bool rayTracing_;
-};
-
-} /* namespace rtabmap */
-
-#include <rtabmap/core/impl/LocalMapMaker.hpp>
-
-#endif /* SRC_MAP_H_ */
@@ -57,7 +57,7 @@ class RegistrationInfo;
 class RegistrationIcp;
 class RegistrationVis;
 class Stereo;
-class LocalGridMaker;
+class OccupancyGrid;
 class MarkerDetector;

 class RTABMAP_CORE_EXPORT Memory
@@ -330,7 +330,6 @@ private:
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
 	bool _useOdometryGravity;
-	bool _rotateImagesUpsideUp;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
 	bool _imagesAlreadyRectified;
@@ -372,7 +371,7 @@ private:
 	RegistrationIcp * _registrationIcpMulti;
 	RegistrationVis * _registrationVis;

-	LocalGridMaker * _localMapMaker;
+	OccupancyGrid * _occupancy;

 	MarkerDetector * _markerDetector;
 };
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -25,14 +25,145 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_
-#define CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_
+#ifndef CORELIB_SRC_OCCUPANCYGRID_H_
+#define CORELIB_SRC_OCCUPANCYGRID_H_

-/*
- * Deprecated header, use the one below directly!
- */
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

-#include <rtabmap/core/global_map/OccupancyGrid.h>
+#include <pcl/point_cloud.h>
+#include <pcl/pcl_base.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>

+namespace rtabmap {

-#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_ */
+class RTABMAP_CORE_EXPORT OccupancyGrid
+{
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
+public:
+	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
+	void parseParameters(const ParametersMap & parameters);
+	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
+	void setCellSize(float cellSize);
+	float getCellSize() const {return cellSize_;}
+	void setCloudAssembling(bool enabled);
+	float getMinMapSize() const {return minMapSize_;}
+	bool isGridFromDepth() const {return occupancySensor_;}
+	bool isFullUpdate() const {return fullUpdate_;}
+	float getUpdateError() const {return updateError_;}
+	bool isMapFrameProjection() const {return projMapFrame_;}
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	int cacheSize() const {return (int)cache_.size();}
+	const std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > & getCache() const {return cache_;}
+
+	template<typename PointT>
+	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
+			const typename pcl::PointCloud<PointT>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			const Transform & pose,
+			const cv::Point3f & viewPoint,
+			pcl::IndicesPtr & groundIndices,        // output cloud indices
+			pcl::IndicesPtr & obstaclesIndices,     // output cloud indices
+			pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
+
+	void createLocalMap(
+			const Signature & node,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPoint);
+
+	void createLocalMap(
+			const LaserScan & cloud,
+			const Transform & pose,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPointInOut) const;
+
+	void clear();
+	void addToCache(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty);
+	bool update(const std::map<int, Transform> & poses); // return true if map has changed
+	cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getProbMap(float & xMin, float & yMin) const;
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
+
+	unsigned long getMemoryUsed() const;
+
+private:
+	ParametersMap parameters_;
+	unsigned int cloudDecimation_;
+	float cloudMaxDepth_;
+	float cloudMinDepth_;
+	std::vector<float> roiRatios_;
+	float footprintLength_;
+	float footprintWidth_;
+	float footprintHeight_;
+	int scanDecimation_;
+	float cellSize_;
+	bool preVoxelFiltering_;
+	int occupancySensor_;
+	bool projMapFrame_;
+	float maxObstacleHeight_;
+	int normalKSearch_;
+	float groundNormalsUp_;
+	float maxGroundAngle_;
+	float clusterRadius_;
+	int minClusterSize_;
+	bool flatObstaclesDetected_;
+	float minGroundHeight_;
+	float maxGroundHeight_;
+	bool normalsSegmentation_;
+	bool grid3D_;
+	bool groundIsObstacle_;
+	bool labelUndergroundObstaclesAsGround_;
+	float noiseFilteringRadius_;
+	int noiseFilteringMinNeighbors_;
+	bool scan2dUnknownSpaceFilled_;
+	bool rayTracing_;
+	bool fullUpdate_;
+	float minMapSize_;
+	bool erode_;
+	float footprintRadius_;
+	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;
+
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
+	cv::Mat map_;
+	cv::Mat mapInfo_;
+	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
+	float xMin_;
+	float yMin_;
+	std::map<int, Transform> addedNodes_;
+
+	bool cloudAssembling_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledEmptyCells_;
+};
+
+}
+
+#include <rtabmap/core/impl/OccupancyGrid.hpp>
+
+#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -25,14 +25,223 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_
-#define CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_
+#ifndef SRC_OCTOMAP_H_
+#define SRC_OCTOMAP_H_

-/*
- * Deprecated header, use the one below directly!
- */
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines

-#include <rtabmap/core/global_map/OctoMap.h>
+#include <octomap/ColorOcTree.h>
+#include <octomap/OcTreeKey.h>

+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>

-#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_ */
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
+
+#include <map>
+#include <unordered_set>
+#include <string>
+#include <queue>
+
+namespace rtabmap {
+
+// forward declaraton for "friend"
+class RtabmapColorOcTree;
+
+class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode
+{
+public:
+	enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100};
+
+public:
+	friend class RtabmapColorOcTree; // needs access to node children (inherited)
+
+	RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {}
+	RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
+
+	void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
+	void setOccupancyType(char type) {type_=type;}
+	void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
+	int getNodeRefId() const {return nodeRefId_;}
+	int getOccupancyType() const {return type_;}
+	const octomap::point3d & getPointRef() const {return pointRef_;}
+
+	// following methods defined for octomap < 1.8 compatibility
+	RtabmapColorOcTreeNode* getChild(unsigned int i);
+	const RtabmapColorOcTreeNode* getChild(unsigned int i) const;
+	bool pruneNode();
+	void expandNode();
+	bool createChild(unsigned int i);
+
+	void updateOccupancyTypeChildren();
+
+private:
+	int nodeRefId_;
+	int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
+	octomap::point3d pointRef_;
+};
+
+// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
+class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
+
+  public:
+    /// Default constructor, sets resolution of leafs
+	RtabmapColorOcTree(double resolution);
+	virtual ~RtabmapColorOcTree() {}
+
+    /// virtual constructor: creates a new object of same type
+    /// (Covariant return type requires an up-to-date compiler)
+	RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
+
+    std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
+
+     /**
+     * Prunes a node when it is collapsible. This overloaded
+     * version only considers the node occupancy for pruning,
+     * different colors of child nodes are ignored.
+     * @return true if pruning was successful
+     */
+    virtual bool pruneNode(RtabmapColorOcTreeNode* node);
+
+    virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
+
+    // set node color at given key or coordinate. Replaces previous color.
+    RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                 uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* setNodeColor(float x, float y,
+                                 float z, uint8_t r,
+                                 uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return setNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* averageNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap:: OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return averageNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* integrateNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return integrateNodeColor(key,r,g,b);
+    }
+
+    // update inner nodes, sets color to average child color
+    void updateInnerOccupancy();
+
+  protected:
+    void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
+
+    /**
+     * Static member object which ensures that this OcTree's prototype
+     * ends up in the classIDMapping only once. You need this as a
+     * static member in any derived octree class in order to read .ot
+     * files through the AbstractOcTree factory. You should also call
+     * ensureLinking() once from the constructor.
+     */
+    class StaticMemberInitializer{
+       public:
+         StaticMemberInitializer();
+
+         /**
+         * Dummy function to ensure that MSVC does not drop the
+         * StaticMemberInitializer, causing this tree failing to register.
+         * Needs to be called from the constructor of this octree.
+         */
+         void ensureLinking() {};
+    };
+    /// static member to ensure static initialization (only once)
+    static StaticMemberInitializer RtabmapColorOcTreeMemberInit;
+
+  };
+
+class RTABMAP_CORE_EXPORT OctoMap {
+public:
+	OctoMap(const ParametersMap & parameters = ParametersMap());
+
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	void addToCache(int nodeId,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+			const pcl::PointXYZ & viewPoint);
+	void addToCache(int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			const cv::Point3f & viewPoint);
+	bool update(const std::map<int, Transform> & poses); // return true if map has changed
+
+	const RtabmapColorOcTree * octree() const {return octree_;}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			unsigned int treeDepth = 0,
+			std::vector<int> * obstacleIndices = 0,
+			std::vector<int> * emptyIndices = 0,
+			std::vector<int> * groundIndices = 0,
+			bool originalRefPoints = true,
+			std::vector<int> * frontierIndices = 0,
+			std::vector<double> * cloudProb = 0) const;
+
+	cv::Mat createProjectionMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize,
+			float minGridSize = 0.0f,
+			unsigned int treeDepth = 0);
+
+	bool writeBinary(const std::string & path);
+
+	virtual ~OctoMap();
+	void clear();
+
+	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
+	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
+
+	void setMaxRange(float value) {rangeMax_ = value;}
+	void setRayTracing(bool enabled) {rayTracing_ = enabled;}
+	bool hasColor() const {return hasColor_;}
+
+    static std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition);
+    static void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore);
+    static bool isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key);
+    static octomap::point3d findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
+    static bool isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
+
+private:
+	void updateMinMax(const octomap::point3d & point);
+
+private:
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; // [id: < <ground, obstacles>, empty>]
+	std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
+	std::map<int, cv::Point3f> cacheViewPoints_;
+	RtabmapColorOcTree * octree_;
+	std::map<int, Transform> addedNodes_;
+	bool hasColor_;
+	bool fullUpdate_;
+	float updateError_;
+	float rangeMax_;
+	bool rayTracing_;
+    unsigned int emptyFloodFillDepth_;
+	double minValues_[3];
+	double maxValues_[3];
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_OCTOMAP_H_ */
@@ -79,7 +79,8 @@ public:
 			const std::map<int, Transform> & posesIn,
 			const std::multimap<int, Link> & linksIn,
 			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut) const;
+			std::multimap<int, Link> & linksOut,
+			bool adjustPosesWithConstraints = true) const;

 public:
 	virtual ~Optimizer() {}
@@ -231,7 +231,6 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features instead of regenerating them.");
    RTABMAP_PARAM(Mem, UseOdomGravity,              bool, false,    uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
-    RTABMAP_PARAM(Mem, RotateImagesUpsideUp,        bool, false,    "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");

    // KeypointMemory (Keypoint-based)
    RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -378,7 +377,6 @@ class RTABMAP_CORE_EXPORT 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.");
    RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  10,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
    RTABMAP_PARAM(RGBD, LocalizationSmoothing,        bool, true,    uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
-    RTABMAP_PARAM(RGBD, LocalizationPriorError,       double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));

    // Local/Proximity loop closure detection
    RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
@@ -528,19 +526,12 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");

    // Odometry ORB_SLAM2
-    RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",       "Path to ORB vocabulary (*.txt).");
-    RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076,    "Fake IR projector baseline (m) used only when stereo is not used.");
-    RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,     "Close/Far threshold. Baseline times.");
-    RTABMAP_PARAM(OdomORBSLAM, Fps,             float,  0.0,      "Camera FPS (0 to estimate from input data).");
-    RTABMAP_PARAM(OdomORBSLAM, MaxFeatures,     int,    1000,     "Maximum ORB features extracted per frame.");
-    RTABMAP_PARAM(OdomORBSLAM, MapSize,         int,    3000,     "Maximum size of the feature map (0 means infinite). Only supported with ORB_SLAM2.");
-    RTABMAP_PARAM(OdomORBSLAM, Inertial,        bool,   false,    "Enable IMU. Only supported with ORB_SLAM3.");
-    RTABMAP_PARAM(OdomORBSLAM, GyroNoise,       double, 0.01,     "IMU gyroscope \"white noise\".");
-    RTABMAP_PARAM(OdomORBSLAM, AccNoise,        double, 0.1,      "IMU accelerometer \"white noise\".");
-    RTABMAP_PARAM(OdomORBSLAM, GyroWalk,        double, 0.000001, "IMU gyroscope \"random walk\".");
-    RTABMAP_PARAM(OdomORBSLAM, AccWalk,         double, 0.0001,   "IMU accelerometer \"random walk\".");
-    RTABMAP_PARAM(OdomORBSLAM, SamplingRate,    double, 0,        "IMU sampling rate (0 to estimate from input data).");
-
+    RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM, Fps,             float,  0.0,   "Camera FPS.");
+    RTABMAP_PARAM(OdomORBSLAM, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
+    RTABMAP_PARAM(OdomORBSLAM, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");

    // Odometry OKVIS
    RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
@@ -585,68 +576,6 @@ class RTABMAP_CORE_EXPORT Parameters
    // Odometry VINS
    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");

-    // Odometry OpenVINS
-    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
-    RTABMAP_PARAM(OdomOpenVINS, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching");
-    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track");
-    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information)");
-    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d");
-    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline,             double, 40,     "Max baseline ratio to accept triangulated features");
-    RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber,           double, 10000,  "Max condition number of linear triangulation matrix accept triangulated features");
-
-    RTABMAP_PARAM(OdomOpenVINS, UseFEJ,                    bool,   true,   "If first-estimate Jacobians should be used (enable for good consistency)");
-    RTABMAP_PARAM(OdomOpenVINS, Integration,               int,    1,      "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics,        bool,   false,  "Bool to determine whether or not to calibrate imu-to-camera pose");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate camera intrinsics");
-    RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset,        bool,   false,  "Bool to determine whether or not to calibrate camera to IMU time offset");
-    RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate the IMU intrinsics");
-    RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity,      bool,   false,  "Bool to determine whether or not to calibrate the Gravity sensitivity");
-    RTABMAP_PARAM(OdomOpenVINS, MaxClones,                 int,    11,     "Max clone size of sliding window");
-    RTABMAP_PARAM(OdomOpenVINS, MaxSLAM,                   int,    50,     "Max number of estimated SLAM features");
-    RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate,           int,    25,     "Max number of SLAM features we allow to be included in a single EKF update.");
-    RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate,          int,    50,     "Max number of MSCKF features we will use at a given image timestep.");
-    RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF,              int,    0,      "What representation our features are in (msckf features)");
-    RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM,               int,    4,      "What representation our features are in (slam features)");
-    RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay,               double, 0.0,    "Delay, in seconds, that we should wait from init before we start estimating SLAM features");
-    RTABMAP_PARAM(OdomOpenVINS, GravityMag,                double, 9.81,   "Gravity magnitude in the global frame (i.e. should be 9.81 typically)");
-    RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath,                  "",     "Mask for left image");
-    RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath,                 "",     "Mask for right image");
-
-    RTABMAP_PARAM(OdomOpenVINS, InitWindowTime,            double, 2.0,    "Amount of time we will initialize over (seconds)");
-    RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh,             double, 1.0,    "Variance threshold on our acceleration to be classified as moving");
-    RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity,          double, 10.0,   "Max disparity to consider the platform stationary (dependent on resolution)");
-    RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures,           int,    50,     "How many features to track during initialization (saves on computation)");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynUse,                bool,   false,  "If dynamic initialization should be used");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib,        bool,   false,  "If we should optimize calibration during intialization (not recommended)");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter,         int,    50,     "How many iterations the MLE refinement should use (zero to skip the MLE)");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime,         double, 0.05,   "How many seconds the MLE should be completed in");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads,      int,    6,      "How many threads the MLE should use");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose,            int,    6,      "Number of poses to use within our window time (evenly spaced)");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg,             double, 10.0,   "Orientation change needed to try to init");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri,       double, 10.0,   "What to inflate the recovered q_GtoI covariance by");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel,       double, 100.0,  "What to inflate the recovered v_IinG covariance by");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg,        double, 10.0,   "What to inflate the recovered bias_g covariance by");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa,        double, 100.0,  "What to inflate the recovered bias_a covariance by");
-    RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond,         double, 1e-15,  "Reciprocal condition number thresh for info inversion");
-
-    RTABMAP_PARAM(OdomOpenVINS, TryZUPT,                   bool,   true,   "If we should try to use zero velocity update");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier,        double, 0.0,    "Chi2 multiplier for zero velocity");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy,           double, 0.1,    "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier,       double, 10.0,   "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity,          double, 0.5,    "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
-    RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning,       bool,   false,  "If we should only use the zupt at the very beginning static initialization phase");
-
-    RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01,   "[m/s^2/sqrt(Hz)] (accel \"white noise\")");
-    RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk,   double, 0.001,  "[m/s^3/sqrt(Hz)] (accel bias diffusion)");
-    RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity,     double, 0.001,  "[rad/s/sqrt(Hz)] (gyro \"white noise\")");
-    RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk,       double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)");
-    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise for MSCKF features");
-    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier,     double, 1.0,    "Chi2 multiplier for MSCKF features");
-    RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx,             double, 1.0,    "Pixel noise for SLAM features");
-    RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features");
-
    // Odometry Open3D
    RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
    RTABMAP_PARAM(OdomOpen3D, Method,           int, 0,  "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");
@@ -657,57 +586,54 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");

    // Visual registration parameters
-    RTABMAP_PARAM(Vis, EstimationType,           int,    1,     "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
-    RTABMAP_PARAM(Vis, ForwardEstOnly,           bool,   true,  "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
-    RTABMAP_PARAM(Vis, InlierDistance,           float,  0.1,   uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
-    RTABMAP_PARAM(Vis, RefineIterations,         int,    5,     uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
-    RTABMAP_PARAM(Vis, PnPReprojError,           float,  2,     uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
-    RTABMAP_PARAM(Vis, PnPFlags,                 int,    0,     uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, EstimationType,           int, 1,        "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
+    RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true,    "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
+    RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, PnPReprojError,           float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, PnPFlags,                 int, 0,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
-    RTABMAP_PARAM(Vis, PnPRefineIterations,      int,    0,     uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
+    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 0,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #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()));
+    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, PnPVarianceMedianRatio,   int,    4,     uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str()));
-    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, PnPSamplingPolicy, unsigned int,  1,     uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str());
-    RTABMAP_PARAM(Vis, PnPSplitLinearCovComponents, bool,  false, uFormat("[%s = 1] Compute variance for each linear component instead of using the combined XYZ variance for all linear components.", kVisEstimationType().c_str()).c_str());
+    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.");
-    RTABMAP_PARAM(Vis, MeanInliersDistance,      float,  0.0,   "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.");
-    RTABMAP_PARAM(Vis, MinInliersDistribution,   float,  0.0,   "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled.");
+    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.");
+    RTABMAP_PARAM(Vis, MeanInliersDistance,      float, 0.0,    "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.");
+    RTABMAP_PARAM(Vis, MinInliersDistribution,   float, 0.0,    "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled.");

-    RTABMAP_PARAM(Vis, Iterations,               int,    300,   "Maximum iterations to compute the transform.");
+    RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
    RTABMAP_PARAM(Vis, FeatureType, int, 8, "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 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #else
    RTABMAP_PARAM(Vis, FeatureType, int, 6, "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 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #endif
-    RTABMAP_PARAM(Vis, MaxFeatures,               int,   1000,  "0 no limits.");
-    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
-    RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
-    RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
-    RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
-    RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
-    RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");
-    RTABMAP_PARAM(Vis, SubPixEps,                 float, 0.02,  "See cv::cornerSubPix().");
-    RTABMAP_PARAM(Vis, GridRows,                  int,   1,     uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
-    RTABMAP_PARAM(Vis, GridCols,                  int,   1,     uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
-    RTABMAP_PARAM(Vis, CorType,                   int,   0,     "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-    RTABMAP_PARAM(Vis, CorNNType,                 int,   1,     uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorNNDR,                   float, 0.8,   uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorGuessWinSize,           int,   40,    uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool,  false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorFlowWinSize,            int,   16,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorFlowIterations,         int,   30,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorFlowEps,                float, 0.01,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorFlowMaxLevel,           int,   3,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,   "0 no limits.");
+    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,   "Max depth of the features (0 means no limit).");
+    RTABMAP_PARAM(Vis, MinDepth,                  float, 0,   "Min depth of the features (0 means no limit).");
+    RTABMAP_PARAM(Vis, DepthAsMask,               bool, true, "Use depth image as mask when extracting features.");
+    RTABMAP_PARAM_STR(Vis, RoiRatios,      "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
+    RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,      "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,      "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+    RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
+    RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
+    RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
+    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 40,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,      uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
-    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   1,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 1,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
-    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   0,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #endif

    // Features matching approaches
@@ -834,10 +760,13 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #endif
    RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, UndergroundIsGround,        bool,   false,   uFormat("[%s=true] Label all underground points under largest flat surface detected as ground.", kGridNormalsSegmentation().c_str()));
    RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
    RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
    RTABMAP_PARAM(Grid, RayTracing,                 bool,   false,   uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
+
+    RTABMAP_PARAM(GridGlobal, FullUpdate,           bool,   true,    "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
    RTABMAP_PARAM(GridGlobal, UpdateError,          float,  0.01,    "Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value.");
    RTABMAP_PARAM(GridGlobal, FootprintRadius,      float,  0.0,     "Footprint radius (m) used to clear all obstacles under the graph.");
    RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
@@ -11,30 +11,30 @@

 #include <string>
 #include <rtabmap/utilite/UMutex.h>
-
-namespace pybind11 {
-class scoped_interpreter;
-class gil_scoped_release;
-}
+#include <Python.h>

 namespace rtabmap {

-/**
- * Create a single PythonInterface on main thread at
- * global scope before any Python classes.
- */
 class PythonInterface
 {
 public:
 	PythonInterface();
 	virtual ~PythonInterface();

-private:
-	pybind11::scoped_interpreter* guard_;
-	pybind11::gil_scoped_release* release_;
-};
+protected:
+	std::string getTraceback(); // should be called between lock() and unlock()
+	void lock();
+	void unlock();

-std::string getPythonTraceback();
+private:
+	static UMutex mutex_;
+	static int refCount_;
+
+protected:
+	static PyThreadState * mainThreadState_;
+	static unsigned long mainThreadID_;
+	PyThreadState * threadState_;
+};

 }

@@ -82,10 +82,7 @@ private:
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;
-	int  _PnPVarMedianRatio;
 	float _PnPMaxVar;
-	bool _PnPSplitLinearCovarianceComponents;
-	unsigned int _multiSamplingPolicy;
 	int _correspondencesApproach;
 	int _flowWinSize;
 	int _flowIterations;
@@ -327,7 +327,6 @@ private:
 	bool _loopGPS;
 	int _maxOdomCacheSize;
 	bool _localizationSmoothing;
-	double _localizationPriorInf;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;
@@ -49,21 +49,18 @@ public:

 public:
 	CameraDepthAI(
-			const std::string & mxidOrName = "",
+			const std::string & deviceSerial = "",
 			int resolution = 1, // 0=720p, 1=800p, 2=400p
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraDepthAI();

-	void setOutputMode(int outputMode = 0);
-	void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
-	void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
-	void setIMU(bool imuPublished, bool publishInterIMU);
-	void setIrBrightness(float dotProjectormA = 0.0f, float floodLightmA = 200.0f);
-	void setDetectFeatures(int detectFeatures = 0);
-	void setBlobPath(const std::string & blobPath);
-	void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
-	void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4);
+	void setOutputDepth(bool enabled, int confidence = 200);
+	void setIMUFirmwareUpdate(bool enabled);
+	void setIMUPublished(bool published);
+	void publishInterIMU(bool enabled);
+	void setLaserDotBrightness(float dotProjectormA = 0.0f);
+	void setFloodLightBrightness(float floodLightmA = 200.0f);

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -75,31 +72,19 @@ protected:
 private:
 #ifdef RTABMAP_DEPTHAI
 	StereoCameraModel stereoModel_;
-	cv::Size targetSize_;
 	Transform imuLocalTransform_;
-	std::string mxidOrName_;
-	int outputMode_;
-	int confThreshold_;
-	int lrcThreshold_;
+	std::string deviceSerial_;
+	bool outputDepth_;
+	int depthConfidence_;
 	int resolution_;
-	bool useSpecTranslation_;
-	float alphaScaling_;
+	bool imuFirmwareUpdate_;
 	bool imuPublished_;
 	bool publishInterIMU_;
 	float dotProjectormA_;
 	float floodLightmA_;
-	int detectFeatures_;
-	bool useHarrisDetector_;
-	float minDistance_;
-	int numTargetFeatures_;
-	float threshold_;
-	bool nms_;
-	int nmsRadius_;
-	std::string blobPath_;
 	std::shared_ptr<dai::Device> device_;
-	std::shared_ptr<dai::DataOutputQueue> leftOrColorQueue_;
+	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
-	std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
 	UMutex imuMutex_;
@@ -1,64 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef CORELIB_SRC_CLOUDMAP_H_
-#define CORELIB_SRC_CLOUDMAP_H_
-
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <rtabmap/core/GlobalMap.h>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT CloudMap : public GlobalMap
-{
-public:
-	CloudMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
-
-	virtual void clear();
-
-	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
-	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
-	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
-
-	unsigned long getMemoryUsed() const;
-
-protected:
-	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses);
-
-private:
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
-	pcl::PointCloud<pcl::PointXYZ>::Ptr assembledEmptyCells_;
-};
-
-}
-
-#endif /* CORELIB_SRC_CLOUDMAP_H_ */
@@ -1,69 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef CORELIB_SRC_GRIDMAP_H_
-#define CORELIB_SRC_GRIDMAP_H_
-
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <rtabmap/core/GlobalMap.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/PolygonMesh.h>
-
-#include <grid_map_core/GridMap.hpp>
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT GridMap : public GlobalMap
-{
-public:
-	GridMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
-
-	virtual void clear();
-
-	const grid_map::GridMap & gridMap() const {return gridMap_;}
-
-	cv::Mat createHeightMap(float & xMin, float & yMin, float & cellSize) const;
-	cv::Mat createColorMap(float & xMin, float & yMin, float & cellSize) const;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createTerrainCloud() const;
-	pcl::PolygonMesh::Ptr createTerrainMesh() const;
-
-protected:
-	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses);
-
-private:
-	cv::Mat toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const;
-
-private:
-	grid_map::GridMap gridMap_;
-	float minMapSize_;
-};
-
-}
-
-#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */
@@ -1,69 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef CORELIB_SRC_OCCUPANCYGRID_H_
-#define CORELIB_SRC_OCCUPANCYGRID_H_
-
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <rtabmap/core/GlobalMap.h>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT OccupancyGrid : public GlobalMap
-{
-public:
-	OccupancyGrid(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
-	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
-	float getMinMapSize() const {return minMapSize_;}
-
-	virtual void clear();
-
-	cv::Mat getMap(float & xMin, float & yMin) const;
-	cv::Mat getProbMap(float & xMin, float & yMin) const;
-
-	unsigned long getMemoryUsed() const;
-
-protected:
-	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses);
-
-private:
-	cv::Mat map_;
-	cv::Mat mapInfo_;
-	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
-
-	float minMapSize_;
-	bool erode_;
-	float footprintRadius_;
-};
-
-}
-
-#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */
@@ -1,227 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef SRC_OCTOMAP_H_
-#define SRC_OCTOMAP_H_
-
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <octomap/ColorOcTree.h>
-#include <octomap/OcTreeKey.h>
-
-#include <pcl/pcl_base.h>
-#include <pcl/point_types.h>
-
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/GlobalMap.h>
-
-#include <map>
-#include <unordered_set>
-#include <string>
-#include <queue>
-
-namespace rtabmap {
-
-// forward declaraton for "friend"
-class RtabmapColorOcTree;
-
-class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode
-{
-public:
-	enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100};
-
-public:
-	friend class RtabmapColorOcTree; // needs access to node children (inherited)
-
-	RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {}
-	RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
-
-	void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
-	void setOccupancyType(char type) {type_=type;}
-	void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
-	int getNodeRefId() const {return nodeRefId_;}
-	int getOccupancyType() const {return type_;}
-	const octomap::point3d & getPointRef() const {return pointRef_;}
-
-	// following methods defined for octomap < 1.8 compatibility
-	RtabmapColorOcTreeNode* getChild(unsigned int i);
-	const RtabmapColorOcTreeNode* getChild(unsigned int i) const;
-	bool pruneNode();
-	void expandNode();
-	bool createChild(unsigned int i);
-
-	void updateOccupancyTypeChildren();
-
-private:
-	int nodeRefId_;
-	int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
-	octomap::point3d pointRef_;
-};
-
-// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
-class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
-
-  public:
-    /// Default constructor, sets resolution of leafs
-	RtabmapColorOcTree(double resolution);
-	virtual ~RtabmapColorOcTree() {}
-
-    /// virtual constructor: creates a new object of same type
-    /// (Covariant return type requires an up-to-date compiler)
-	RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
-
-    std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
-
-     /**
-     * Prunes a node when it is collapsible. This overloaded
-     * version only considers the node occupancy for pruning,
-     * different colors of child nodes are ignored.
-     * @return true if pruning was successful
-     */
-    virtual bool pruneNode(RtabmapColorOcTreeNode* node);
-
-    virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
-
-    // set node color at given key or coordinate. Replaces previous color.
-    RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
-                                 uint8_t g, uint8_t b);
-
-    RtabmapColorOcTreeNode* setNodeColor(float x, float y,
-                                 float z, uint8_t r,
-                                 uint8_t g, uint8_t b) {
-    	octomap::OcTreeKey key;
-      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
-      return setNodeColor(key,r,g,b);
-    }
-
-    // integrate color measurement at given key or coordinate. Average with previous color
-    RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
-                                  uint8_t g, uint8_t b);
-
-    RtabmapColorOcTreeNode* averageNodeColor(float x, float y,
-                                      float z, uint8_t r,
-                                      uint8_t g, uint8_t b) {
-    	octomap:: OcTreeKey key;
-      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
-      return averageNodeColor(key,r,g,b);
-    }
-
-    // integrate color measurement at given key or coordinate. Average with previous color
-    RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
-                                  uint8_t g, uint8_t b);
-
-    RtabmapColorOcTreeNode* integrateNodeColor(float x, float y,
-                                      float z, uint8_t r,
-                                      uint8_t g, uint8_t b) {
-    	octomap::OcTreeKey key;
-      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
-      return integrateNodeColor(key,r,g,b);
-    }
-
-    // update inner nodes, sets color to average child color
-    void updateInnerOccupancy();
-
-  protected:
-    void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
-
-    /**
-     * Static member object which ensures that this OcTree's prototype
-     * ends up in the classIDMapping only once. You need this as a
-     * static member in any derived octree class in order to read .ot
-     * files through the AbstractOcTree factory. You should also call
-     * ensureLinking() once from the constructor.
-     */
-    class StaticMemberInitializer{
-       public:
-         StaticMemberInitializer();
-
-         /**
-         * Dummy function to ensure that MSVC does not drop the
-         * StaticMemberInitializer, causing this tree failing to register.
-         * Needs to be called from the constructor of this octree.
-         */
-         void ensureLinking() {};
-    };
-    /// static member to ensure static initialization (only once)
-    static StaticMemberInitializer RtabmapColorOcTreeMemberInit;
-
-  };
-
-class RTABMAP_CORE_EXPORT OctoMap : public GlobalMap {
-public:
-	OctoMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
-
-	const RtabmapColorOcTree * octree() const {return octree_;}
-
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
-			unsigned int treeDepth = 0,
-			std::vector<int> * obstacleIndices = 0,
-			std::vector<int> * emptyIndices = 0,
-			std::vector<int> * groundIndices = 0,
-			bool originalRefPoints = true,
-			std::vector<int> * frontierIndices = 0,
-			std::vector<double> * cloudProb = 0) const;
-
-	cv::Mat createProjectionMap(
-			float & xMin,
-			float & yMin,
-			float & gridCellSize,
-			float minGridSize = 0.0f,
-			unsigned int treeDepth = 0);
-
-	bool writeBinary(const std::string & path);
-
-	virtual ~OctoMap();
-	virtual void clear();
-	virtual unsigned long getMemoryUsed() const;
-
-	bool hasColor() const {return hasColor_;}
-
-    static std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition);
-    static void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore);
-    static bool isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key);
-    static octomap::point3d findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
-    static bool isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
-
-protected:
-	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses);
-
-private:
-	void updateMinMax(const octomap::point3d & point);
-
-private:
-	RtabmapColorOcTree * octree_;
-	bool hasColor_;
-	float rangeMax_;
-	bool rayTracing_;
-    unsigned int emptyFloodFillDepth_;
-};
-
-} /* namespace rtabmap */
-
-#endif /* SRC_OCTOMAP_H_ */
@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_
-#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_

 #include <rtabmap/core/util3d_mapping.h>
 #include <rtabmap/core/util3d_transforms.h>
@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {

 template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
+typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
 		const pcl::IndicesPtr & indicesIn,
 		const Transform & pose,
@@ -44,6 +44,8 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 		pcl::IndicesPtr & obstaclesIndices,
 		pcl::IndicesPtr * flatObstacles) const
 {
+	UDEBUG("cloudIn=%dx%d indicesIn=%ld", cloudIn->width, cloudIn->height, indicesIn->size());
+
 	groundIndices.reset(new std::vector<int>);
 	obstaclesIndices.reset(new std::vector<int>);
 	if(flatObstacles)
@@ -54,6 +56,7 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 	pcl::IndicesPtr indices(new std::vector<int>);

+	UDEBUG("preVoxelFiltering=%d", preVoxelFiltering_?1:0);
 	if(preVoxelFiltering_)
 	{
 		// voxelize to grid cell size
@@ -127,6 +130,9 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
 			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
 			UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
+			UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0);
+			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+			UDEBUG("cloud=%dx%d indices=%ld", cloud->width, cloud->height, indices->size());
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -140,8 +146,8 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 					maxGroundHeight_,
 					flatObstacles,
 					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
-					groundNormalsUp_);
-			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+					groundNormalsUp_,
+					labelUndergroundObstaclesAsGround_);
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
 			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
@@ -165,6 +171,42 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(

 		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());

+		if(!preVoxelFiltering_ && (!groundIndices->empty() || !obstaclesIndices->empty()))
+		{
+			// voxelize to grid cell size
+			typename pcl::PointCloud<PointT>::Ptr cloudWithTransform = cloud;
+			cloud.reset(new pcl::PointCloud<PointT>);
+			if(!groundIndices->empty())
+			{
+				*cloud += *util3d::voxelize(cloudWithTransform, groundIndices, cellSize_);
+				groundIndices->resize(cloud->size());
+				for(size_t i=0; i<groundIndices->size(); ++i)
+				{
+					groundIndices->at(i) = i;
+				}
+			}
+			if(!obstaclesIndices->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, obstaclesIndices, cellSize_);
+				obstaclesIndices->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<obstaclesIndices->size(); ++i)
+				{
+					obstaclesIndices->at(i) = previousSize+i;
+				}
+			}
+			if(flatObstacles && !(*flatObstacles)->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, *flatObstacles, cellSize_);
+				(*flatObstacles)->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<(*flatObstacles)->size(); ++i)
+				{
+					(*flatObstacles)->at(i) = previousSize+i;
+				}
+			}
+		}
+
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
 		{
@@ -205,4 +247,4 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 }


-#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_ */
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ */
@@ -50,6 +50,79 @@ typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 	return output;
 }

+void clusterIndicesFloodfill(std::vector<int> & cluster,
+	float * visitedIndices,
+	int width,
+	int height,
+	float clusterRadius,
+	int currentIndex,
+	float previousHeight);
+
+/**
+ * @brief Cluster indices of an organized cloud
+ * 
+ * @tparam PointT 
+ * @param cloud 
+ * @param indices 
+ * @param minClusterSize 
+ * @param maxClusterSize 
+ * @param biggestClusterIndex 
+ * @return std::vector<pcl::IndicesPtr> 
+ */
+template<typename PointT>
+std::vector<pcl::IndicesPtr> clusterIndices(
+	const typename pcl::PointCloud<PointT>::Ptr & cloud,
+	const typename pcl::IndicesPtr & indices,
+	float clusterRadius,
+	int minClusterSize,
+	int maxClusterSize,
+	int * biggestClusterIndex)
+{
+	std::vector<pcl::IndicesPtr> clusters;
+	if(cloud->empty())
+	{
+		return clusters;
+	}
+
+	UASSERT(cloud->isOrganized());
+
+	cv::Mat visitedIndices = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
+	float * ptr = visitedIndices.ptr<float>();
+	// init search image
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		ptr[indices->at(i)] = cloud->at(indices->at(i)).z;
+	}
+
+	int largestCluster = -1;
+	int largestClusterSize = 0;
+	int sum = 0;
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		if(ptr[indices->at(i)] != 0.0f)
+		{
+			pcl::IndicesPtr cluster(new pcl::Indices());
+			clusterIndicesFloodfill(*cluster, ptr, visitedIndices.cols, visitedIndices.rows, clusterRadius, indices->at(i), ptr[indices->at(i)]);
+			if(cluster->size()>0 && (int)cluster->size()>=minClusterSize && (int)cluster->size()<=maxClusterSize)
+			{
+				clusters.push_back(cluster);
+				if((int)cluster->size() > largestClusterSize)
+				{
+					sum+=cluster->size();
+					largestCluster = clusters.size()-1;
+					largestClusterSize = cluster->size();
+				}
+			}
+		}
+	}
+	if(biggestClusterIndex)
+	{
+		*biggestClusterIndex = largestCluster;
+	}
+
+	return clusters;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -64,7 +137,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -75,6 +149,8 @@ void segmentObstaclesFromGround(

 	if(cloud->size())
 	{
+		UDEBUG("Normal filtering.... cloud=%ld indices=%ld organized=%d",
+			cloud->size(), indices->size(), cloud->isOrganized()?1:0);
 		// Find the ground
 		pcl::IndicesPtr flatSurfaces = normalFiltering(
 				cloud,
@@ -84,22 +160,40 @@ void segmentObstaclesFromGround(
 				normalKSearch,
 				viewPoint,
 				groundNormalsUp);
+		UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)",
+			flatSurfaces->size(), indices->size(), cloud->size());

+		Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax(0,0,0,0);
 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
 			int biggestFlatSurfaceIndex;
-			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
+
+			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces;
+			if(cloud->isOrganized())
+			{
+				clusteredFlatSurfaces = clusterIndices<PointT>(
 					cloud,
 					flatSurfaces,
 					clusterRadius,
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);
+					UDEBUG("clusteredFlatSurfaces=%ld", clusteredFlatSurfaces.size());
+			}
+			else
+			{
+				clusteredFlatSurfaces = extractClusters(
+					cloud,
+					flatSurfaces,
+					clusterRadius,
+					minClusterSize,
+					std::numeric_limits<int>::max(),
+					&biggestFlatSurfaceIndex);
+			}

 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
 			{
-				Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
 				if(maxGroundHeight != 0.0f)
 				{
 					// Search for biggest surface under max ground height
@@ -125,9 +219,12 @@ void segmentObstaclesFromGround(
 				if(biggestFlatSurfaceIndex>=0)
 				{
 					ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+					UDEBUG("Biggest flat surface size = %ld (%d%%) (z min=%f max=%f)",
+						ground->size(), 100*ground->size()/cloud->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]);
 				}

-				if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
+				if(!ground->empty() && 
+				  (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
@@ -135,7 +232,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
+							if(centroid[2] <= biggestSurfaceMax[2]) // relative to ground detected
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -145,9 +242,46 @@ void segmentObstaclesFromGround(
 							}
 						}
 					}
+
+					int groundRatio = 100*ground->size()/cloud->size();
+					int minGroundRatio = 10;
+					if(minGroundRatio != 0 && groundRatio<minGroundRatio)
+					{
+						if(labelUndergroundObstaclesAsGround && maxGroundHeight!=0.0f)
+						{
+							// just do passthrough (e.g. reflective floor)
+							UWARN("Failed normal segmentation (ground ratio=%d%%, ground height=%f), fallback to passThrough (label underground as ground is true).",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// passthrough filter
+							ground = rtabmap::util3d::passThrough(cloud, indices, "z",
+									std::numeric_limits<int>::min(),
+									maxGroundHeight!=0.0f?maxGroundHeight:std::numeric_limits<int>::max());
+
+							pcl::IndicesPtr notObstacles = ground;
+							if(indices->size())
+							{
+								notObstacles = util3d::extractIndices(cloud, indices, true);
+								notObstacles = util3d::concatenate(notObstacles, ground);
+							}
+							obstacles = rtabmap::util3d::extractIndices(cloud, notObstacles, true);
+							return;
+						}
+						else
+						{
+							UWARN("Failed normal segmentation, ground surface is too small (ground ratio=%d%%, ground height=%f)!",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// reject ground!
+							ground.reset(new std::vector<int>);
+							if(flatObstacles)
+							{
+								*flatObstacles = flatSurfaces;
+							}
+						}
+					}
 				}
 				else
 				{
+					UWARN("Failed normal segmentation, could not detect the ground!");
 					// reject ground!
 					ground.reset(new std::vector<int>);
 					if(flatObstacles)
@@ -168,28 +302,49 @@ void segmentObstaclesFromGround(
 			pcl::IndicesPtr notObstacles = ground;
 			if(indices->size())
 			{
+				// This will ignore all points not in input indices for obstacles.
 				notObstacles = util3d::extractIndices(cloud, indices, true);
 				notObstacles = util3d::concatenate(notObstacles, ground);
 			}
-			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);

-			// If ground height is set, remove obstacles under it
-			if(maxGroundHeight != 0.0f)
+			// If ground height is set and if we label obstacles under it as ground
+			if(labelUndergroundObstaclesAsGround)
 			{
-				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+				float max = biggestSurfaceMax[2];
+				if(maxGroundHeight > 0)
+				{
+					max += maxGroundHeight;
+				}
+
+				pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+				pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits<int>::min(), max);
+				if(!underground->empty())
+				{
+					ground = util3d::concatenate(ground, underground);
+					notObstacles = util3d::concatenate(underground, notObstacles);
+				}
 			}

+			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+
 			//Cluster remaining stuff (obstacles)
 			if(otherStuffIndices->size())
 			{
-				std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
-						cloud,
-						otherStuffIndices,
-						clusterRadius,
-						minClusterSize);
+				if(minClusterSize>1)
+				{
+					std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+							cloud,
+							otherStuffIndices,
+							clusterRadius,
+							minClusterSize);

-				// merge indices
-				obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+					// merge indices
+					obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+				}
+				else
+				{
+					obstacles = otherStuffIndices;
+				}
 			}
 		}
 	}
@@ -208,7 +363,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -224,7 +380,8 @@ void segmentObstaclesFromGround(
 			maxGroundHeight,
 			flatObstacles,
 			viewPoint,
-			groundNormalsUp);
+			groundNormalsUp,
+			labelUndergroundObstaclesAsGround);
 }

 template<typename PointT>
@@ -25,38 +25,48 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef ODOMETRYORBSLAM2_H_
-#define ODOMETRYORBSLAM2_H_
+#ifndef ODOMETRYORBSLAM_H_
+#define ODOMETRYORBSLAM_H_

 #include <rtabmap/core/Odometry.h>

+#if RTABMAP_ORB_SLAM == 3
+namespace ORB_SLAM3 {
+#else
+namespace ORB_SLAM2 {
+#endif
+class System;
+}

-class ORBSLAM2System;
+class ORBSLAMSystem;

 namespace rtabmap {

-class RTABMAP_CORE_EXPORT OdometryORBSLAM2 : public Odometry
+class RTABMAP_CORE_EXPORT OdometryORBSLAM : public Odometry
 {
 public:
-	OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryORBSLAM2();
+	OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryORBSLAM();

 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;}
+	virtual bool canProcessAsyncIMU() const;

 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

 private:
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
-	ORBSLAM2System * orbslam_;
+#ifdef RTABMAP_ORB_SLAM
+	ORBSLAMSystem * orbslam_;
 	bool firstFrame_;
 	Transform originLocalTransform_;
 	Transform previousPose_;
+	bool useIMU_;
+	Transform imuLocalTransform_;
 #endif

 };

 }

-#endif /* ODOMETRYORBSLAM2_H_ */
+#endif /* ODOMETRYORBSLAM_H_ */
@@ -1,71 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef ODOMETRYORBSLAM3_H_
-#define ODOMETRYORBSLAM3_H_
-
-#include <rtabmap/core/Odometry.h>
-
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-#include <System.h>
-#endif
-
-namespace rtabmap {
-
-class RTABMAP_CORE_EXPORT OdometryORBSLAM3 : public Odometry
-{
-public:
-	OdometryORBSLAM3(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryORBSLAM3();
-
-	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;}
-	virtual bool canProcessAsyncIMU() const;
-
-private:
-	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
-
-	bool init(const rtabmap::CameraModel & model1, const rtabmap::CameraModel & model2, double stamp, bool stereo, double baseline);
-private:
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-	ORB_SLAM3::System * orbslam_;
-	bool firstFrame_;
-	Transform originLocalTransform_;
-	Transform previousPose_;
-	bool useIMU_;
-	Transform imuLocalTransform_;
-	ParametersMap parameters_;
-	std::vector<ORB_SLAM3::IMU::Point> orbslamImus_;
-	double lastImuStamp_;
-	double lastImageStamp_;
-#endif
-
-};
-
-}
-
-#endif /* ODOMETRYORBSLAM_H3_ */
@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace ov_msckf {
 class VioManager;
-struct VioManagerOptions;
 }

 namespace rtabmap {
@@ -41,6 +40,7 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry
 {
 public:
 	OdometryOpenVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryOpenVINS();

 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeOpenVINS;}
@@ -52,12 +52,12 @@ private:

 private:
 #ifdef RTABMAP_OPENVINS
-	std::unique_ptr<ov_msckf::VioManager> vioManager_;
-	std::unique_ptr<ov_msckf::VioManagerOptions> params_;
+	ov_msckf::VioManager * vioManager_;
 	bool initGravity_;
-	Transform previousPoseInv_;
-	Transform imuLocalTransformInv_;
-	Eigen::Matrix<double, 6, 6> Phi_;
+	Transform previousPose_;
+	Transform previousLocalTransform_;
+	Transform imuLocalTransform_;
+	std::map<double, IMU> imuBuffer_;
 #endif
 };

@@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/CameraModel.h>
 #include <vector>

 namespace rtabmap
@@ -157,32 +156,6 @@ cv::Mat RTABMAP_CORE_EXPORT exposureFusion(

 void RTABMAP_CORE_EXPORT HSVtoRGB( float *r, float *g, float *b, float h, float s, float v );

-void RTABMAP_CORE_EXPORT NMS(
-		const std::vector<cv::KeyPoint> & ptsIn,
-		const cv::Mat & descriptorsIn,
-		std::vector<cv::KeyPoint> & ptsOut,
-		cv::Mat & descriptorsOut,
-		int border, int dist_thresh, int img_width, int img_height);
-
-/**
- * @brief Rotate images and camera model so that the top of the image is up.
- *
- * The roll value of local transform of the camera model is used to estimate
- * if the images have to be rotated. If there is a pitch value higher than
- * 45 deg, the original images and camera model will be returned (no rotation will happen).
- * The return local transform of the camera model is updated accordingly. The distortion
- * model is ignored and won't be transfered to modified camera model, so this function
- * expects already rectified images.
- *
- * @param model a valid camera model
- * @param rgb a rgb/grayscale image (set cv::Mat() if not used)
- * @param depth a depth image (set cv::Mat() if not used)
- */
-void RTABMAP_CORE_EXPORT rotateImagesUpsideUpIfNecessary(
-	CameraModel & model,
-	cv::Mat & rgb,
-	cv::Mat & depth);
-
 } // namespace util3d
 } // namespace rtabmap

@@ -144,43 +144,6 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages
 		std::vector<int> * validIndices = 0,
 		const ParametersMap & parameters = ParametersMap());

-/**
- * Create a XYZ cloud from the images contained in SensorData, one for each camera
- *
- * @param sensorData, the sensor data.
- * @param decimation, images are decimated by this factor before projecting points to 3D. The factor
- * should be a factor of the image width and height.
- * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
- * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
- * @param validIndices, the indices of valid points in the cloud
- * @param stereoParameters, stereo optional parameters (in case it is stereo data)
- * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
- * @return XYZ cloud(s), one per camera
- */
-std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromSensorData(
-		const SensorData & sensorData,
-		int decimation = 1,
-		float maxDepth = 0.0f,
-		float minDepth = 0.0f,
-		std::vector<pcl::IndicesPtr> * validIndices = 0,
-		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
-
-/**
- * Create a XYZ cloud from the images contained in SensorData. If there is only one camera,
- * the returned cloud is organized. Otherwise, all NaN
- * points are removed and the cloud will be dense.
- *
- * @param sensorData, the sensor data.
- * @param decimation, images are decimated by this factor before projecting points to 3D. The factor
- * should be a factor of the image width and height.
- * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
- * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
- * @param validIndices, the indices of valid points in the cloud
- * @param stereoParameters, stereo optional parameters (in case it is stereo data)
- * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
- * @return a XYZ cloud.
- */
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
@@ -190,28 +153,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
 		const ParametersMap & stereoParameters = ParametersMap(),
 		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo

-/**
- * Create an RGB cloud from the images contained in SensorData, one for each camera
- *
- * @param sensorData, the sensor data.
- * @param decimation, images are decimated by this factor before projecting points to 3D. The factor
- * should be a factor of the image width and height.
- * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
- * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
- * @param validIndices, the indices of valid points in the cloud
- * @param stereoParameters, stereo optional parameters (in case it is stereo data)
- * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
- * @return RGB cloud(s), one per camera
- */
-std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(
-		const SensorData & sensorData,
-		int decimation = 1,
-		float maxDepth = 0.0f,
-		float minDepth = 0.0f,
-		std::vector<pcl::IndicesPtr > * validIndices = 0,
-		const ParametersMap & stereoParameters = ParametersMap(),
-		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
-
 /**
 * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
 * the returned cloud is organized. Otherwise, all NaN
@@ -223,7 +164,6 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRG
 * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
 * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
 * @param validIndices, the indices of valid points in the cloud
- * @param stereoParameters, stereo optional parameters (in case it is stereo data)
 * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
 * @return a RGB cloud.
 */
@@ -157,7 +157,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
-		float groundNormalsUp = 0);
+		float groundNormalsUp = 0,
+		bool labelUndergroundObstaclesAsGround = false);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -171,7 +172,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
-		float groundNormalsUp = 0);
+		float groundNormalsUp = 0,
+		bool labelUndergroundObstaclesAsGround = false);

 template<typename PointT>
 void occupancy2DFromGroundObstacles(
@@ -48,33 +48,28 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
-			int varianceMedianRatio = 4,
 			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,
-			bool splitLinearCovarianceComponents = false);
+			std::vector<int> * inliersOut = 0);
 			
 Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
 			const std::map<int, cv::Point3f> & words3A,
 			const std::map<int, cv::KeyPoint> & words2B,
 			const std::vector<CameraModel> & cameraModels,
-			unsigned int samplingPolicy = 0, // 0=AUTO, 1=ANY, 2=HOMOGENEOUS
 			int minInliers = 10,
 			int iterations = 100,
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
-			int varianceMedianRatio = 4,
 			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,
-			bool splitLinearCovarianceComponents = false);
+			std::vector<int> * inliersOut = 0);

 Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D(
 			const std::map<int, cv::Point3f> & words3A,
@@ -381,6 +381,17 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormal
 		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));

+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float maxDepthChangeFactor = 0.02f,
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor = 0.02f,
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,
@@ -87,8 +87,7 @@ SET(SRC_FILES
    odometry/OdometryViso2.cpp
    odometry/OdometryDVO.cpp
    odometry/OdometryOkvis.cpp
-    odometry/OdometryORBSLAM2.cpp
-    odometry/OdometryORBSLAM3.cpp
+    odometry/OdometryORBSLAM.cpp
    odometry/OdometryLOAM.cpp
    odometry/OdometryFLOAM.cpp
    odometry/OdometryMSCKF.cpp
@@ -107,11 +106,7 @@ SET(SRC_FILES
    stereo/StereoBM.cpp
    stereo/StereoSGBM.cpp
    
-    GlobalMap.cpp
-    LocalGridMaker.cpp
-    LocalGrid.cpp
-    global_map/OccupancyGrid.cpp
-    global_map/CloudMap.cpp
+    OccupancyGrid.cpp
    
    MarkerDetector.cpp
    
@@ -173,14 +168,14 @@ SET(PUBLIC_LIBRARIES
 	${PCL_LIBRARIES}
 )

-IF(SQLite3_FOUND)
+IF(Sqlite3_FOUND)
    SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
-		${SQLite3_INCLUDE_DIRS}
+		${Sqlite3_INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
 		${LIBRARIES}
-		${SQLite3_LIBRARIES}
+		${Sqlite3_LIBRARIES}
 	)
 ELSE()
 	SET(SRC_FILES
@@ -210,14 +205,10 @@ IF(TORCH_FOUND)
 ENDIF(TORCH_FOUND)

 IF(WITH_PYTHON AND Python3_FOUND)
-	SET(PUBLIC_LIBRARIES
-		${PUBLIC_LIBRARIES}
-		Python3::Python
-		Python3::NumPy
-	)
 	SET(LIBRARIES
 		${LIBRARIES}
-		pybind11::embed
+		Python3::Python
+		Python3::NumPy
 	)
 	SET(SRC_FILES
 		${SRC_FILES}
@@ -594,32 +585,10 @@ IF(octomap_FOUND)
    ENDIF()
 	SET(SRC_FILES
    	${SRC_FILES}
-		global_map/OctoMap.cpp
+		OctoMap.cpp
 	)
 ENDIF(octomap_FOUND)

-IF(grid_map_core_FOUND)
-    IF(TARGET grid_map_core)
-        SET(PUBLIC_LIBRARIES
-            ${PUBLIC_LIBRARIES}
-            grid_map_core
-        )
-    ELSE()
-        SET(PUBLIC_INCLUDE_DIRS
-            ${PUBLIC_INCLUDE_DIRS}
-            ${grid_map_core_INCLUDE_DIRS}
-        )
-        SET(PUBLIC_LIBRARIES
-            ${PUBLIC_LIBRARIES}
-            ${grid_map_core_LIBRARIES}
-        )
-    ENDIF()
-	SET(SRC_FILES
-    	${SRC_FILES}
-		global_map/GridMap.cpp
-	)
-ENDIF(grid_map_core_FOUND)
-
 IF(AliceVision_FOUND)
  SET(LIBRARIES
    ${LIBRARIES}
@@ -781,7 +750,6 @@ foreach(arg ${RESOURCES})
   get_filename_component(filename ${arg} NAME)
   string(REPLACE "." "_" output ${filename})
   set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
-   set_property(SOURCE "${CMAKE_CURRENT_BINARY_DIR}/${output}.h" PROPERTY SKIP_AUTOGEN ON)
 endforeach(arg ${RESOURCES})

 #MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
@@ -36,13 +36,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/StereoDense.h"
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/IMUFilter.h"
-#include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
-#include <opencv2/imgproc/types_c.h>
 #include <opencv2/stitching/detail/exposure_compensate.hpp>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UStl.h>

 #include <pcl/io/io.h>

@@ -60,7 +57,6 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_stereoExposureCompensation(false),
 		_colorOnly(false),
 		_imageDecimation(1),
-		_histogramMethod(0),
 		_stereoToDepth(false),
 		_scanFromDepth(false),
 		_scanDownsampleStep(1),
@@ -76,9 +72,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_bilateralSigmaS(10),
 		_bilateralSigmaR(0.1),
 		_imuFilter(0),
-		_imuBaseFrameConversion(false),
-		_featureDetector(0),
-		_depthAsMask(Parameters::defaultVisDepthAsMask())
+		_imuBaseFrameConversion(false)
 {
 	UASSERT(_camera != 0);
 }
@@ -102,7 +96,6 @@ CameraThread::CameraThread(
 			_stereoExposureCompensation(false),
 			_colorOnly(false),
 			_imageDecimation(1),
-			_histogramMethod(0),
 			_stereoToDepth(false),
 			_scanFromDepth(false),
 			_scanDownsampleStep(1),
@@ -118,9 +111,7 @@ CameraThread::CameraThread(
 			_bilateralSigmaS(10),
 			_bilateralSigmaR(0.1),
 			_imuFilter(0),
-			_imuBaseFrameConversion(false),
-			_featureDetector(0),
-			_depthAsMask(Parameters::defaultVisDepthAsMask())
+			_imuBaseFrameConversion(false)
 {
 	UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
 	UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
@@ -143,7 +134,6 @@ CameraThread::CameraThread(
 			_stereoExposureCompensation(false),
 			_colorOnly(false),
 			_imageDecimation(1),
-			_histogramMethod(0),
 			_stereoToDepth(false),
 			_scanFromDepth(false),
 			_scanDownsampleStep(1),
@@ -159,9 +149,7 @@ CameraThread::CameraThread(
 			_bilateralSigmaS(10),
 			_bilateralSigmaR(0.1),
 			_imuFilter(0),
-			_imuBaseFrameConversion(false),
-			_featureDetector(0),
-			_depthAsMask(Parameters::defaultVisDepthAsMask())
+			_imuBaseFrameConversion(false)
 {
 	UASSERT(_camera != 0);
 	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
@@ -175,7 +163,6 @@ CameraThread::~CameraThread()
 	delete _distortionModel;
 	delete _stereoDense;
 	delete _imuFilter;
-	delete _featureDetector;
 }

 void CameraThread::setImageRate(float imageRate)
@@ -227,30 +214,6 @@ void CameraThread::disableIMUFiltering()
 	_imuFilter = 0;
 }

-void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
-{
-	delete _featureDetector;
-	ParametersMap params = parameters;
-	ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
-	uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
-	uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
-	uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
-	uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
-	uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));
-	uInsert(params, ParametersPair(Parameters::kKpSubPixEps(), uValue(params, Parameters::kVisSubPixEps(), defaultParams.at(Parameters::kVisSubPixEps()))));
-	uInsert(params, ParametersPair(Parameters::kKpSubPixIterations(), uValue(params, Parameters::kVisSubPixIterations(), defaultParams.at(Parameters::kVisSubPixIterations()))));
-	uInsert(params, ParametersPair(Parameters::kKpSubPixWinSize(), uValue(params, Parameters::kVisSubPixWinSize(), defaultParams.at(Parameters::kVisSubPixWinSize()))));
-	uInsert(params, ParametersPair(Parameters::kKpGridRows(), uValue(params, Parameters::kVisGridRows(), defaultParams.at(Parameters::kVisGridRows()))));
-	uInsert(params, ParametersPair(Parameters::kKpGridCols(), uValue(params, Parameters::kVisGridCols(), defaultParams.at(Parameters::kVisGridCols()))));
-	_featureDetector = Feature2D::create(params);
-	_depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask);
-}
-void CameraThread::disableFeatureDetection()
-{
-	delete _featureDetector;
-	_featureDetector = 0;
-}
-
 void CameraThread::setScanParameters(
 	bool fromDepth,
 	int downsampleStep,
@@ -492,21 +455,9 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 			{
 				data.setStereoImage(image, depthOrRight, stereoModels);
 			}
-
-			std::vector<cv::KeyPoint> kpts = data.keypoints();
-			double log2value = log(double(_imageDecimation))/log(2.0);
-			for(unsigned int i=0; i<kpts.size(); ++i)
-			{
-				kpts[i].pt.x /= _imageDecimation;
-				kpts[i].pt.y /= _imageDecimation;
-				kpts[i].size /= _imageDecimation;
-				kpts[i].octave -= log2value;
-			}
-			data.setFeatures(kpts, data.keypoints3D(), data.descriptors());
 		}
 		if(info) info->timeImageDecimation = timer.ticks();
 	}
-
 	if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1)
 	{
 		if(data.cameraModels().size() == 1)
@@ -542,91 +493,6 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 	}

-	if(_histogramMethod && !data.imageRaw().empty())
-	{
-		UDEBUG("");
-		UTimer timer;
-		cv::Mat image;
-		if(_histogramMethod == 1)
-		{
-			if(data.imageRaw().type() == CV_8UC1)
-			{
-				cv::equalizeHist(data.imageRaw(), image);
-			}
-			else if(data.imageRaw().type() == CV_8UC3)
-			{
-				cv::Mat channels[3];
-				cv::cvtColor(data.imageRaw(), image, CV_BGR2YCrCb);
-				cv::split(image, channels);
-				cv::equalizeHist(channels[0], channels[0]);
-				cv::merge(channels, 3, image);
-				cv::cvtColor(image, image, CV_YCrCb2BGR);
-			}
-			if(!data.depthRaw().empty())
-			{
-				data.setRGBDImage(image, data.depthRaw(), data.cameraModels());
-			}
-			else if(!data.rightRaw().empty())
-			{
-				cv::Mat right;
-				if(data.rightRaw().type() == CV_8UC1)
-				{
-					cv::equalizeHist(data.rightRaw(), right);
-				}
-				else if(data.rightRaw().type() == CV_8UC3)
-				{
-					cv::Mat channels[3];
-					cv::cvtColor(data.rightRaw(), right, CV_BGR2YCrCb);
-					cv::split(right, channels);
-					cv::equalizeHist(channels[0], channels[0]);
-					cv::merge(channels, 3, right);
-					cv::cvtColor(right, right, CV_YCrCb2BGR);
-				}
-				data.setStereoImage(image, right, data.stereoCameraModels()[0]);
-			}
-		}
-		else if(_histogramMethod == 2)
-		{
-			cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0);
-			if(data.imageRaw().type() == CV_8UC1)
-			{
-				clahe->apply(data.imageRaw(), image);
-			}
-			else if(data.imageRaw().type() == CV_8UC3)
-			{
-				cv::Mat channels[3];
-				cv::cvtColor(data.imageRaw(), image, CV_BGR2YCrCb);
-				cv::split(image, channels);
-				clahe->apply(channels[0], channels[0]);
-				cv::merge(channels, 3, image);
-				cv::cvtColor(image, image, CV_YCrCb2BGR);
-			}
-			if(!data.depthRaw().empty())
-			{
-				data.setRGBDImage(image, data.depthRaw(), data.cameraModels());
-			}
-			else if(!data.rightRaw().empty())
-			{
-				cv::Mat right;
-				if(data.rightRaw().type() == CV_8UC1)
-				{
-					clahe->apply(data.rightRaw(), right);
-				}
-				else if(data.rightRaw().type() == CV_8UC3)
-				{
-					cv::Mat channels[3];
-					cv::cvtColor(data.rightRaw(), right, CV_BGR2YCrCb);
-					cv::split(right, channels);
-					clahe->apply(channels[0], channels[0]);
-					cv::merge(channels, 3, right);
-					cv::cvtColor(right, right, CV_YCrCb2BGR);
-				}
-				data.setStereoImage(image, right, data.stereoCameraModels()[0]);
-			}
-		}
-		if(info) info->timeHistogramEqualization = timer.ticks();
-	}
-
 	if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
 	{
 		if(data.stereoCameraModels().size()==1)
@@ -807,50 +673,6 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 						data.stamp());
 		}
 	}
-
-	if(_featureDetector && !data.imageRaw().empty())
-	{
-		UDEBUG("Detecting features");
-		cv::Mat grayScaleImg = data.imageRaw();
-		if(data.imageRaw().channels() > 1)
-		{
-			cv::Mat tmp;
-			cv::cvtColor(grayScaleImg, tmp, cv::COLOR_BGR2GRAY);
-			grayScaleImg = tmp;
-		}
-
-		cv::Mat depthMask;
-		if(!data.depthRaw().empty() && _depthAsMask)
-		{
-			if( data.imageRaw().rows % data.depthRaw().rows == 0 &&
-				data.imageRaw().cols % data.depthRaw().cols == 0 &&
-				data.imageRaw().rows/data.depthRaw().rows == data.imageRaw().cols/data.depthRaw().cols)
-			{
-				depthMask = util2d::interpolate(data.depthRaw(), data.imageRaw().rows/data.depthRaw().rows, 0.1f);
-			}
-			else
-			{
-				UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
-						Parameters::kVisDepthAsMask().c_str(),
-						data.imageRaw().rows, data.imageRaw().cols,
-						data.depthRaw().rows, data.depthRaw().cols);
-			}
-		}
-
-		std::vector<cv::KeyPoint> keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask);
-		cv::Mat descriptors;
-		std::vector<cv::Point3f> keypoints3D;
-		if(!keypoints.empty())
-		{
-			descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints);
-			if(!keypoints.empty())
-			{
-				keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints);
-			}
-		}
-
-		data.setFeatures(keypoints, keypoints3D, descriptors);
-	}
 }

 } // namespace rtabmap
@@ -502,16 +502,6 @@ void DBDriver::updateOccupancyGrid(
 	_dbSafeAccessMutex.unlock();
 }

-void DBDriver::updateCalibration(int nodeId, const std::vector<CameraModel> & models, const std::vector<StereoCameraModel> & stereoModels)
-{
-	_dbSafeAccessMutex.lock();
-	this->updateCalibrationQuery(
-			nodeId,
-			models,
-			stereoModels);
-	_dbSafeAccessMutex.unlock();
-}
-
 void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
 {
 	_dbSafeAccessMutex.lock();
@@ -4298,9 +4298,9 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		{
 			_memoryUsedEstimate += (*i)->getMemoryUsed();
 			// raw data are not kept in database
-			_memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
-			_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
-			_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
+			_memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
+			_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
+			_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();

 			stepNode(ppStmt, *i);
 		}
@@ -4615,39 +4615,6 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
 	}
 }

-void DBDriverSqlite3::updateCalibrationQuery(
-	int nodeId,
-	const std::vector<CameraModel> & models,
-	const std::vector<StereoCameraModel> & stereoModels) const
-{
-	UDEBUG("");
-	if(_ppDb)
-	{
-		std::string type;
-		UTimer timer;
-		timer.start();
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-
-		// Create query
-		std::string query = queryStepCalibrationUpdate();
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		// step calibration
-		stepCalibrationUpdate(ppStmt,
-				nodeId,
-				models,
-				stereoModels);
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-		UDEBUG("Time=%fs", timer.ticks());
-	}
-}
-
 void DBDriverSqlite3::updateDepthImageQuery(
 		int nodeId,
 		const cv::Mat & image) const
@@ -5804,131 +5771,6 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 }

-std::string DBDriverSqlite3::queryStepCalibrationUpdate() const
-{
-	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	return "UPDATE Data SET calibration=? WHERE id=?;";
-}
-void DBDriverSqlite3::stepCalibrationUpdate(
-	sqlite3_stmt * ppStmt,
-	int nodeId,
-	const std::vector<CameraModel> & models,
-	const std::vector<StereoCameraModel> & stereoModels) const
-{
-	if(!ppStmt)
-	{
-		UFATAL("");
-	}
-
-	int rc = SQLITE_OK;
-	int index = 1;
-
-	// calibration
-	std::vector<unsigned char> calibrationData;
-	std::vector<float> calibration;
-	// multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
-	// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
-	if(models.size() && models[0].isValidForProjection())
-	{
-		if(uStrNumCmp(_version, "0.18.0") >= 0)
-		{
-			for(unsigned int i=0; i<models.size(); ++i)
-			{
-				UASSERT(models[i].isValidForProjection());
-				std::vector<unsigned char> data = models[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 if(uStrNumCmp(_version, "0.11.2") >= 0)
-		{
-			calibration.resize(models.size() * (6+Transform().size()));
-			for(unsigned int i=0; i<models.size(); ++i)
-			{
-				UASSERT(models[i].isValidForProjection());
-				const Transform & localTransform = models[i].localTransform();
-				calibration[i*(6+localTransform.size())] = models[i].fx();
-				calibration[i*(6+localTransform.size())+1] = models[i].fy();
-				calibration[i*(6+localTransform.size())+2] = models[i].cx();
-				calibration[i*(6+localTransform.size())+3] = models[i].cy();
-				calibration[i*(6+localTransform.size())+4] = models[i].imageWidth();
-				calibration[i*(6+localTransform.size())+5] = models[i].imageHeight();
-				memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
-			}
-		}
-		else
-		{
-			calibration.resize(models.size() * (4+Transform().size()));
-			for(unsigned int i=0; i<models.size(); ++i)
-			{
-				UASSERT(models[i].isValidForProjection());
-				const Transform & localTransform = models[i].localTransform();
-				calibration[i*(4+localTransform.size())] = models[i].fx();
-				calibration[i*(4+localTransform.size())+1] = models[i].fy();
-				calibration[i*(4+localTransform.size())+2] = models[i].cx();
-				calibration[i*(4+localTransform.size())+3] = models[i].cy();
-				memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
-			}
-		}
-	}
-	else if(stereoModels.size() && stereoModels[0].isValidForProjection())
-	{
-		if(uStrNumCmp(_version, "0.18.0") >= 0)
-		{
-			for(unsigned int i=0; i<stereoModels.size(); ++i)
-			{
-				UASSERT(stereoModels[i].isValidForProjection());
-				std::vector<unsigned char> data = stereoModels[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
-		{
-			UASSERT_MSG(stereoModels.size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
-			const Transform & localTransform = stereoModels[0].left().localTransform();
-			calibration.resize(7+localTransform.size());
-			calibration[0] = stereoModels[0].left().fx();
-			calibration[1] = stereoModels[0].left().fy();
-			calibration[2] = stereoModels[0].left().cx();
-			calibration[3] = stereoModels[0].left().cy();
-			calibration[4] = stereoModels[0].baseline();
-			calibration[5] = stereoModels[0].left().imageWidth();
-			calibration[6] = stereoModels[0].left().imageHeight();
-			memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
-		}
-	}
-
-	if(calibrationData.size())
-	{
-		rc = sqlite3_bind_blob(ppStmt, index++, calibrationData.data(), calibrationData.size(), SQLITE_STATIC);
-	}
-	else if(calibration.size())
-	{
-		rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
-	}
-	else
-	{
-		rc = sqlite3_bind_null(ppStmt, index++);
-	}
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-	//id
-	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-	//step
-	rc=sqlite3_step(ppStmt);
-	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-	rc = sqlite3_reset(ppStmt);
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-}
-
 std::string DBDriverSqlite3::queryStepDepthUpdate() const
 {
 	if(uStrNumCmp(_version, "0.10.0") < 0)
@@ -732,22 +732,19 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 		for (int j = 0; j<gridCols_; ++j)
 		{
 			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
-			std::vector<cv::KeyPoint> subKeypoints;
-			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
-			if (this->getType() != Feature2D::Type::kFeaturePyDetector)
-			{
-				limitKeypoints(subKeypoints, maxFeatures);
-			}
+			std::vector<cv::KeyPoint> sub_keypoints;
+			sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
+			limitKeypoints(sub_keypoints, maxFeatures);
 			if(roi.x || roi.y)
 			{
 				// Adjust keypoint position to raw image
-				for(std::vector<cv::KeyPoint>::iterator iter=subKeypoints.begin(); iter!=subKeypoints.end(); ++iter)
+				for(std::vector<cv::KeyPoint>::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter)
 				{
 					iter->pt.x += roi.x;
 					iter->pt.y += roi.y;
 				}
 			}
-			keypoints.insert( keypoints.end(), subKeypoints.begin(), subKeypoints.end() );
+			keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
 		}
 	}
 	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
@@ -2122,24 +2119,6 @@ std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image

 	(*_orb)(imgRoi, maskRoi, keypoints, descriptors_);

-	// OrbOctree ignores the mask, so we have to apply it manually here
-	if(!keypoints.empty() && !maskRoi.empty())
-	{
-		std::vector<cv::KeyPoint> validKeypoints;
-		validKeypoints.reserve(keypoints.size());
-		cv::Mat validDescriptors;
-		for(size_t i=0; i<keypoints.size(); ++i)
-		{
-			if(maskRoi.at<unsigned char>(keypoints[i].pt.y+roi.y, keypoints[i].pt.x+roi.x) != 0)
-			{
-				validKeypoints.push_back(keypoints[i]);
-				validDescriptors.push_back(descriptors_.row(i));
-			}
-		}
-		keypoints = validKeypoints;
-		descriptors_ = validDescriptors;
-	}
-
 	if((int)keypoints.size() > this->getMaxFeatures())
 	{
 		limitKeypoints(keypoints, descriptors_, this->getMaxFeatures());
@@ -1,169 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include <rtabmap/core/GlobalMap.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UStl.h>
-
-namespace rtabmap {
-
-GlobalMap::GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters) :
-	cellSize_(Parameters::defaultGridCellSize()),
-	updateError_(Parameters::defaultGridGlobalUpdateError()),
-	occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()),
-	logOddsHit_(logodds(Parameters::defaultGridGlobalProbHit())),
-	logOddsMiss_(logodds(Parameters::defaultGridGlobalProbMiss())),
-	logOddsClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())),
-	logOddsClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())),
-	cache_(cache)
-{
-	UASSERT(cache_);
-
-	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
-	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
-
-	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_);
-	UASSERT(cellSize_>0.0f);
-
-	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
-
-	UDEBUG("cellSize_           =%f", cellSize_);
-	UDEBUG("updateError_        =%f", updateError_);
-
-	// Probabilistic parameters
-	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_);
-	if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), logOddsHit_))
-	{
-		logOddsHit_ = logodds(logOddsHit_);
-		UASSERT_MSG(logOddsHit_ >= 0.0f, uFormat("probHit_=%f",logOddsHit_).c_str());
-	}
-	if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), logOddsMiss_))
-	{
-		logOddsMiss_ = logodds(logOddsMiss_);
-		UASSERT_MSG(logOddsMiss_ <= 0.0f, uFormat("probMiss_=%f",logOddsMiss_).c_str());
-	}
-	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), logOddsClampingMin_))
-	{
-		logOddsClampingMin_ = logodds(logOddsClampingMin_);
-	}
-	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), logOddsClampingMax_))
-	{
-		logOddsClampingMax_ = logodds(logOddsClampingMax_);
-	}
-	UASSERT(logOddsClampingMax_ > logOddsClampingMin_);
-}
-
-GlobalMap::~GlobalMap()
-{
-	clear();
-}
-
-void GlobalMap::clear()
-{
-	UDEBUG("Clearing");
-	addedNodes_.clear();
-	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
-	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
-}
-
-unsigned long GlobalMap::getMemoryUsed() const
-{
-	unsigned long memoryUsage = 0;
-
-	memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
-
-	return memoryUsage;
-}
-
-bool GlobalMap::update(const std::map<int, Transform> & poses)
-{
-	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
-
-	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
-	bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
-	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
-	float updateErrorSqrd = updateError_*updateError_;
-	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
-	{
-		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
-		if(jter != poses.end())
-		{
-			graphChanged = false;
-			UASSERT(!iter->second.isNull() && !jter->second.isNull());
-			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
-			{
-				graphOptimized = true;
-			}
-		}
-		else
-		{
-			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
-		}
-	}
-
-	if(graphOptimized || graphChanged)
-	{
-		// clear all but keep cache
-		clear();
-	}
-
-	std::list<std::pair<int, Transform> > orderedPoses;
-
-	// add old poses that were not in the current map (they were just retrieved from LTM)
-	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
-	{
-		if(!isNodeAssembled(iter->first))
-		{
-			UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
-			orderedPoses.push_back(*iter);
-		}
-	}
-
-	// insert zero after
-	if(poses.find(0) != poses.end())
-	{
-		orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
-	}
-
-	if(!orderedPoses.empty())
-	{
-		assemble(orderedPoses);
-	}
-
-	return !orderedPoses.empty();
-}
-
-void GlobalMap::addAssembledNode(int id, const Transform & pose)
-{
-	if(id > 0)
-	{
-		uInsert(addedNodes_, std::make_pair(id, pose));
-	}
-}
-
-
-} // namespace rtabmap
@@ -430,7 +430,7 @@ bool importPoses(
 			else if(format == 1 || format==10 || format==11) // rgbd-slam format
 			{
 				std::list<std::string> strList = uSplit(str);
-				if((strList.size() >=  8 && format!=11) || (strList.size() ==  9 && format==11))
+				if((strList.size() ==  8 && format!=11) || (strList.size() ==  9 && format==11))
 				{
 					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
@@ -1054,39 +1054,6 @@ std::multimap<int, Link>::iterator findLink(
 	return links.end();
 }

-std::multimap<int, std::pair<int, Link::Type> >::iterator findLink(
-		std::multimap<int, std::pair<int, Link::Type> > & links,
-		int from,
-		int to,
-		bool checkBothWays,
-		Link::Type type)
-{
-	std::multimap<int, std::pair<int, Link::Type> >::iterator iter = links.find(from);
-	while(iter != links.end() && iter->first == from)
-	{
-		if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
-		{
-			return iter;
-		}
-		++iter;
-	}
-
-	if(checkBothWays)
-	{
-		// let's try to -> from
-		iter = links.find(to);
-		while(iter != links.end() && iter->first == to)
-		{
-			if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
-			{
-				return iter;
-			}
-			++iter;
-		}
-	}
-	return links.end();
-}
-
 std::multimap<int, int>::iterator findLink(
 		std::multimap<int, int> & links,
 		int from,
@@ -1151,39 +1118,6 @@ std::multimap<int, Link>::const_iterator findLink(
 	return links.end();
 }

-std::multimap<int, std::pair<int, Link::Type> >::const_iterator findLink(
-		const std::multimap<int, std::pair<int, Link::Type> > & links,
-		int from,
-		int to,
-		bool checkBothWays,
-		Link::Type type)
-{
-	std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter = links.find(from);
-	while(iter != links.end() && iter->first == from)
-	{
-		if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
-		{
-			return iter;
-		}
-		++iter;
-	}
-
-	if(checkBothWays)
-	{
-		// let's try to -> from
-		iter = links.find(to);
-		while(iter != links.end() && iter->first == to)
-		{
-			if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
-			{
-				return iter;
-			}
-			++iter;
-		}
-	}
-	return links.end();
-}
-
 std::multimap<int, int>::const_iterator findLink(
 		const std::multimap<int, int> & links,
 		int from,
@@ -2316,7 +2250,7 @@ std::map<int, Transform> findNearestPoses(
 	{
 		foundPoses.insert(*poses.find(iter->first));
 	}
-	UDEBUG("found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (int)foundPoses.size(), (int)poses.size(), radius, angle, k);
+	UDEBUG("found nodes=%d", (int)foundPoses.size());
 	return foundPoses;
 }

@@ -163,7 +163,7 @@ cv::Mat Link::uncompressUserDataConst() const

 Link Link::merge(const Link & link, Type outputType) const
 {
-	UASSERT_MSG(to_ == link.from(), uFormat("merging this=%d->%d to link=%d->%d", from_, to_, link.from(), link.to()).c_str());
+	UASSERT(to_ == link.from());
 	UASSERT(outputType != Link::kUndef);
 	UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
 	UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
@@ -1,126 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include <rtabmap/core/GlobalMap.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UStl.h>
-
-namespace rtabmap {
-
-LocalGrid::LocalGrid(const cv::Mat & groundIn,
-			 const cv::Mat & obstaclesIn,
-			 const cv::Mat & emptyIn,
-			 float cellSizeIn,
-			 const cv::Point3f & viewPointIn) :
-		groundCells(groundIn),
-		obstacleCells(obstaclesIn),
-		emptyCells(emptyIn),
-		cellSize(cellSizeIn),
-		viewPoint(viewPointIn)
-{
-	UASSERT(cellSize > 0.0f);
-}
-
-bool LocalGrid::is3D() const
-{
-	return (groundCells.empty() || groundCells.type() == CV_32FC3 || groundCells.type() == CV_32FC(4) || groundCells.type() == CV_32FC(6)) &&
-		   (obstacleCells.empty() || obstacleCells.type() == CV_32FC3 || obstacleCells.type() == CV_32FC(4) || obstacleCells.type() == CV_32FC(6)) &&
-		   (emptyCells.empty() || emptyCells.type() == CV_32FC3 || emptyCells.type() == CV_32FC(4) || emptyCells.type() == CV_32FC(6));
-}
-
-void LocalGridCache::add(int nodeId,
-		const cv::Mat & ground,
-		const cv::Mat & obstacles,
-		const cv::Mat & empty,
-		float cellSize,
-		const cv::Point3f & viewPoint)
-{
-	add(nodeId, LocalGrid(ground, obstacles, empty, cellSize, viewPoint));
-}
-
-void LocalGridCache::add(int nodeId, const LocalGrid & localGrid)
-{
-	UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
-			nodeId, localGrid.groundCells.cols,  localGrid.groundCells.channels(),  localGrid.obstacleCells.cols,  localGrid.obstacleCells.channels(), localGrid.emptyCells.cols,  localGrid.emptyCells.channels());
-	if(nodeId < 0)
-	{
-		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
-		return;
-	}
-	uInsert(localGrids_, std::make_pair(nodeId==0?-1:nodeId, localGrid));
-}
-
-bool LocalGridCache::shareTo(int nodeId, LocalGridCache & anotherCache) const
-{
-	if(uContains(localGrids_, nodeId) && !uContains(anotherCache.localGrids(), nodeId))
-	{
-		const LocalGrid & localGrid = localGrids_.at(nodeId);
-		anotherCache.add(nodeId, localGrid.groundCells, localGrid.obstacleCells, localGrid.emptyCells, localGrid.cellSize, localGrid.viewPoint);
-		return true;
-	}
-	return false;
-}
-
-unsigned long LocalGridCache::getMemoryUsed() const
-{
-	unsigned long memoryUsage = 0;
-	memoryUsage += localGrids_.size()*(sizeof(int) + sizeof(LocalGrid) + sizeof(std::map<int, LocalGrid>::iterator)) + sizeof(std::map<int, LocalGrid>);
-	for(std::map<int, LocalGrid>::const_iterator iter=localGrids_.begin(); iter!=localGrids_.end(); ++iter)
-	{
-		memoryUsage += iter->second.groundCells.total() * iter->second.groundCells.elemSize();
-		memoryUsage += iter->second.obstacleCells.total() * iter->second.obstacleCells.elemSize();
-		memoryUsage += iter->second.emptyCells.total() * iter->second.emptyCells.elemSize();
-		memoryUsage += sizeof(int);
-		memoryUsage += sizeof(cv::Point3f);
-	}
-	return memoryUsage;
-}
-
-void LocalGridCache::clear(bool temporaryOnly)
-{
-	if(temporaryOnly)
-	{
-		//clear only negative ids
-		for(std::map<int, LocalGrid>::iterator iter=localGrids_.begin(); iter!=localGrids_.end();)
-		{
-			if(iter->first < 0)
-			{
-				localGrids_.erase(iter++);
-			}
-			else
-			{
-				break;
-			}
-		}
-	}
-	else
-	{
-		localGrids_.clear();
-	}
-}
-
-} // namespace rtabmap
@@ -1,587 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include <rtabmap/core/LocalGridMaker.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/util3d_filtering.h>
-#include <rtabmap/core/util3d_mapping.h>
-#include <rtabmap/core/util2d.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UTimer.h>
-
-#ifdef RTABMAP_OCTOMAP
-#include <rtabmap/core/global_map/OctoMap.h>
-#endif
-
-#include <pcl/io/pcd_io.h>
-
-namespace rtabmap {
-
-LocalGridMaker::LocalGridMaker(const ParametersMap & parameters) :
-	parameters_(parameters),
-	cloudDecimation_(Parameters::defaultGridDepthDecimation()),
-	rangeMax_(Parameters::defaultGridRangeMax()),
-	rangeMin_(Parameters::defaultGridRangeMin()),
-	//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
-	footprintLength_(Parameters::defaultGridFootprintLength()),
-	footprintWidth_(Parameters::defaultGridFootprintWidth()),
-	footprintHeight_(Parameters::defaultGridFootprintHeight()),
-	scanDecimation_(Parameters::defaultGridScanDecimation()),
-	cellSize_(Parameters::defaultGridCellSize()),
-	preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
-	occupancySensor_(Parameters::defaultGridSensor()),
-	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
-	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
-	normalKSearch_(Parameters::defaultGridNormalK()),
-	groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
-	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
-	clusterRadius_(Parameters::defaultGridClusterRadius()),
-	minClusterSize_(Parameters::defaultGridMinClusterSize()),
-	flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
-	minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
-	maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
-	normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
-	grid3D_(Parameters::defaultGrid3D()),
-	groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
-	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
-	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
-	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
-	rayTracing_(Parameters::defaultGridRayTracing())
-{
-	this->parseParameters(parameters);
-}
-
-LocalGridMaker::~LocalGridMaker()
-{
-}
-
-void LocalGridMaker::parseParameters(const ParametersMap & parameters)
-{
-	uInsert(parameters_, parameters);
-
-	Parameters::parse(parameters, Parameters::kGridSensor(), occupancySensor_);
-	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
-	if(cloudDecimation_ == 0)
-	{
-		cloudDecimation_ = 1;
-	}
-	Parameters::parse(parameters, Parameters::kGridRangeMin(), rangeMin_);
-	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
-	Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
-	Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
-	Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
-	Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
-	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_);
-	UASSERT(cellSize_>0.0f);
-
-	Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_);
-	Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
-	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
-	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
-	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
-	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
-	Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_);
-	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
-	{
-		maxGroundAngle_ *= M_PI/180.0f;
-	}
-	Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_);
-	UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str());
-	Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
-	Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
-	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
-	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
-	Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
-	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
-	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
-	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
-	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
-
-	// convert ROI from string to vector
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
-	{
-		std::list<std::string> strValues = uSplit(iter->second, ' ');
-		if(strValues.size() != 4)
-		{
-			ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
-		}
-		else
-		{
-			std::vector<float> tmpValues(4);
-			unsigned int i=0;
-			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
-			{
-				tmpValues[i] = uStr2Float(*jter);
-				++i;
-			}
-
-			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
-				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
-				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
-				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
-			{
-				roiRatios_ = tmpValues;
-			}
-			else
-			{
-				ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
-			}
-		}
-	}
-
-	if(maxGroundHeight_ == 0.0f && !normalsSegmentation_)
-	{
-		UWARN("\"%s\" should be not equal to 0 if not using normals "
-				"segmentation approach. Setting it to cell size (%f).",
-				Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
-		maxGroundHeight_ = cellSize_;
-	}
-	if(maxGroundHeight_ != 0.0f &&
-		maxObstacleHeight_ != 0.0f &&
-		maxObstacleHeight_ < maxGroundHeight_)
-	{
-		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
-				Parameters::kGridMaxGroundHeight().c_str(),
-				Parameters::kGridMaxObstacleHeight().c_str(),
-				Parameters::kGridMaxObstacleHeight().c_str());
-		maxObstacleHeight_ = 0;
-	}
-	if(maxGroundHeight_ != 0.0f &&
-		minGroundHeight_ != 0.0f &&
-		maxGroundHeight_ < minGroundHeight_)
-	{
-		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
-				Parameters::kGridMinGroundHeight().c_str(),
-				Parameters::kGridMaxGroundHeight().c_str(),
-				Parameters::kGridMinGroundHeight().c_str());
-		minGroundHeight_ = 0;
-	}
-}
-
-void LocalGridMaker::createLocalMap(
-		const Signature & node,
-		cv::Mat & groundCells,
-		cv::Mat & obstacleCells,
-		cv::Mat & emptyCells,
-		cv::Point3f & viewPoint)
-{
-	UDEBUG("scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d",
-			node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancySensor_, normalsSegmentation_?1:0, grid3D_?1:0);
-
-	if((node.sensorData().laserScanRaw().is2d()) && occupancySensor_ == 0)
-	{
-		UDEBUG("2D laser scan");
-		//2D
-		viewPoint = cv::Point3f(
-				node.sensorData().laserScanRaw().localTransform().x(),
-				node.sensorData().laserScanRaw().localTransform().y(),
-				node.sensorData().laserScanRaw().localTransform().z());
-
-		LaserScan scan = node.sensorData().laserScanRaw();
-		if(rangeMin_ > 0.0f)
-		{
-			scan = util3d::rangeFiltering(scan, rangeMin_, 0.0f);
-		}
-
-		float maxRange = rangeMax_;
-		if(rangeMax_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f)
-		{
-			maxRange = rangeMax_ < node.sensorData().laserScanRaw().rangeMax()?rangeMax_:node.sensorData().laserScanRaw().rangeMax();
-		}
-		else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f)
-		{
-			maxRange = node.sensorData().laserScanRaw().rangeMax();
-		}
-		util3d::occupancy2DFromLaserScan(
-				util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
-				cv::Mat(),
-				viewPoint,
-				emptyCells,
-				obstacleCells,
-				cellSize_,
-				scan2dUnknownSpaceFilled_,
-				maxRange);
-
-		UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
-	}
-	else
-	{
-		// 3D
-		if(occupancySensor_ == 0 || occupancySensor_ == 2)
-		{
-			if(!node.sensorData().laserScanRaw().isEmpty())
-			{
-				UDEBUG("3D laser scan");
-				const Transform & t = node.sensorData().laserScanRaw().localTransform();
-				LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
-#ifdef RTABMAP_OCTOMAP
-				// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
-				float maxRange = rayTracing_?0.0f:rangeMax_;
-#else
-				// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
-				float maxRange = !grid3D_ && rayTracing_?0.0f:rangeMax_;
-#endif
-				if(rangeMin_ > 0.0f || maxRange > 0.0f)
-				{
-					scan = util3d::rangeFiltering(scan, rangeMin_, maxRange);
-				}
-
-				// update viewpoint
-				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
-
-				UDEBUG("scan format=%d", scan.format());
-
-				bool normalSegmentationTmp = normalsSegmentation_;
-				float minGroundHeightTmp = minGroundHeight_;
-				float maxGroundHeightTmp = maxGroundHeight_;
-				if(scan.is2d())
-				{
-					// if 2D, assume the whole scan is obstacle
-					normalsSegmentation_ = false;
-					minGroundHeight_ = std::numeric_limits<int>::min();
-					maxGroundHeight_ = std::numeric_limits<int>::min()+100;
-				}
-
-				createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
-
-				if(scan.is2d())
-				{
-					// restore
-					normalsSegmentation_ = normalSegmentationTmp;
-					minGroundHeight_ = minGroundHeightTmp;
-					maxGroundHeight_ = maxGroundHeightTmp;
-				}
-			}
-			else
-			{
-				UWARN("Cannot create local map from scan: scan is empty (node=%d, %s=%d).", node.id(), Parameters::kGridSensor().c_str(), occupancySensor_);
-			}
-		}
-
-		if(occupancySensor_ >= 1)
-		{
-			pcl::IndicesPtr indices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-			UDEBUG("Depth image : decimation=%d max=%f min=%f",
-					cloudDecimation_,
-					rangeMax_,
-					rangeMin_);
-			cloud = util3d::cloudRGBFromSensorData(
-					node.sensorData(),
-					cloudDecimation_,
-#ifdef RTABMAP_OCTOMAP
-					// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
-					rayTracing_?0.0f:rangeMax_,
-#else
-					// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
-					!grid3D_&&rayTracing_?0.0f:rangeMax_,
-#endif
-					rangeMin_,
-					indices.get(),
-					parameters_,
-					roiRatios_);
-
-			// update viewpoint
-			viewPoint = cv::Point3f(0,0,0);
-			if(node.sensorData().cameraModels().size())
-			{
-				// average of all local transforms
-				float sum = 0;
-				for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
-				{
-					const Transform & t = node.sensorData().cameraModels()[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;
-				}
-			}
-			else
-			{
-				// 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;
-			cv::Mat scanObstacleCells;
-			cv::Mat scanEmptyCells;
-			if(occupancySensor_ == 2)
-			{
-				// backup
-				scanGroundCells = groundCells;
-				scanObstacleCells = obstacleCells;
-				scanEmptyCells = emptyCells;
-				groundCells = cv::Mat();
-				obstacleCells = cv::Mat();
-				emptyCells = cv::Mat();
-			}
-
-			createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
-
-			if(occupancySensor_ == 2)
-			{
-				if(grid3D_)
-				{
-					// We should convert scans to 4 channels (XYZRGB) to be compatible
-					scanGroundCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanGroundCells), Transform::getIdentity(), 255, 255, 255)).data();
-					scanObstacleCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanObstacleCells), Transform::getIdentity(), 255, 255, 255)).data();
-					scanEmptyCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanEmptyCells), Transform::getIdentity(), 255, 255, 255)).data();
-				}
-
-				UDEBUG("groundCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", groundCells.cols, groundCells.channels(), scanGroundCells.cols, scanGroundCells.channels());
-				UDEBUG("obstacleCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", obstacleCells.cols, obstacleCells.channels(), scanObstacleCells.cols, scanObstacleCells.channels());
-				UDEBUG("emptyCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", emptyCells.cols, emptyCells.channels(), scanEmptyCells.cols, scanEmptyCells.channels());
-
-				if(!groundCells.empty() && !scanGroundCells.empty())
-					cv::hconcat(groundCells, scanGroundCells, groundCells);
-				else if(!scanGroundCells.empty())
-					groundCells = scanGroundCells;
-
-				if(!obstacleCells.empty() && !scanObstacleCells.empty())
-					cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells);
-				else if(!scanObstacleCells.empty())
-					obstacleCells = scanObstacleCells;
-
-				if(!emptyCells.empty() && !scanEmptyCells.empty())
-					cv::hconcat(emptyCells, scanEmptyCells, emptyCells);
-				else if(!scanEmptyCells.empty())
-					emptyCells = scanEmptyCells;
-			}
-		}
-	}
-}
-
-void LocalGridMaker::createLocalMap(
-		const LaserScan & scan,
-		const Transform & pose,
-		cv::Mat & groundCells,
-		cv::Mat & obstacleCells,
-		cv::Mat & emptyCells,
-		cv::Point3f & viewPointInOut) const
-{
-	if(projMapFrame_)
-	{
-		//we should rotate viewPoint in /map frame
-		float roll, pitch, yaw;
-		pose.getEulerAngles(roll, pitch, yaw);
-		Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
-		viewPointInOut.x = viewpointRotated.x();
-		viewPointInOut.y = viewpointRotated.y();
-		viewPointInOut.z = viewpointRotated.z();
-	}
-
-	if(scan.size())
-	{
-		pcl::IndicesPtr groundIndices(new std::vector<int>);
-		pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
-		cv::Mat groundCloud;
-		cv::Mat obstaclesCloud;
-
-		if(scan.hasRGB() && scan.hasNormals())
-		{
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGBNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
-			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
-			if(grid3D_)
-			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
-			}
-			else
-			{
-				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
-			}
-		}
-		else if(scan.hasRGB())
-		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGB>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
-			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
-			if(grid3D_)
-			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
-			}
-			else
-			{
-				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
-			}
-		}
-		else if(scan.hasNormals())
-		{
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
-			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
-			if(grid3D_)
-			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
-			}
-			else
-			{
-				util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
-			}
-		}
-		else
-		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZ>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
-			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
-			if(grid3D_)
-			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
-			}
-			else
-			{
-				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
-			}
-		}
-
-		if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
-		{
-			UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
-			if(groundIsObstacle_ && !groundCloud.empty())
-			{
-				if(obstaclesCloud.empty())
-				{
-					obstaclesCloud = groundCloud;
-					groundCloud = cv::Mat();
-				}
-				else
-				{
-					UASSERT(obstaclesCloud.type() == groundCloud.type());
-					cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type());
-					obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols)));
-					groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols)));
-				}
-			}
-
-			// transform back in base frame
-			float roll, pitch, yaw;
-			pose.getEulerAngles(roll, pitch, yaw);
-			Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
-
-			if(rayTracing_)
-			{
-#ifdef RTABMAP_OCTOMAP
-				if(!groundCloud.empty() || !obstaclesCloud.empty())
-				{
-					//create local octomap
-					ParametersMap params;
-					params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize_)));
-					params.insert(ParametersPair(Parameters::kGridRangeMax(), uNumber2Str(rangeMax_)));
-					params.insert(ParametersPair(Parameters::kGridRayTracing(), uNumber2Str(rayTracing_)));
-					LocalGridCache cache;
-					OctoMap octomap(&cache, params);
-					cache.add(1, groundCloud, obstaclesCloud, cv::Mat(), cellSize_, cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
-					std::map<int, Transform> poses;
-					poses.insert(std::make_pair(1, Transform::getIdentity()));
-					octomap.update(poses);
-
-					pcl::IndicesPtr groundIndices(new std::vector<int>);
-					pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
-					pcl::IndicesPtr emptyIndices(new std::vector<int>);
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
-					UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
-					if(scan.hasRGB())
-					{
-						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv).data();
-						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv).data();
-						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv).data();
-					}
-					else
-					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
-						pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
-						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv).data();
-						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv).data();
-						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv).data();
-					}
-				}
-			}
-			else
-#else
-					UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
-				}
-#endif
-			{
-				groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
-				obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
-			}
-
-		}
-		else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
-		{
-			cv::Mat laserScan = obstacleCells;
-			cv::Mat laserScanNoHit = groundCells;
-			obstacleCells = cv::Mat();
-			groundCells = cv::Mat();
-			util3d::occupancy2DFromLaserScan(
-					laserScan,
-					laserScanNoHit,
-					viewPointInOut,
-					emptyCells,
-					obstacleCells,
-					cellSize_,
-					false, // don't fill unknown space
-					rangeMax_);
-		}
-	}
-	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
-}
-
-} // namespace rtabmap
@@ -60,9 +60,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/optimizer/OptimizerG2O.h"
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
+#include <rtabmap/core/OccupancyGrid.h>
 #include <rtabmap/core/MarkerDetector.h>
 #include <opencv2/imgproc/types_c.h>
-#include <rtabmap/core/LocalGridMaker.h>

 namespace rtabmap {

@@ -107,7 +107,6 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
 	_useOdometryGravity(Parameters::defaultMemUseOdomGravity()),
-	_rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()),
 	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
@@ -154,7 +153,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	}
 	_registrationIcpMulti = new RegistrationIcp(paramsMulti);

-	_localMapMaker = new LocalGridMaker(parameters);
+	_occupancy = new OccupancyGrid(parameters);
 	_markerDetector = new MarkerDetector(parameters);
 	this->parseParameters(parameters);
 }
@@ -546,7 +545,7 @@ Memory::~Memory()
 	delete _registrationPipeline;
 	delete _registrationIcpMulti;
 	delete _registrationVis;
-	delete _localMapMaker;
+	delete _occupancy;
 }

 void Memory::parseParameters(const ParametersMap & parameters)
@@ -598,7 +597,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
 	Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
 	Parameters::parse(params, Parameters::kMemUseOdomGravity(), _useOdometryGravity);
-	Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp);
 	Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
 	Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
@@ -751,9 +749,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		}
 	}

-	if(_localMapMaker)
+	if(_occupancy)
 	{
-		_localMapMaker->parseParameters(params);
+		_occupancy->parseParameters(params);
 	}

 	if(_markerDetector)
@@ -3372,7 +3370,7 @@ bool Memory::addLink(const Link & link, bool addInDatabase)
 {
 	UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);

-	ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance(false));
+	ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
 	Signature * toS = _getSignature(link.to());
 	Signature * fromS = _getSignature(link.from());
 	if(toS && fromS)
@@ -3708,7 +3706,7 @@ unsigned long Memory::getMemoryUsed() const
 	memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
 	memoryUsage += sizeof(Registration);
 	memoryUsage += sizeof(RegistrationIcp);
-	memoryUsage += sizeof(LocalGridMaker);
+	memoryUsage += _occupancy->getMemoryUsed();
 	memoryUsage += sizeof(MarkerDetector);
 	memoryUsage += sizeof(DBDriver);

@@ -4669,96 +4667,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		preUpdateThread.start();
 	}

-	if(_rotateImagesUpsideUp && !data.imageRaw().empty() && !data.cameraModels().empty())
-	{
-		// Currently stereo is not supported
-		UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
-		int subInputImageWidth = data.imageRaw().cols/data.cameraModels().size();
-		int subInputDepthWidth = data.depthRaw().cols/data.cameraModels().size();
-		int subOutputImageWidth = 0;
-		int subOutputDepthWidth = 0;
-		cv::Mat rotatedColorImages;
-		cv::Mat rotatedDepthImages;
-		std::vector<CameraModel> rotatedCameraModels;
-		bool allOutputSizesAreOkay = true;
-		for(size_t i=0; i<data.cameraModels().size(); ++i)
-		{
-			UDEBUG("Rotating camera %ld", i);
-			cv::Mat rgb = cv::Mat(data.imageRaw(), cv::Rect(subInputImageWidth*i, 0, subInputImageWidth, data.imageRaw().rows));
-			cv::Mat depth = !data.depthRaw().empty()?cv::Mat(data.depthRaw(), cv::Rect(subInputDepthWidth*i, 0, subInputDepthWidth, data.depthRaw().rows)):cv::Mat();
-			CameraModel model = data.cameraModels()[i];
-			util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
-			if(rotatedColorImages.empty())
-			{
-				rotatedColorImages = cv::Mat(cv::Size(rgb.cols * data.cameraModels().size(), rgb.rows), rgb.type());
-				subOutputImageWidth = rgb.cols;;
-				if(!depth.empty())
-				{
-					rotatedDepthImages = cv::Mat(cv::Size(depth.cols * data.cameraModels().size(), depth.rows), depth.type());
-					subOutputDepthWidth = depth.cols;
-				}
-			}
-			else if(rgb.cols != subOutputImageWidth || depth.cols != subOutputDepthWidth ||
-					rgb.rows != rotatedColorImages.rows || depth.rows != rotatedDepthImages.rows)
-			{
-				UWARN("Rotated image for camera index %d (rgb=%dx%d depth=%dx%d) doesn't tally "
-				      "with the first camera (rgb=%dx%d, depth=%dx%d). Aborting upside up rotation, "
-					  "will use original image orientation. Set parameter %s to false to avoid "
-					  "this warning.",
-						i,
-						rgb.cols, rgb.rows,
-						depth.cols, depth.rows,
-						subOutputImageWidth, rotatedColorImages.rows,
-						subOutputDepthWidth, rotatedDepthImages.rows,
-						Parameters::kMemRotateImagesUpsideUp().c_str());
-				allOutputSizesAreOkay = false;
-				break;
-			}
-			rgb.copyTo(cv::Mat(rotatedColorImages, cv::Rect(subOutputImageWidth*i, 0, subOutputImageWidth, rgb.rows)));
-			if(!depth.empty())
-			{
-				depth.copyTo(cv::Mat(rotatedDepthImages, cv::Rect(subOutputDepthWidth*i, 0, subOutputDepthWidth, depth.rows)));
-			}
-			rotatedCameraModels.push_back(model);
-		}
-		if(allOutputSizesAreOkay)
-		{
-			data.setRGBDImage(rotatedColorImages, rotatedDepthImages, rotatedCameraModels);
-
-			// Clear any features to avoid confusion with the rotated cameras.
-			if(!data.keypoints().empty() || !data.keypoints3D().empty() || !data.descriptors().empty())
-			{
-				if(_useOdometryFeatures)
-				{
-					static bool warned = false;
-					if(!warned)
-					{
-						UWARN("Because parameter %s is enabled, parameter %s is inhibited as "
-							"features have to be regenerated. To avoid this warning, set "
-							"explicitly %s to false. This message is only "
-							"printed once.",
-							Parameters::kMemRotateImagesUpsideUp().c_str(),
-							Parameters::kMemUseOdomFeatures().c_str(),
-							Parameters::kMemUseOdomFeatures().c_str());
-						warned = true;
-					}
-				}
-				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-			}
-		}
-	}
-	else if(_rotateImagesUpsideUp)
-	{
-		static bool warned = false;
-		if(!warned)
-		{
-			UWARN("Parameter %s can only be used with RGB-only or RGB-D cameras. "
-			      "Ignoring upside up rotation. This message is only printed once.",
-				  Parameters::kMemRotateImagesUpsideUp().c_str());
-			warned = true;
-		}
-	}
-
 	unsigned int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
 	SensorData decimatedData;
@@ -5062,10 +4970,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
 				}
-				if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f))
-				{
-					_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
-				}
 			}
 		}
 		else if(data.imageRaw().empty())
@@ -5934,14 +5838,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// Occupancy grid map stuff
 	if(_createOccupancyGrid && !isIntermediateNode)
 	{
-		if( (_localMapMaker->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
-			(!_localMapMaker->isGridFromDepth() && !data.laserScanRaw().empty()))
+		if( (_occupancy->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
+			(!_occupancy->isGridFromDepth() && !data.laserScanRaw().empty()))
 		{
 			cv::Mat ground, obstacles, empty;
 			float cellSize = 0.0f;
 			cv::Point3f viewPoint(0,0,0);
-			_localMapMaker->createLocalMap(*s, ground, obstacles, empty, viewPoint);
-			cellSize = _localMapMaker->getCellSize();
+			_occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
+			cellSize = _occupancy->getCellSize();
 			s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);

 			t = timer.ticks();
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#include <rtabmap/core/global_map/OctoMap.h>
+#include <rtabmap/core/OctoMap.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -288,40 +288,55 @@ RtabmapColorOcTree::StaticMemberInitializer RtabmapColorOcTree::RtabmapColorOcTr
 // OctoMap
 //////////////////////////////////////

-OctoMap::OctoMap(const LocalGridCache * cache, const ParametersMap & parameters) :
-		GlobalMap(cache, parameters),
+OctoMap::OctoMap(const ParametersMap & parameters) :
 		hasColor_(false),
+		fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
+		updateError_(Parameters::defaultGridGlobalUpdateError()),
 		rangeMax_(Parameters::defaultGridRangeMax()),
 		rayTracing_(Parameters::defaultGridRayTracing()),
 		emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth())
 {
-	octree_ = new RtabmapColorOcTree(cellSize_);
-	if(occupancyThr_ <= 0.0f)
+	float cellSize = Parameters::defaultGridCellSize();
+	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
+	UASSERT(cellSize>0.0f);
+
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
+	float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	float probHit = Parameters::defaultGridGlobalProbHit();
+	float probMiss = Parameters::defaultGridGlobalProbMiss();
+	float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
+	float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);
+
+	octree_ = new RtabmapColorOcTree(cellSize);
+	if(occupancyThr <= 0.0f)
 	{
 		UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
 				Parameters::kGridGlobalOccupancyThr().c_str(),
 				Parameters::defaultGridGlobalOccupancyThr());
-		occupancyThr_ = Parameters::defaultGridGlobalOccupancyThr();
+		occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
 	}
+	octree_->setOccupancyThres(occupancyThr);
+	octree_->setProbHit(probHit);
+	octree_->setProbMiss(probMiss);
+	octree_->setClampingThresMin(clampingMin);
+	octree_->setClampingThresMax(clampingMax);
+	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
   
-	UDEBUG("occupancyThr_=%f", occupancyThr_);
-	UDEBUG("probHit_=%f", probability(logOddsHit_));
-	UDEBUG("probMiss_=%f", probability(logOddsMiss_));
-	UDEBUG("probClampingMin_=%f", probability(logOddsClampingMin_));
-	UDEBUG("probClampingMax_=%f", probability(logOddsClampingMax_));
-	
-	octree_->setOccupancyThres(occupancyThr_);
-	octree_->setProbHit(probability(logOddsHit_));
-	octree_->setProbMiss(probability(logOddsMiss_));
-	octree_->setClampingThresMin(probability(logOddsClampingMin_));
-	octree_->setClampingThresMax(probability(logOddsClampingMax_));
-
+	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
 	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
-
 	Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_);
 	UASSERT(emptyFloodFillDepth_>=0 && emptyFloodFillDepth_<=16);

+	UDEBUG("fullUpdate_         =%s", fullUpdate_?"true":"false");
+	UDEBUG("updateError_        =%f", updateError_);
 	UDEBUG("rangeMax_           =%f", rangeMax_);
 	UDEBUG("rayTracing_         =%s", rayTracing_?"true":"false");
 	UDEBUG("emptyFloodFillDepth_=%d", emptyFloodFillDepth_);
@@ -336,17 +351,47 @@ OctoMap::~OctoMap()
 void OctoMap::clear()
 {
 	octree_->clear();
+	cache_.clear();
+	cacheClouds_.clear();
+	cacheViewPoints_.clear();
+	addedNodes_.clear();
 	hasColor_ = false;
-	GlobalMap::clear();
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
 }

-unsigned long OctoMap::getMemoryUsed() const
+void OctoMap::addToCache(int nodeId,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+		const pcl::PointXYZ & viewPoint)
 {
-	unsigned long memoryUsage = GlobalMap::getMemoryUsed();
-
-	// Note: size of OctoMap object is missing.
-
-	return memoryUsage;
+	UDEBUG("nodeId=%d", nodeId);
+	if(nodeId < 0)
+	{
+		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
+		return;
+	}
+	cacheClouds_.erase(nodeId==0?-1:nodeId);
+	cacheClouds_.insert(std::make_pair(nodeId==0?-1:nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
+}
+void OctoMap::addToCache(int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		const cv::Mat & empty,
+		const cv::Point3f & viewPoint)
+{
+	UDEBUG("nodeId=%d", nodeId);
+	if(nodeId < 0)
+	{
+		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
+		return;
+	}
+	UASSERT_MSG(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", ground.type()).c_str());
+	UASSERT_MSG(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", obstacles.type()).c_str());
+	UASSERT_MSG(empty.empty() || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", empty.type()).c_str());
+	uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
+	uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint));
 }

 bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
@@ -464,67 +509,227 @@ std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> OctoMap::fin
 }


-void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
+bool OctoMap::update(const std::map<int, Transform> & poses)
 {
+	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
+
+	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
+	bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
+	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
+	std::map<int, Transform> transforms;
+	std::map<int, Transform> updatedAddedNodes;
+	float updateErrorSqrd = updateError_*updateError_;
+	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
+		if(jter != poses.end())
+		{
+			graphChanged = false;
+			UASSERT(!iter->second.isNull() && !jter->second.isNull());
+			Transform t = Transform::getIdentity();
+			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
+			{
+				t = jter->second * iter->second.inverse();
+				graphOptimized = true;
+			}
+			transforms.insert(std::make_pair(jter->first, t));
+			updatedAddedNodes.insert(std::make_pair(jter->first, jter->second));
+		}
+		else
+		{
+			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
+		}
+	}
+
+	if(graphOptimized || graphChanged)
+	{
+		if(graphChanged)
+		{
+			UWARN("Graph has changed! The whole map should be rebuilt.");
+		}
+		else
+		{
+			UINFO("Graph optimized!");
+		}
+
+		minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+		maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
+		if(fullUpdate_ || graphChanged)
+		{
+			// clear all but keep cache
+			octree_->clear();
+			addedNodes_.clear();
+			hasColor_ = false;
+		}
+		else
+		{
+			RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution());
+			int copied=0;
+			int count=0;
+			UTimer t;
+			for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count)
+			{
+				RtabmapColorOcTreeNode & nOld = *it;
+				if(nOld.getNodeRefId() > 0)
+				{
+					std::map<int, Transform>::iterator jter = transforms.find(nOld.getNodeRefId());
+					if(jter != transforms.end())
+					{
+						octomap::point3d pt;
+						std::map<int, Transform>::iterator pter = addedNodes_.find(nOld.getNodeRefId());
+						UASSERT(pter != addedNodes_.end());
+
+						if(nOld.getOccupancyType() > 0)
+						{
+							pt = nOld.getPointRef();
+						}
+						else
+						{
+							pt = octree_->keyToCoord(it.getKey());
+						}
+
+						cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
+						cvPt = util3d::transformPoint(cvPt, jter->second);
+						octomap::point3d ptTransformed(cvPt.x, cvPt.y, cvPt.z);
+
+						octomap::OcTreeKey key;
+						if(newOcTree->coordToKeyChecked(ptTransformed, key))
+						{
+							RtabmapColorOcTreeNode * n = newOcTree->search(key);
+							if(n)
+							{
+								if(n->getNodeRefId() > nOld.getNodeRefId())
+								{
+									// The cell has been updated from more recent node, don't update the cell
+									continue;
+								}
+								else if(nOld.getOccupancyType() <= 0 && n->getOccupancyType() > 0)
+								{
+									// empty cells cannot overwrite ground/obstacle cells
+									continue;
+								}
+							}
+
+							RtabmapColorOcTreeNode * nNew = newOcTree->updateNode(key, nOld.getLogOdds());
+							if(nNew)
+							{
+								++copied;
+								updateMinMax(ptTransformed);
+								nNew->setNodeRefId(nOld.getNodeRefId());
+								if(nOld.getOccupancyType() > 0)
+								{
+									nNew->setPointRef(pt);
+								}
+								nNew->setOccupancyType(nOld.getOccupancyType());
+								nNew->setColor(nOld.getColor());
+							}
+							else
+							{
+								UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+							}
+						}
+						else
+						{
+							UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+						}
+					}
+					else if(jter == transforms.end())
+					{
+						// Note: normal if old nodes were transfered to LTM
+						//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
+					}
+				}
+			}
+			UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks());
+			delete octree_;
+			octree_ = newOcTree;
+
+			//update added poses
+			addedNodes_ = updatedAddedNodes;
+		}
+	}
+
 	// Original version from A. Hornung:
 	// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
 	//
+	std::list<std::pair<int, Transform> > orderedPoses;

-	int lastId = assembledNodes().size()?assembledNodes().rbegin()->first:0;
+	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
 	UDEBUG("Last id = %d", lastId);

-	UDEBUG("newPoses = %d", (int)newPoses.size());
+	// add old poses that were not in the current map (they were just retrieved from LTM)
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
+	{
+		if(addedNodes_.find(iter->first) == addedNodes_.end())
+		{
+			orderedPoses.push_back(*iter);
+		}
+	}

-	if(!newPoses.empty())
+	// insert zero after
+	if(poses.find(0) != poses.end())
+	{
+		orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
+	}
+
+	UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
+
+	if(!orderedPoses.empty())
 	{
 		float rangeMaxSqrd = rangeMax_*rangeMax_;
 		float cellSize = octree_->getResolution();
-		for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
+		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
 		{
-			std::map<int, LocalGrid>::const_iterator localGridIter;
-			localGridIter = cache().find(iter->first);
-			if(localGridIter != cache().end())
+			std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
+			std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator occupancyIter;
+			std::map<int, cv::Point3f>::iterator viewPointIter;
+			cloudIter = cacheClouds_.find(iter->first);
+			occupancyIter = cache_.find(iter->first);
+			viewPointIter = cacheViewPoints_.find(iter->first);
+			if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
 			{
-				cv::Mat ground = localGridIter->second.groundCells;
-				cv::Mat obstacles = localGridIter->second.obstacleCells;
-				cv::Mat emptyCells = localGridIter->second.emptyCells;
-
-				if(!localGridIter->second.is3D())
-				{
-					UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! (ground type=%d, obstacles type=%d, empty type=%d)",
-											ground.type(), obstacles.type(), emptyCells.type());
-					continue;
-				}
-
 				UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());

+				UASSERT(viewPointIter != cacheViewPoints_.end());
 				octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
-				sensorOrigin += octomap::point3d(localGridIter->second.viewPoint.x, localGridIter->second.viewPoint.y, localGridIter->second.viewPoint.z);
+				sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);

 				updateMinMax(sensorOrigin);

 				octomap::OcTreeKey tmpKey;
-				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey))
+				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
+						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
 				{
 					UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
 				}

-				bool computeRays = rayTracing_ && emptyCells.empty();
+				bool computeRays = rayTracing_ && (occupancyIter == cache_.end() || occupancyIter->second.second.empty());

 				// instead of direct scan insertion, compute update to filter ground:
 				octomap::KeySet free_cells;
 				// insert ground points only as free:
-				unsigned int maxGroundPts = ground.cols;
+				unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
 				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
 				Eigen::Affine3f t = iter->second.toEigen3f();
-				LaserScan tmpGround = LaserScan::backwardCompatibility(ground);
-				UASSERT(tmpGround.size() == (int)maxGroundPts);
+				LaserScan tmpGround;
+				if(occupancyIter != cache_.end())
+				{
+					tmpGround = LaserScan::backwardCompatibility(occupancyIter->second.first.first);
+					UASSERT(tmpGround.size() == (int)maxGroundPts);
+				}
 				for (unsigned int i=0; i<maxGroundPts; ++i)
 				{
 					pcl::PointXYZRGB pt;
-					pt = util3d::laserScanToPointRGB(tmpGround, i);
-					pt = pcl::transformPoint(pt, t);
-
+					if(occupancyIter != cache_.end())
+					{
+						pt = util3d::laserScanToPointRGB(tmpGround, i);
+						pt = pcl::transformPoint(pt, t);
+					}
+					else
+					{
+						pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
+					}
 					octomap::point3d point(pt.x, pt.y, pt.z);
 					bool ignoreOccupiedCell = false;
 					if(rangeMaxSqrd > 0.0f)
@@ -588,15 +793,26 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 				UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());

 				// all other points: free on ray, occupied on endpoint:
-				unsigned int maxObstaclePts = obstacles.cols;
+				unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
 				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
-				LaserScan tmpObstacle = LaserScan::backwardCompatibility(obstacles);
-				UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
+				LaserScan tmpObstacle;
+				if(occupancyIter != cache_.end())
+				{
+					tmpObstacle = LaserScan::backwardCompatibility(occupancyIter->second.first.second);
+					UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
+				}
 				for (unsigned int i=0; i<maxObstaclePts; ++i)
 				{
 					pcl::PointXYZRGB pt;
-					pt = util3d::laserScanToPointRGB(tmpObstacle, i);
-					pt = pcl::transformPoint(pt, t);
+					if(occupancyIter != cache_.end())
+					{
+						pt = util3d::laserScanToPointRGB(tmpObstacle, i);
+						pt = pcl::transformPoint(pt, t);
+					}
+					else
+					{
+						pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
+					}

 					octomap::point3d point(pt.x, pt.y, pt.z);

@@ -687,11 +903,11 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 				}

 				// all empty cells
-				if(emptyCells.cols)
+				if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
 				{
-					unsigned int maxEmptyPts = emptyCells.cols;
+					unsigned int maxEmptyPts = occupancyIter->second.second.cols;
 					UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
-					LaserScan tmpEmpty = LaserScan::backwardCompatibility(emptyCells);
+					LaserScan tmpEmpty = LaserScan::backwardCompatibility(occupancyIter->second.second);
 					UASSERT(tmpEmpty.size() == (int)maxEmptyPts);
 					for (unsigned int i=0; i<maxEmptyPts; ++i)
 					{
@@ -743,18 +959,22 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 					}
 				}

-				if(emptyCells.cols || !free_cells.empty())
+				if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty())
 				{
 					octree_->updateInnerOccupancy();
 				}

 				// compress map
-				//if(newPoses.size() > 1)
+				//if(orderedPoses.size() > 1)
 				//{
 				//	octree_->prune();
 				//}

-				addAssembledNode(iter->first, iter->second);
+				// ignore negative ids as they are temporary clouds
+				if(iter->first > 0)
+				{
+					addedNodes_.insert(*iter);
+				}
 				UDEBUG("%d: end", iter->first);
 			}
 			else
@@ -785,12 +1005,21 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
            }
        }

+
        for(unsigned int y=0; y < nodeToDelete.size(); y++)
        {
            octree_->deleteNode(nodeToDelete[y],emptyFloodFillDepth_);
        }
        UDEBUG("Flood Fill: deleted %d empty cells (%fs)", (int)nodeToDelete.size(), t.ticks());
    }
+
+	if(!fullUpdate_)
+	{
+		cache_.clear();
+		cacheClouds_.clear();
+		cacheViewPoints_.clear();
+	}
+	return !orderedPoses.empty() || graphOptimized || graphChanged || emptyFloodFillDepth_>0;
 }

 void OctoMap::updateMinMax(const octomap::point3d & point)
@@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryViso2.h"
 #include "rtabmap/core/odometry/OdometryDVO.h"
 #include "rtabmap/core/odometry/OdometryOkvis.h"
-#include "rtabmap/core/odometry/OdometryORBSLAM3.h"
+#include "rtabmap/core/odometry/OdometryORBSLAM.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
@@ -51,7 +51,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"

 #include <pcl/pcl_base.h>
-#include <rtabmap/core/odometry/OdometryORBSLAM2.h>

 namespace rtabmap {

@@ -85,11 +84,7 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 		odometry = new OdometryDVO(parameters);
 		break;
 	case Odometry::kTypeORBSLAM:
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
-		odometry = new OdometryORBSLAM2(parameters);
-#else
-		odometry = new OdometryORBSLAM3(parameters);
-#endif
+		odometry = new OdometryORBSLAM(parameters);
 		break;
 	case Odometry::kTypeOkvis:
 		odometry = new OdometryOkvis(parameters);
@@ -335,15 +330,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}

-	if(!data.imageRaw().empty())
-	{
-		UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",
-			data.imageRaw().cols,
-			data.imageRaw().rows,
-			data.cameraModels().size(),
-			data.stereoCameraModels().size());
-	}
-

 	if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
 	{
@@ -689,12 +675,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty());
 			for(unsigned int i=0; i<info->newCorners.size(); ++i)
 			{
-				info->newCorners[i].x *= _imageDecimation;
-				info->newCorners[i].y *= _imageDecimation;
+				info->refCorners[i].x *= _imageDecimation;
+				info->refCorners[i].y *= _imageDecimation;
 				if(!info->refCorners.empty())
 				{
-					info->refCorners[i].x *= _imageDecimation;
-					info->refCorners[i].y *= _imageDecimation;
+					info->newCorners[i].x *= _imageDecimation;
+					info->newCorners[i].y *= _imageDecimation;
 				}
 			}
 			for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
@@ -915,7 +901,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		{
 			UWARN("Odometry automatically reset to latest pose!");
 			this->reset(_pose);
-			_resetCurrentCount = _resetCountdown;
 			if(info)
 			{
 				*info = OdometryInfo();
@@ -190,7 +190,8 @@ void Optimizer::getConnectedGraph(
 		const std::map<int, Transform> & posesIn,
 		const std::multimap<int, Link> & linksIn,
 		std::map<int, Transform> & posesOut,
-		std::multimap<int, Link> & linksOut) const
+		std::multimap<int, Link> & linksOut,
+		bool adjustPosesWithConstraints) const
 {
 	UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
 	UASSERT(fromId>0);
@@ -201,15 +202,15 @@ void Optimizer::getConnectedGraph(

 	std::set<int> nextPoses;
 	nextPoses.insert(fromId);
-	std::multimap<int, std::pair<int, Link::Type> > biLinks;
+	std::multimap<int, int> biLinks;
 	for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
 	{
 		if(iter->second.from() != iter->second.to())
 		{
-			if(graph::findLink(biLinks, iter->second.from(), iter->second.to(), true, iter->second.type()) == biLinks.end())
+			if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end())
 			{
-				biLinks.insert(std::make_pair(iter->second.from(), std::make_pair(iter->second.to(), iter->second.type())));
-				biLinks.insert(std::make_pair(iter->second.to(), std::make_pair(iter->second.from(), iter->second.type())));
+				biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
+				biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
 			}
 		}
 	}
@@ -233,35 +234,41 @@ void Optimizer::getConnectedGraph(
 			}
 		}

-		for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
+		for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
-			int toId = iter->second.first;
-			Link::Type type = iter->second.second;
+			int toId = iter->second;
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
-				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
+				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
 				if(nextPoses.find(toId) == nextPoses.end())
 				{
 					if(!uContains(posesOut, toId))
 					{
-						const Transform & poseToIn = posesIn.at(toId);
-						Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
-						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
+						if(adjustPosesWithConstraints)
 						{
-							if(poseToIn.is3DoF())
+							if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
 							{
+								Transform t;
+								if(kter->second.from()==currentId)
+								{
+									t = kter->second.transform();
+								}
+								else
+								{
+									t = kter->second.transform().inverse();
+								}
 								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
 							}
 							else
 							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
+								Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
+								posesOut.insert(std::make_pair(toId, t));
 							}
 						}
 						else
 						{
-							posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
+							posesOut.insert(*posesIn.find(toId));
 						}
-
 						// add prior links
 						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
 						{
@@ -275,7 +282,7 @@ void Optimizer::getConnectedGraph(
 					}

 					// only add unique links
-					if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+					if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
 					{
 						if(kter->second.to() < 0)
 						{
@@ -214,14 +214,14 @@ ParametersMap Parameters::getDefaultParameters(const std::string & groupIn)
 	return parameters;
 }

-ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & groupIn, bool remove)
+ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & group, bool remove)
 {
 	ParametersMap output;
 	for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
 		UASSERT(uSplit(iter->first, '/').size()  == 2);
 		std::string group = uSplit(iter->first, '/').front();
-		bool sameGroup = group.compare(groupIn) == 0;
+		bool sameGroup = group.compare(group) == 0;
 		if((!remove && sameGroup) || (remove && !sameGroup))
 		{
 			output.insert(*iter);
@@ -236,9 +236,6 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

-		// 0.21.3
-		removedParameters_.insert(std::make_pair("GridGlobal/FullUpdate",    std::make_pair(false, "")));
-
 		// 0.20.15
 		removedParameters_.insert(std::make_pair("Grid/FromDepth",           std::make_pair(true, Parameters::kGridSensor())));

@@ -304,7 +301,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("Rtabmap/VhStrategy",            std::make_pair(true,  Parameters::kVhEpEnabled())));

 		// 0.12.5
-		removedParameters_.insert(std::make_pair("Grid/FullUpdate",               std::make_pair(false,  "")));
+		removedParameters_.insert(std::make_pair("Grid/FullUpdate",               std::make_pair(true,  Parameters::kGridGlobalFullUpdate())));

 		// 0.12.1
 		removedParameters_.insert(std::make_pair("Grid/3DGroundIsObstacle",       std::make_pair(true,  Parameters::kGridGroundIsObstacle())));
@@ -815,23 +812,11 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
-				str = "With Open3D:";
-#ifdef RTABMAP_OPEN3D
-				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 OctoMap:";
+				str = "With octomap:";
 #ifdef RTABMAP_OCTOMAP
 				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 GridMap:";
-#ifdef RTABMAP_GRIDMAP
-				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 cpu-tsdf:";
 #ifdef RTABMAP_CPUTSDF
@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RegistrationVis.h>
 #include <rtabmap/core/util3d_motion_estimation.h>
 #include <rtabmap/core/util3d_features.h>
-#include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/VWDictionary.h>
 #include <rtabmap/core/util2d.h>
@@ -70,10 +69,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
-		_PnPVarMedianRatio(Parameters::defaultVisPnPVarianceMedianRatio()),
 		_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
-		_PnPSplitLinearCovarianceComponents(Parameters::defaultVisPnPSplitLinearCovComponents()),
-		_multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()),
 		_correspondencesApproach(Parameters::defaultVisCorType()),
 		_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -129,10 +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::kVisPnPVarianceMedianRatio(), _PnPVarMedianRatio);
 	Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
-	Parameters::parse(parameters, Parameters::kVisPnPSplitLinearCovComponents(), _PnPSplitLinearCovarianceComponents);
-	Parameters::parse(parameters, Parameters::kVisPnPSamplingPolicy(), _multiSamplingPolicy);
 	Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
 	Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -297,7 +290,6 @@ Transform RegistrationVis::computeTransformationImpl(
 	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=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
 	UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -492,7 +484,6 @@ Transform RegistrationVis::computeTransformationImpl(

 			if(!imageFrom.empty() && !imageTo.empty())
 			{
-				UASSERT(!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty());
 				std::vector<cv::Point2f> cornersFrom;
 				cv::KeyPoint::convert(kptsFrom, cornersFrom);
 				std::vector<cv::Point2f> cornersTo;
@@ -515,48 +506,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					}
 					else
 					{
-						UTimer t;
-						int nCameras = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels().size():toSignature.sensorData().stereoCameraModels().size();
-						cornersTo = cornersFrom;
-						// compute inverse transforms one time
-						std::vector<Transform> inverseTransforms(nCameras);
-						for(int c=0; c<nCameras; ++c)
-						{
-							Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[c].localTransform():toSignature.sensorData().stereoCameraModels()[c].left().localTransform();
-							inverseTransforms[c] = (guess * localTransform).inverse();
-							UDEBUG("inverse transforms: cam %d -> %s", c, inverseTransforms[c].prettyPrint().c_str());
-						}
-						// Project 3D points in each camera
-						int inFrame = 0;
-						UASSERT(kptsFrom3D.size() == cornersTo.size());
-						int subImageWidth = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].imageWidth():toSignature.sensorData().stereoCameraModels()[0].left().imageWidth();
-						UASSERT(subImageWidth>0);
-						for(size_t i=0; i<kptsFrom3D.size(); ++i)
-						{
-							// Start from camera having the reference corner first (in case there is overlap between the cameras)
-							int startIndex = cornersFrom[i].x/subImageWidth;
-							UASSERT(startIndex < nCameras);
-							for(int c=startIndex; (c+1)%nCameras != 0; ++c)
-							{
-								const CameraModel & model = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[c]:toSignature.sensorData().stereoCameraModels()[c].left();
-								cv::Point3f ptsInCamFrame = util3d::transformPoint(kptsFrom3D[i], inverseTransforms[c]);
-								if(ptsInCamFrame.z > 0)
-								{
-									float u,v;
-									model.reproject(ptsInCamFrame.x, ptsInCamFrame.y, ptsInCamFrame.z, u, v);
-									if(model.inFrame(u,v))
-									{
-										cornersTo[i].x = u+model.imageWidth()*c;
-										cornersTo[i].y = v;
-										++inFrame;
-										break;
-									}
-								}
-							}
-						}
-				
-						UDEBUG("Projected %d/%ld points inside %d cameras (time=%fs)", 
-							inFrame, cornersTo.size(), nCameras, t.ticks());
+						UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored...");
 					}
 				}

@@ -1098,7 +1048,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							std::vector<std::vector<float> > dists;
 							float radius = (float)_guessWinSize; // pixels
 							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
-							index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams(32, 0, false));
+							index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());

 							UASSERT(indices.size() == cornersProjectedMat.rows);
 							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
@@ -1628,20 +1578,17 @@ Transform RegistrationVis::computeTransformationImpl(
 									words3A,
 									wordsB,
 									models,
-									_multiSamplingPolicy,
 									_minInliers,
 									_iterations,
 									_PnPReprojError,
 									_PnPFlags,
 									_PnPRefineIterations,
-									_PnPVarMedianRatio,
 									_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,
-									_PnPSplitLinearCovarianceComponents);
+									&inliersV);
 							inliers[dir] = inliersV;
 							matches[dir] = matchesV;
 						}
@@ -1658,14 +1605,12 @@ Transform RegistrationVis::computeTransformationImpl(
 									_PnPReprojError,
 									_PnPFlags,
 									_PnPRefineIterations,
-									_PnPVarMedianRatio,
 									_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,
-									_PnPSplitLinearCovarianceComponents);
+									&inliersV);
 							inliers[dir] = inliersV;
 							matches[dir] = matchesV;
 						}
@@ -148,7 +148,6 @@ Rtabmap::Rtabmap() :
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
 	_localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()),
-	_localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())),
 	_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
 	_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
 	_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
@@ -621,10 +620,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
 	Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
 	Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing);
-	double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError();
-	Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
-	UASSERT(localizationPriorError>0.0);
-	_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
 	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);

 	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
@@ -856,7 +851,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
 		if(!_memory->isIncremental())
 		{
 			_lastLocalizationPose = initialPose;
-			_localizationCovariance = cv::Mat();
+			_localizationCovariance = 0;
 			_lastLocalizationNodeId = 0;
 			_odomCachePoses.clear();
 			_odomCacheConstraints.clear();
@@ -1442,10 +1437,8 @@ bool Rtabmap::process(
 	float angleToClosestNodeInTheGraph = 0;
 	if(_rgbdSlamMode)
 	{
-		double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
-		double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
-		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar);
-		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar);
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(5,5));

 		//Verify if there was a rehearsal
 		int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -1674,10 +1667,7 @@ bool Rtabmap::process(

 			Link tmp = signature->getLinks().begin()->second.inverse();

-			if(!smallDisplacement)
-			{
-				_distanceTravelled += tmp.transform().getNorm();
-			}
+			_distanceTravelled += tmp.transform().getNorm();

 			// if the previous node is an intermediate node, remove it from the local graph
 			if(_constraints.size() &&
@@ -1702,11 +1692,11 @@ bool Rtabmap::process(
 			odomCovariance.type() == CV_64FC1 &&
 			odomCovariance.at<double>(0,0) < 1)
 		{
-			if( _memory->isIncremental() && _localizationCovariance.empty())
+			if(_localizationCovariance.empty() || _lastLocalizationPose.isNull())
 			{
-				_localizationCovariance = cv::Mat::zeros(6,6,CV_64FC1);
+				_localizationCovariance = odomCovariance.clone();
 			}
-			if(_localizationCovariance.total() == 36)
+			else
 			{
 #ifdef RTABMAP_MRPT
 				// Transform odometry covariance (which in base frame) into global frame
@@ -1724,6 +1714,7 @@ bool Rtabmap::process(
 				// build rtabmap with MRPT to use approach above.
 				_localizationCovariance += odomCovariance;
 #endif
+
 			}
 		}
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
@@ -1733,10 +1724,7 @@ bool Rtabmap::process(
 			if(!_odomCachePoses.empty())
 			{
 				float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm();
-				if(!smallDisplacement)
-				{
-					_distanceTravelled += odomDistance;
-				}
+				_distanceTravelled += odomDistance;

 				while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize)
 				{
@@ -1868,46 +1856,9 @@ bool Rtabmap::process(
 	//============================================================
 	// Bayes filter update
 	//============================================================
-	bool localizationOnPreviousUpdate = false;
-	if(_memory->isIncremental())
-	{
-		localizationOnPreviousUpdate =
-			signature->getLinks().size() &&
-			signature->getLinks().begin()->first!=signature->id() &&
-			_memory->getLoopClosureLinks(signature->getLinks().begin()->first, false).size() != 0;
-	}
-	else
-	{
-		// localization mode
-
-		// Count how many localization links are in the constraints
-		int localizationLinks = 0;
-		int previousIdWithLocalizationLink = 0;
-		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin();
-				iter!=_odomCacheConstraints.end(); ++iter)
-		{
-			if(previousIdWithLocalizationLink == iter->first)
-			{
-				// ignore links with node already counted
-				continue;
-			}
-			if(iter->second.type() == Link::kGlobalClosure ||
-			   iter->second.type() == Link::kLocalSpaceClosure ||
-			   iter->second.type() == Link::kLocalTimeClosure ||
-			   iter->second.type() == Link::kUserClosure ||
-			   iter->second.type() == Link::kNeighborMerged ||
-			   iter->second.type() == Link::kLandmark)
-			{
-				++localizationLinks;
-				previousIdWithLocalizationLink = iter->first;
-			}
-		}
-
-		localizationOnPreviousUpdate = localizationLinks > 1; // need two links in case we have delayed localization
-	}
-
+	int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
 	// Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure, not too fast movement
-	if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement)
+	if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
 	{
 		// If the working memory is empty, don't do the detection. It happens when it
 		// is the first time the detector is started (there needs some images to
@@ -2569,8 +2520,7 @@ bool Rtabmap::process(
 	{
 		if(_startNewMapOnLoopClosure &&
 			_memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place)
-			_localizationCovariance.empty() && // if we didn't localize yet
-			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session
+			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session)
 		{
 			UINFO("Proximity detection by space disabled as if we force to have a global loop "
 					"closure with previous map before doing proximity detections (%s=true).",
@@ -2587,7 +2537,7 @@ bool Rtabmap::process(

 			// don't do it if it is a small displacement unless the previous signature didn't have a loop closure
 			// don't do it if there is a too fast movement
-			if((!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement)
+			if((!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
 			{

 				//============================================================
@@ -2746,15 +2696,6 @@ bool Rtabmap::process(
 								}
 							}
 						}
-						else if(!signature->hasLink(nearestId) && proximityFilteringRadius>0.0f)
-						{
-							UDEBUG("Skipping path %d as most likely ID %d is too far %f > %f (%s)",
-								iter->first.id,
-								nearestId,
-								_optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)),
-								proximityFilteringRadius,
-								Parameters::kRGBDProximityPathFilteringRadius().c_str());
-						}
 					}
 				}

@@ -2998,10 +2939,9 @@ bool Rtabmap::process(
 			{
 				// 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);
-				
-				loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
-				loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
 				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)
 				{
@@ -3027,7 +2967,7 @@ bool Rtabmap::process(
 	// Landmark
 	//============================================================
 	std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
-	if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored())
+	if(!signature->getLandmarks().empty())
 	{
 		bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
 		UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
@@ -3182,7 +3122,6 @@ bool Rtabmap::process(
 				{
 					constraints.insert(std::make_pair(iter->second.from(), iter->second));
 				}
-				cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
 					std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -3190,11 +3129,16 @@ bool Rtabmap::process(
 					{
 						poses.insert(*iterPose);
 						// make the poses in the map fixed
-						constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
-						UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
+						constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*1000000)));
+						UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str());
 					}
-					UDEBUG("Constraint %d->%d: %s (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
+					UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
 				}
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str());
+				}
+

 				std::map<int, Transform> posesOut;
 				std::multimap<int, Link> edgeConstraintsOut;
@@ -3202,25 +3146,9 @@ bool Rtabmap::process(
 				UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
 				_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
 				// If slam2d: get connected graph while keeping original roll,pitch,z values.
-				_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
-				if(ULogger::level() == ULogger::kDebug)
-				{
-					for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
-					{
-						UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str());
-					}
-				}
+				_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
 				cv::Mat locOptCovariance;
-				std::map<int, Transform> optPoses;
-				if(!posesOut.empty() &&
-				   posesOut.begin()->first < _odomCachePoses.begin()->first)
-				{
-					optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
-				}
-				else
-				{
-					UERROR("Invalid localization constraints");
-				}
+				std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
 				_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
 				for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
 				{
@@ -3232,7 +3160,7 @@ bool Rtabmap::process(
 					UWARN("Optimization failed, rejecting localization!");
 					rejectLocalization = true;
 				}
-				else
+				else if(_optimizationMaxError > 0.0f)
 				{
 					UINFO("Compute max graph errors...");
 					const Link * maxLinearLink = 0;
@@ -3262,7 +3190,7 @@ bool Rtabmap::process(
 								maxLinearLink->transVariance(),
 								maxLinearError/sqrt(maxLinearLink->transVariance()),
 								_optimizationMaxError);
-						if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
+						if(maxLinearErrorRatio > _optimizationMaxError)
 						{
 							UWARN("Rejecting localization (%d <-> %d) in this "
 									"iteration because a wrong loop closure has been "
@@ -3281,19 +3209,6 @@ bool Rtabmap::process(
 									_optimizationMaxError);
 							rejectLocalization = true;
 						}
-						else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust())
-						{
-							UERROR("Huge optimization error detected!"
-									"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-									"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-									maxLinearErrorRatio,
-									maxLinearLink->from(),
-									maxLinearLink->to(),
-									maxLinearLink->type(),
-									maxLinearError,
-									sqrt(maxLinearLink->transVariance()),
-									Parameters::kRGBDOptimizeMaxError().c_str());
-						}
 					}
 					if(maxAngularLink)
 					{
@@ -3304,7 +3219,7 @@ bool Rtabmap::process(
 								maxAngularLink->rotVariance(),
 								maxAngularError/sqrt(maxAngularLink->rotVariance()),
 								_optimizationMaxError);
-						if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
+						if(maxAngularErrorRatio > _optimizationMaxError)
 						{
 							UWARN("Rejecting localization (%d <-> %d) in this "
 									"iteration because a wrong loop closure has been "
@@ -3323,19 +3238,6 @@ bool Rtabmap::process(
 									_optimizationMaxError);
 							rejectLocalization = true;
 						}
-						else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust())
-						{
-							UERROR("Huge optimization error detected!"
-									"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-									"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-									maxAngularErrorRatio,
-									maxAngularLink->from(),
-									maxAngularLink->to(),
-									maxAngularLink->type(),
-									maxAngularError*180.0f/CV_PI,
-									sqrt(maxAngularLink->rotVariance()),
-									Parameters::kRGBDOptimizeMaxError().c_str());
-						}
 					}
 				}

@@ -3359,17 +3261,8 @@ bool Rtabmap::process(
 						UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
 						_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
 						// If slam2d: get connected graph while keeping original roll,pitch,z values.
-						_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
-						optPoses.clear();
-						if(!posesOut.empty() &&
-						   posesOut.begin()->first < _odomCachePoses.begin()->first)
-						{
-							optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
-						}
-						else
-						{
-							UERROR("Invalid localization constraints");
-						}
+						_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
+						optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
 						_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
 						for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
 						{
@@ -3381,7 +3274,7 @@ bool Rtabmap::process(
 							UWARN("Optimization failed, rejecting localization!");
 							rejectLocalization = true;
 						}
-						else
+						else if(_optimizationMaxError > 0.0f)
 						{
 							UINFO("Compute max graph errors...");
 							const Link * maxLinearLink = 0;
@@ -3411,7 +3304,7 @@ bool Rtabmap::process(
 										maxLinearLink->transVariance(),
 										maxLinearError/sqrt(maxLinearLink->transVariance()),
 										_optimizationMaxError);
-								if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
+								if(maxLinearErrorRatio > _optimizationMaxError)
 								{
 									UWARN("Rejecting localization (%d <-> %d) in this "
 											"iteration because a wrong loop closure has been "
@@ -3430,19 +3323,6 @@ bool Rtabmap::process(
 											_optimizationMaxError);
 									rejectLocalization = true;
 								}
-								else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust())
-								{
-									UERROR("Huge optimization error detected!"
-											"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-											"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-											maxLinearErrorRatio,
-											maxLinearLink->from(),
-											maxLinearLink->to(),
-											maxLinearLink->type(),
-											maxLinearError,
-											sqrt(maxLinearLink->transVariance()),
-											Parameters::kRGBDOptimizeMaxError().c_str());
-								}
 							}
 							if(maxAngularLink)
 							{
@@ -3453,7 +3333,7 @@ bool Rtabmap::process(
 										maxAngularLink->rotVariance(),
 										maxAngularError/sqrt(maxAngularLink->rotVariance()),
 										_optimizationMaxError);
-								if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
+								if(maxAngularErrorRatio > _optimizationMaxError)
 								{
 									UWARN("Rejecting localization (%d <-> %d) in this "
 											"iteration because a wrong loop closure has been "
@@ -3472,19 +3352,6 @@ bool Rtabmap::process(
 											_optimizationMaxError);
 									rejectLocalization = true;
 								}
-								else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust())
-								{
-									UERROR("Huge optimization error detected!"
-											"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-											"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-											maxAngularErrorRatio,
-											maxAngularLink->from(),
-											maxAngularLink->to(),
-											maxAngularLink->type(),
-											maxAngularError*180.0f/CV_PI,
-											sqrt(maxAngularLink->rotVariance()),
-											Parameters::kRGBDOptimizeMaxError().c_str());
-								}
 							}
 						}
 					}
@@ -3527,7 +3394,6 @@ bool Rtabmap::process(
 					}

 					// update localization links
-					UASSERT(uContains(optPoses, signature->id()));
 					Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
 					for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
 					{
@@ -3540,7 +3406,6 @@ bool Rtabmap::process(
 						else
 						{
 							// Adjust with optimized poses, this will smooth the localization
-							UASSERT(uContains(optPoses, iter->first));
 							Transform newT = newOptPoseInv * optPoses.at(iter->first);
 							UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
 							UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());
@@ -3723,6 +3588,7 @@ bool Rtabmap::process(
 				rejectedLandmark = true;
 			}
 			else if(_memory->isIncremental() &&
+			  _optimizationMaxError > 0.0f &&
 			  loopClosureLinksAdded.size() &&
 			  optimizationIterations > 0 &&
 			  constraints.size())
@@ -3748,7 +3614,7 @@ bool Rtabmap::process(
 				if(maxLinearLink)
 				{
 					UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-					if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
+					if(maxLinearErrorRatio > _optimizationMaxError)
 					{
 						UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
 							  "iteration because a wrong loop closure has been "
@@ -3768,24 +3634,11 @@ bool Rtabmap::process(
 							  _optimizationMaxError);
 						reject = true;
 					}
-					else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust())
-					{
-						UERROR("Huge optimization error detected!"
-								"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-								"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-								maxLinearErrorRatio,
-								maxLinearLink->from(),
-								maxLinearLink->to(),
-								maxLinearLink->type(),
-								maxLinearError,
-								sqrt(maxLinearLink->transVariance()),
-								Parameters::kRGBDOptimizeMaxError().c_str());
-					}
 				}
 				if(maxAngularLink)
 				{
 					UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
-					if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
+					if(maxAngularErrorRatio > _optimizationMaxError)
 					{
 						UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
 							  "iteration because a wrong loop closure has been "
@@ -3805,19 +3658,6 @@ bool Rtabmap::process(
 							  _optimizationMaxError);
 						reject = true;
 					}
-					else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust())
-					{
-						UERROR("Huge optimization error detected!"
-								"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-								"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-								maxAngularErrorRatio,
-								maxAngularLink->from(),
-								maxAngularLink->to(),
-								maxAngularLink->type(),
-								maxAngularError*180.0f/CV_PI,
-								sqrt(maxAngularLink->rotVariance()),
-								Parameters::kRGBDOptimizeMaxError().c_str());
-					}
 				}

 				if(reject)
@@ -4149,16 +3989,15 @@ bool Rtabmap::process(
 		ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
 	}

-	Signature lastSignatureData = *signature;
+	Signature lastSignatureData(signature->id());
 	Transform lastSignatureLocalizedPose;
 	if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
 	{
 		lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
 	}
-	if(!_publishLastSignatureData)
+	if(_publishLastSignatureData)
 	{
-		lastSignatureData.sensorData().clearCompressedData();
-		lastSignatureData.sensorData().clearRawData();
+		lastSignatureData = *signature;
 	}
 	if(!_rawDataKept)
 	{
@@ -4441,73 +4280,96 @@ bool Rtabmap::process(
 			poses = _optimizedPoses;
 			constraints = _constraints;
 		}
-		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);
+		UDEBUG("");
+		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_.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())
+			if(_nodesToRepublish.size())
 			{
-				missingIds.insert(std::make_pair(-1, tmpId));
-			}
+				std::multimap<int, int> missingIds;

-			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)
+				// priority to loopId
+				int tmpId = loopId>0?loopId:_highestHypothesis.first;
+				if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end())
 				{
-					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));
-						}
-					}
+					missingIds.insert(std::make_pair(-1, tmpId));
+				}

-					if(_nodesToRepublish.size() != missingIds.size())
+				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)
 					{
-						// remove requested nodes not anymore in the graph
-						for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
+						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(ids.find(*iter) == ids.end())
+							if(iter->first != loopId &&
+									_nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
 							{
-								iter = _nodesToRepublish.erase(iter);
+								missingIds.insert(std::make_pair(iter->second, iter->first));
 							}
-							else
+						}
+
+						if(_nodesToRepublish.size() != missingIds.size())
+						{
+							// remove requested nodes not anymore in the graph
+							for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
 							{
-								++iter;
+								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);
+				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
+		{
+			// only copy node info
+			Signature nodeInfo(
+					lastSignatureData.id(),
+					lastSignatureData.mapId(),
+					lastSignatureData.getWeight(),
+					lastSignatureData.getStamp(),
+					lastSignatureData.getLabel(),
+					lastSignatureData.getPose(),
+					lastSignatureData.getGroundTruthPose());
+			const std::vector<float> & v = lastSignatureData.getVelocity();
+			if(v.size() == 6)
+			{
+				nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]);
+			}
+			nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
+			nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
+			statistics_.addSignatureData(nodeInfo);
+		}
 		UDEBUG("");
 		localGraphSize = (int)poses.size();
 		if(!lastSignatureLocalizedPose.isNull())
@@ -5651,130 +5513,106 @@ int Rtabmap::detectMoreLoopClosures(
 							if(!t.isNull())
 							{
 								bool updateConstraints = true;
+								if(_optimizationMaxError > 0.0f)
+								{
+									//optimize the graph to see if the new constraint is globally valid

-								//optimize the graph to see if the new constraint is globally valid
-
-								int fromId = from;
-								int mapId = signatures.at(from).mapId();
-								// use first node of the map containing from
-								for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
-								{
-									if(ster->second.mapId() == mapId)
+									int fromId = from;
+									int mapId = signatures.at(from).mapId();
+									// use first node of the map containing from
+									for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
 									{
-										fromId = ster->first;
-										break;
-									}
-								}
-								std::multimap<int, Link> linksIn = links;
-								linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
-								const Link * maxLinearLink = 0;
-								const Link * maxAngularLink = 0;
-								float maxLinearError = 0.0f;
-								float maxAngularError = 0.0f;
-								float maxLinearErrorRatio = 0.0f;
-								float maxAngularErrorRatio = 0.0f;
-								std::map<int, Transform> optimizedPoses;
-								std::multimap<int, Link> links;
-								UASSERT(poses.find(fromId) != poses.end());
-								UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
-								UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-								_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
-								UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
-								UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT(graph::findLink(links, from, to) != links.end());
-								optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
-								std::string msg;
-								if(optimizedPoses.size())
-								{
-									graph::computeMaxGraphErrors(
-											optimizedPoses,
-											links,
-											maxLinearErrorRatio,
-											maxAngularErrorRatio,
-											maxLinearError,
-											maxAngularError,
-											&maxLinearLink,
-											&maxAngularLink);
-									if(maxLinearLink)
-									{
-										UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-										if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
+										if(ster->second.mapId() == mapId)
 										{
-											msg = uFormat("Rejecting edge %d->%d because "
-														"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
-														"\"%s\" is %f.",
-														from,
-														to,
-														maxLinearError,
-														maxLinearLink->from(),
-														maxLinearLink->to(),
-														maxLinearErrorRatio,
-														sqrt(maxLinearLink->transVariance()),
-														Parameters::kRGBDOptimizeMaxError().c_str(),
-														_optimizationMaxError);
-										}
-										else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust())
-										{
-											UERROR("Huge optimization error detected!"
-													"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-													"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-													maxLinearErrorRatio,
-													maxLinearLink->from(),
-													maxLinearLink->to(),
-													maxLinearLink->type(),
-													maxLinearError,
-													sqrt(maxLinearLink->transVariance()),
-													Parameters::kRGBDOptimizeMaxError().c_str());
+											fromId = ster->first;
+											break;
 										}
 									}
-									else if(maxAngularLink)
+									std::multimap<int, Link> linksIn = links;
+									linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
+									const Link * maxLinearLink = 0;
+									const Link * maxAngularLink = 0;
+									float maxLinearError = 0.0f;
+									float maxAngularError = 0.0f;
+									float maxLinearErrorRatio = 0.0f;
+									float maxAngularErrorRatio = 0.0f;
+									std::map<int, Transform> optimizedPoses;
+									std::multimap<int, Link> links;
+									UASSERT(poses.find(fromId) != poses.end());
+									UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
+									UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
+									_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
+									UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
+									UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
+									UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
+									UASSERT(graph::findLink(links, from, to) != links.end());
+									optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
+									std::string msg;
+									if(optimizedPoses.size())
 									{
-										UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-										if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
+										graph::computeMaxGraphErrors(
+												optimizedPoses,
+												links,
+												maxLinearErrorRatio,
+												maxAngularErrorRatio,
+												maxLinearError,
+												maxAngularError,
+												&maxLinearLink,
+												&maxAngularLink);
+										if(maxLinearLink)
 										{
-											msg = uFormat("Rejecting edge %d->%d because "
-														"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
-														"\"%s\" is %f m.",
-														from,
-														to,
-														maxAngularError*180.0f/M_PI,
-														maxAngularLink->from(),
-														maxAngularLink->to(),
-														maxAngularErrorRatio,
-														sqrt(maxAngularLink->rotVariance()),
-														Parameters::kRGBDOptimizeMaxError().c_str(),
-														_optimizationMaxError);
+											UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+											if(maxLinearErrorRatio > _optimizationMaxError)
+											{
+												msg = uFormat("Rejecting edge %d->%d because "
+														  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+														  "\"%s\" is %f.",
+														  from,
+														  to,
+														  maxLinearError,
+														  maxLinearLink->from(),
+														  maxLinearLink->to(),
+														  maxLinearErrorRatio,
+														  sqrt(maxLinearLink->transVariance()),
+														  Parameters::kRGBDOptimizeMaxError().c_str(),
+														  _optimizationMaxError);
+											}
 										}
-										else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust())
+										else if(maxAngularLink)
 										{
-											UERROR("Huge optimization error detected!"
-													"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-													"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-													maxAngularErrorRatio,
-													maxAngularLink->from(),
-													maxAngularLink->to(),
-													maxAngularLink->type(),
-													maxAngularError*180.0f/CV_PI,
-													sqrt(maxAngularLink->rotVariance()),
-													Parameters::kRGBDOptimizeMaxError().c_str());
+											UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+											if(maxAngularErrorRatio > _optimizationMaxError)
+											{
+												msg = uFormat("Rejecting edge %d->%d because "
+														  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+														  "\"%s\" is %f m.",
+														  from,
+														  to,
+														  maxAngularError*180.0f/M_PI,
+														  maxAngularLink->from(),
+														  maxAngularLink->to(),
+														  maxAngularErrorRatio,
+														  sqrt(maxAngularLink->rotVariance()),
+														  Parameters::kRGBDOptimizeMaxError().c_str(),
+														  _optimizationMaxError);
+											}
 										}
 									}
-								}
-								else
-								{
-									msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
-												from,
-												to);
-								}
-								if(!msg.empty())
-								{
-									UWARN("%s", msg.c_str());
-									updateConstraints = false;
-								}
-								else
-								{
-									poses = optimizedPoses;
+									else
+									{
+										msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+												  from,
+												  to);
+									}
+									if(!msg.empty())
+									{
+										UWARN("%s", msg.c_str());
+										updateConstraints = false;
+									}
+									else
+									{
+										poses = optimizedPoses;
+									}
 								}

 								if(updateConstraints)
@@ -6057,7 +5895,7 @@ bool Rtabmap::addLink(const Link & link)
 		{
 			msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to());
 		}
-		else
+		else if(_optimizationMaxError > 0.0f)
 		{
 			float maxLinearError = 0.0f;
 			float maxLinearErrorRatio = 0.0f;
@@ -6078,7 +5916,7 @@ bool Rtabmap::addLink(const Link & link)
 			if(maxLinearLink)
 			{
 				UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-				if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
+				if(maxLinearErrorRatio > _optimizationMaxError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
 							  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
@@ -6093,24 +5931,11 @@ bool Rtabmap::addLink(const Link & link)
 							  Parameters::kRGBDOptimizeMaxError().c_str(),
 							  _optimizationMaxError);
 				}
-				else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust())
-				{
-					UERROR("Huge optimization error detected!"
-							"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-							"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-							maxLinearErrorRatio,
-							maxLinearLink->from(),
-							maxLinearLink->to(),
-							maxLinearLink->type(),
-							maxLinearError,
-							sqrt(maxLinearLink->transVariance()),
-							Parameters::kRGBDOptimizeMaxError().c_str());
-				}
 			}
 			else if(maxAngularLink)
 			{
 				UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-				if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
+				if(maxAngularErrorRatio > _optimizationMaxError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
 							  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
@@ -6125,19 +5950,6 @@ bool Rtabmap::addLink(const Link & link)
 							  Parameters::kRGBDOptimizeMaxError().c_str(),
 							  _optimizationMaxError);
 				}
-				else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust())
-				{
-					UERROR("Huge optimization error detected!"
-							"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-							"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-							maxAngularErrorRatio,
-							maxAngularLink->from(),
-							maxAngularLink->to(),
-							maxAngularLink->type(),
-							maxAngularError*180.0f/CV_PI,
-							sqrt(maxAngularLink->rotVariance()),
-							Parameters::kRGBDOptimizeMaxError().c_str());
-				}
 			}
 		}
 		if(!msg.empty())
@@ -6242,7 +6054,7 @@ bool Rtabmap::addLink(const Link & link)
 			UWARN("Optimization failed, rejecting localization!");
 			rejectLocalization = true;
 		}
-		else
+		else if(_optimizationMaxError > 0.0f)
 		{
 			UINFO("Compute max graph errors...");
 			float maxLinearError = 0.0f;
@@ -6269,7 +6081,7 @@ bool Rtabmap::addLink(const Link & link)
 			if(maxLinearLink)
 			{
 				UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-				if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
+				if(maxLinearErrorRatio > _optimizationMaxError)
 				{
 					UWARN("Rejecting localization (%d <-> %d) in this "
 							"iteration because a wrong loop closure has been "
@@ -6288,24 +6100,11 @@ bool Rtabmap::addLink(const Link & link)
 							_optimizationMaxError);
 					rejectLocalization = true;
 				}
-				else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust())
-				{
-					UERROR("Huge optimization error detected!"
-							"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-							"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-							maxLinearErrorRatio,
-							maxLinearLink->from(),
-							maxLinearLink->to(),
-							maxLinearLink->type(),
-							maxLinearError,
-							sqrt(maxLinearLink->transVariance()),
-							Parameters::kRGBDOptimizeMaxError().c_str());
-				}
 			}
 			if(maxAngularLink)
 			{
 				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
-				if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
+				if(maxAngularErrorRatio > _optimizationMaxError)
 				{
 					UWARN("Rejecting localization (%d <-> %d) in this "
 							"iteration because a wrong loop closure has been "
@@ -6324,19 +6123,6 @@ bool Rtabmap::addLink(const Link & link)
 							_optimizationMaxError);
 					rejectLocalization = true;
 				}
-				else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust())
-				{
-					UERROR("Huge optimization error detected!"
-							"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
-							"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
-							maxAngularErrorRatio,
-							maxAngularLink->from(),
-							maxAngularLink->to(),
-							maxAngularLink->type(),
-							maxAngularError*180.0f/CV_PI,
-							sqrt(maxAngularLink->rotVariance()),
-							Parameters::kRGBDOptimizeMaxError().c_str());
-				}
 			}
 		}

@@ -810,23 +810,23 @@ void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const
 unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 {
 	return sizeof(SensorData) +
-			(_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) +
-			(_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) +
-			(_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) +
-			(_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) +
-			(_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) +
-			(_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) +
-			(_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) +
-			(_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) +
-			(_groundCellsCompressed.empty()?0:_groundCellsCompressed.total()*_groundCellsCompressed.elemSize()) +
-			(_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) +
-			(_obstacleCellsCompressed.empty()?0:_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize()) +
-			(_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+
-			(_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) +
-			(_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+
+			_imageCompressed.total()*_imageCompressed.elemSize() +
+			_imageRaw.total()*_imageRaw.elemSize() +
+			_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() +
+			_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
+			_userDataCompressed.total()*_userDataCompressed.elemSize() +
+			_userDataRaw.total()*_userDataRaw.elemSize() +
+			_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() +
+			_laserScanRaw.data().total()*_laserScanRaw.data().elemSize() +
+			_groundCellsCompressed.total()*_groundCellsCompressed.elemSize() +
+			_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
+			_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
+			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
+			_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
+			_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
 			_keypoints.size() * sizeof(cv::KeyPoint) +
 			_keypoints3D.size() * sizeof(cv::Point3f) +
-			(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
+			_descriptors.total()*_descriptors.elemSize();
 }

 void SensorData::clearCompressedData(bool images, bool scan, bool userData)
@@ -348,7 +348,7 @@ unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memo
 	total += _words.size() * (sizeof(int)*2+sizeof(std::multimap<int, cv::KeyPoint>::iterator)) + sizeof(std::multimap<int, cv::KeyPoint>);
 	total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
 	total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
-	total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
+	total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
 	total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
 	if(withSensorData)
 	{
@@ -211,24 +211,14 @@ Transform Transform::to3DoF() const
 {
 	float x,y,z,roll,pitch,yaw;
 	this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-	float A = std::cos(yaw);
-	float B = std::sin(yaw);
-	return Transform(
-			A,-B, 0, x,
-			B, A, 0, y,
-			0, 0, 1, 0);
+	return Transform(x,y,0, 0,0,yaw);
 }

 Transform Transform::to4DoF() const
 {
 	float x,y,z,roll,pitch,yaw;
 	this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-	float A = std::cos(yaw);
-	float B = std::sin(yaw);
-	return Transform(
-			A,-B, 0, x,
-			B, A, 0, y,
-			0, 0, 1, z);
+	return Transform(x,y,z, 0,0,yaw);
 }

 bool Transform::is3DoF() const
@@ -242,7 +232,7 @@ bool Transform::is4DoF() const
 		   r23() == 0.0 &&
 		   r31() == 0.0 &&
 		   r32() == 0.0 &&
-		   r33() == 1.0;
+		   r33() == 0.0;
 }

 cv::Mat Transform::rotationMatrix() const
@@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

 #include <rtabmap/core/camera/CameraDepthAI.h>
-#include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsManager.h>
@@ -46,31 +45,22 @@ bool CameraDepthAI::available()
 }

 CameraDepthAI::CameraDepthAI(
-		const std::string & mxidOrName,
+		const std::string & deviceSerial,
 		int resolution,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_DEPTHAI
 	,
-	mxidOrName_(mxidOrName),
-	outputMode_(0),
-	confThreshold_(200),
-	lrcThreshold_(5),
+	deviceSerial_(deviceSerial),
+	outputDepth_(false),
+	depthConfidence_(200),
 	resolution_(resolution),
-	useSpecTranslation_(false),
-	alphaScaling_(0.0),
+	imuFirmwareUpdate_(false),
 	imuPublished_(true),
 	publishInterIMU_(false),
 	dotProjectormA_(0.0),
-	floodLightmA_(200.0),
-	detectFeatures_(0),
-	useHarrisDetector_(false),
-	minDistance_(7.0),
-	numTargetFeatures_(1000),
-	threshold_(0.01),
-	nms_(true),
-	nmsRadius_(4)
+	floodLightmA_(200.0)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
@@ -88,299 +78,169 @@ CameraDepthAI::~CameraDepthAI()
 #endif
 }

-void CameraDepthAI::setOutputMode(int outputMode)
+void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
 {
 #ifdef RTABMAP_DEPTHAI
-	outputMode_ = outputMode;
+	outputDepth_ = enabled;
+	if(outputDepth_)
+	{
+		depthConfidence_ = confidence;
+	}
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }

-void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold)
+void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
 {
 #ifdef RTABMAP_DEPTHAI
-	confThreshold_ = confThreshold;
-	lrcThreshold_ = lrcThreshold;
+	imuFirmwareUpdate_ = enabled;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }

-void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling)
+void CameraDepthAI::setIMUPublished(bool published)
 {
 #ifdef RTABMAP_DEPTHAI
-	useSpecTranslation_ = useSpecTranslation;
-	alphaScaling_ = alphaScaling;
+	imuPublished_ = published;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }

-void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
+void CameraDepthAI::publishInterIMU(bool enabled)
 {
 #ifdef RTABMAP_DEPTHAI
-	imuPublished_ = imuPublished;
-	publishInterIMU_ = publishInterIMU;
+	publishInterIMU_ = enabled;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }

-void CameraDepthAI::setIrBrightness(float dotProjectormA, float floodLightmA)
+void CameraDepthAI::setLaserDotBrightness(float dotProjectormA)
 {
 #ifdef RTABMAP_DEPTHAI
 	dotProjectormA_ = dotProjectormA;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setFloodLightBrightness(float floodLightmA)
+{
+#ifdef RTABMAP_DEPTHAI
 	floodLightmA_ = floodLightmA;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }

-void CameraDepthAI::setDetectFeatures(int detectFeatures)
-{
-#ifdef RTABMAP_DEPTHAI
-	detectFeatures_ = detectFeatures;
-#else
-	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
-#endif
-}
-
-void CameraDepthAI::setBlobPath(const std::string & blobPath)
-{
-#ifdef RTABMAP_DEPTHAI
-	blobPath_ = blobPath;
-#else
-	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
-#endif
-}
-
-void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, float minDistance, int numTargetFeatures)
-{
-#ifdef RTABMAP_DEPTHAI
-	useHarrisDetector_ = useHarrisDetector;
-	minDistance_ = minDistance;
-	numTargetFeatures_ = numTargetFeatures;
-#else
-	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
-#endif
-}
-
-void CameraDepthAI::setSuperPointDetector(float threshold, bool nms, int nmsRadius)
-{
-#ifdef RTABMAP_DEPTHAI
-	threshold_ = threshold;
-	nms_ = nms;
-	nmsRadius_ = nmsRadius;
-#else
-	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
-#endif
-}
-
 bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
 #ifdef RTABMAP_DEPTHAI

 	std::vector<dai::DeviceInfo> devices = dai::Device::getAllAvailableDevices();
-	if(devices.empty() && mxidOrName_.empty())
+	if(devices.empty())
 	{
-		UERROR("No DepthAI device found or specified");
 		return false;
 	}

 	if(device_.get())
+	{
 		device_->close();
-
+	}
 	accBuffer_.clear();
 	gyroBuffer_.clear();

-	bool deviceFound = false;
-	dai::DeviceInfo deviceToUse(mxidOrName_);
-	if(mxidOrName_.empty())
-		std::tie(deviceFound, deviceToUse) = dai::Device::getFirstAvailableDevice();
-	else if(!deviceToUse.mxid.empty())
-		std::tie(deviceFound, deviceToUse) = dai::Device::getDeviceByMxId(deviceToUse.mxid);
-	else
-		deviceFound = true;
-
-	if(!deviceFound)
+	dai::DeviceInfo deviceToUse;
+	if(deviceSerial_.empty())
+		deviceToUse = devices[0];
+	for(size_t i=0; i<devices.size(); ++i)
 	{
-		UERROR("Could not find DepthAI device with MXID or IP/USB name \"%s\", found devices:", mxidOrName_.c_str());
-		for(auto& device : devices)
-			UERROR("%s", device.toString().c_str());
+		UINFO("DepthAI device found: %s", devices[i].getMxId().c_str());
+		if(!deviceSerial_.empty() && deviceSerial_.compare(devices[i].getMxId()) == 0)
+		{
+			deviceToUse = devices[i];
+		}
+	}
+
+	if(deviceToUse.getMxId().empty())
+	{
+		UERROR("Could not find device with serial \"%s\", found devices:", deviceSerial_.c_str());
+		for(size_t i=0; i<devices.size(); ++i)
+		{
+			UERROR("DepthAI device found: %s", devices[i].getMxId().c_str());
+		}
 		return false;
 	}
+	deviceSerial_ = deviceToUse.getMxId();

 	// look for calibration files
 	stereoModel_ = StereoCameraModel();
-	targetSize_ = cv::Size(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
+	cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);

 	dai::Pipeline p;
 	auto monoLeft  = p.create<dai::node::MonoCamera>();
 	auto monoRight = p.create<dai::node::MonoCamera>();
 	auto stereo    = p.create<dai::node::StereoDepth>();
-	std::shared_ptr<dai::node::Camera> colorCam;
-	if(outputMode_==2)
-	{
-		colorCam = p.create<dai::node::Camera>();
-		if(detectFeatures_)
-		{
-			UWARN("On-device feature detectors cannot be enabled on color camera input!");
-			detectFeatures_ = 0;
-		}
-	}
 	std::shared_ptr<dai::node::IMU> imu;
 	if(imuPublished_)
 		imu = p.create<dai::node::IMU>();
-	std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
-	std::shared_ptr<dai::node::ImageManip> manip;
-	std::shared_ptr<dai::node::NeuralNetwork> superPointNetwork;
-	if(detectFeatures_ == 1)
-	{
-		gfttDetector = p.create<dai::node::FeatureTracker>();
-	}
-	else if(detectFeatures_ == 2)
-	{
-		if(!blobPath_.empty())
-		{
-			manip = p.create<dai::node::ImageManip>();
-			superPointNetwork = p.create<dai::node::NeuralNetwork>();
-		}
-		else
-		{
-			UWARN("Missing SuperPoint blob file!");
-			detectFeatures_ = 0;
-		}
-	}

-	auto xoutLeftOrColor = p.create<dai::node::XLinkOut>();
+	auto xoutLeft = p.create<dai::node::XLinkOut>();
 	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
 	std::shared_ptr<dai::node::XLinkOut> xoutIMU;
 	if(imuPublished_)
 		xoutIMU = p.create<dai::node::XLinkOut>();
-	std::shared_ptr<dai::node::XLinkOut> xoutFeatures;
-	if(detectFeatures_)
-		xoutFeatures = p.create<dai::node::XLinkOut>();

 	// XLinkOut
-	xoutLeftOrColor->setStreamName(outputMode_<2?"rectified_left":"rectified_color");
-	xoutDepthOrRight->setStreamName(outputMode_?"depth":"rectified_right");
+	xoutLeft->setStreamName("rectified_left");
+	xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
 	if(imuPublished_)
 		xoutIMU->setStreamName("imu");
-	if(detectFeatures_)
-		xoutFeatures->setStreamName("features");

+	// MonoCamera
 	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
+	monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
 	monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
-	monoLeft->setCamera("left");
-	monoRight->setCamera("right");
-	if(detectFeatures_ == 2)
-	{
-		if(this->getImageRate() <= 0 || this->getImageRate() > 15)
-		{
-			UWARN("On-device SuperPoint enabled, image rate is limited to 15 FPS!");
-			monoLeft->setFps(15);
-			monoRight->setFps(15);
-		}
-	}
-	else if(this->getImageRate() > 0)
+	monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
+	if(this->getImageRate()>0)
 	{
 		monoLeft->setFps(this->getImageRate());
 		monoRight->setFps(this->getImageRate());
 	}

 	// StereoDepth
-	if(outputMode_ == 2)
-		stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
-	else
-		stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
+	stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
+	stereo->setSubpixel(true);
+	stereo->setSubpixelFractionalBits(4);
 	stereo->setExtendedDisparity(false);
 	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
-	stereo->enableDistortionCorrection(true);
-	stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
-	stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_);
-	if(alphaScaling_ > -1.0f)
-		stereo->setAlphaScaling(alphaScaling_);
-	stereo->initialConfig.setConfidenceThreshold(confThreshold_);
+	stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
 	stereo->initialConfig.setLeftRightCheck(true);
-	stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
-	stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
+	stereo->initialConfig.setLeftRightCheckThreshold(5);
+	stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5);
 	auto config = stereo->initialConfig.get();
-	config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
-	config.censusTransform.kernelMask = 0X2AA00AA805540155;
-	config.postProcessing.brightnessFilter.maxBrightness = 255;
+	config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64;
+	config.costMatching.enableCompanding = true;
 	stereo->initialConfig.set(config);

 	// Link plugins CAM -> STEREO -> XLINK
 	monoLeft->out.link(stereo->left);
 	monoRight->out.link(stereo->right);

-	if(outputMode_ == 2)
-	{
-		colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
-		colorCam->setSize(targetSize_.width, targetSize_.height);
-		if(this->getImageRate() > 0)
-			colorCam->setFps(this->getImageRate());
-		if(alphaScaling_ > -1.0f)
-			colorCam->setCalibrationAlpha(alphaScaling_);
-	}
-
-	// Using VideoEncoder on PoE devices, Subpixel is not supported
-	if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
-	{
-		auto leftOrColorEnc  = p.create<dai::node::VideoEncoder>();
-		auto depthOrRightEnc  = p.create<dai::node::VideoEncoder>();
-		leftOrColorEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
-		depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG);
-		if(outputMode_ < 2)
-		{
-			stereo->rectifiedLeft.link(leftOrColorEnc->input);
-		}
-		else
-		{
-			colorCam->video.link(leftOrColorEnc->input);
-		}
-		if(outputMode_)
-		{
-			depthOrRightEnc->setQuality(100);
-			stereo->disparity.link(depthOrRightEnc->input);
-		}
-		else
-		{
-			stereo->rectifiedRight.link(depthOrRightEnc->input);
-		}
-		leftOrColorEnc->bitstream.link(xoutLeftOrColor->input);
-		depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
-	}
+	stereo->rectifiedLeft.link(xoutLeft->input);
+	if(outputDepth_)
+		stereo->depth.link(xoutDepthOrRight->input);
 	else
-	{
-		stereo->setSubpixel(true);
-		stereo->setSubpixelFractionalBits(4);
-		config = stereo->initialConfig.get();
-		config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64;
-		config.costMatching.enableCompanding = true;
-		stereo->initialConfig.set(config);
-		if(outputMode_ < 2)
-		{
-			stereo->rectifiedLeft.link(xoutLeftOrColor->input);
-		}
-		else
-		{
-			monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
-			monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
-			colorCam->video.link(xoutLeftOrColor->input);
-		}
-		if(outputMode_)
-			stereo->depth.link(xoutDepthOrRight->input);
-		else
-			stereo->rectifiedRight.link(xoutDepthOrRight->input);
-	}
+		stereo->rectifiedRight.link(xoutDepthOrRight->input);

 	if(imuPublished_)
 	{
-		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 100 hz rate
-		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100);
+		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
+		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
 		// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
 		imu->setBatchReportThreshold(1);
 		// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
@@ -390,74 +250,28 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin

 		// Link plugins IMU -> XLINK
 		imu->out.link(xoutIMU->input);
-	}

-	if(detectFeatures_ == 1)
-	{
-		gfttDetector->setHardwareResources(1, 2);
-		gfttDetector->initialConfig.setCornerDetector(
-			useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI);
-		gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_);
-		gfttDetector->initialConfig.setMotionEstimator(false);
-		auto cfg = gfttDetector->initialConfig.get();
-		cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
-		gfttDetector->initialConfig.set(cfg);
-		stereo->rectifiedLeft.link(gfttDetector->inputImage);
-		gfttDetector->outputFeatures.link(xoutFeatures->input);
-	}
-	else if(detectFeatures_ == 2)
-	{
-		manip->setKeepAspectRatio(false);
-		manip->setMaxOutputFrameSize(320 * 200);
-		manip->initialConfig.setResize(320, 200);
-		superPointNetwork->setBlobPath(blobPath_);
-		superPointNetwork->setNumInferenceThreads(2);
-		superPointNetwork->setNumNCEPerInferenceThread(1);
-		superPointNetwork->input.setBlocking(false);
-		stereo->rectifiedLeft.link(manip->inputImage);
-		manip->out.link(superPointNetwork->input);
-		superPointNetwork->out.link(xoutFeatures->input);
+		imu->enableFirmwareUpdate(imuFirmwareUpdate_);
 	}

 	device_.reset(new dai::Device(p, deviceToUse));

 	UINFO("Loading eeprom calibration data");
 	dai::CalibrationHandler calibHandler = device_->readCalibration();
-
-	auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A;
-	cv::Mat cameraMatrix, distCoeffs, newCameraMatrix;
-
-	std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(cameraId, targetSize_.width, targetSize_.height);
-	cameraMatrix = (cv::Mat_<double>(3,3) <<
-		matrix[0][0], matrix[0][1], matrix[0][2],
-		matrix[1][0], matrix[1][1], matrix[1][2],
-		matrix[2][0], matrix[2][1], matrix[2][2]);
-
-	std::vector<float> coeffs = calibHandler.getDistortionCoefficients(cameraId);
-	if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective)
-		distCoeffs = (cv::Mat_<double>(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]);
-
-	if(alphaScaling_>-1.0f)
-		newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
-	else
-		newCameraMatrix = cameraMatrix;
-
-	double fx = newCameraMatrix.at<double>(0, 0);
-	double fy = newCameraMatrix.at<double>(1, 1);
-	double cx = newCameraMatrix.at<double>(0, 2);
-	double cy = newCameraMatrix.at<double>(1, 2);
-	double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, useSpecTranslation_)/100.0;
-	UINFO("fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
-	if(outputMode_ == 2)
-		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
-	else
-		stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_);
+	std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height));
+	double fx = matrix[0][0];
+	double fy = matrix[1][1];
+	double cx = matrix[0][2];
+	double cy = matrix[1][2];
+	double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT, false)/100.0;
+	UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
+	stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);

 	if(imuPublished_)
 	{
 		// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
 		// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
-		//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::CAM_B);
+		//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
 		//imuLocalTransform_ = Transform(
 		//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
 		//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
@@ -468,29 +282,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		{
 			imuLocalTransform_ = Transform(
 				 0, -1,  0,  0.0525,
-				 1,  0,  0,  0.013662,
+				 1,  0,  0,  0.0137,
 				 0,  0,  1,  0);
 		}
 		else if(eeprom.boardName == "DM9098")
 		{
 			imuLocalTransform_ = Transform(
-				 0,  1,  0,  0.037945,
-				 1,  0,  0,  0.00079,
-				 0,  0, -1,  0);
-		}
-		else if(eeprom.boardName == "NG2094")
-		{
-			imuLocalTransform_ = Transform(
-				 0,  1,  0,  0.0374,
-				 1,  0,  0,  0.00176,
-				 0,  0, -1,  0);
-		}
-		else if(eeprom.boardName == "NG9097")
-		{
-			imuLocalTransform_ = Transform(
-				 0,  1,  0,  0.04,
-				 1,  0,  0,  0.020265,
-				 0,  0, -1,  0);
+				 0,  1,  0,  0.0754,
+				 1,  0,  0,  0.0026,
+				 0,  0, -1, -0.007);
 		}
 		else
 		{
@@ -507,37 +307,38 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	{
 		imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
 		UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
-		device_->getOutputQueue("imu", 50, false)->addCallback([this](const std::shared_ptr<dai::ADatatype> data) {
-			auto imuData = std::dynamic_pointer_cast<dai::IMUData>(data);
-			auto imuPackets = imuData->packets;
-
-			for(auto& imuPacket : imuPackets)
+		device_->getOutputQueue("imu", 50, false)->addCallback([this](std::shared_ptr<dai::ADatatype> callback) {
+			if(dynamic_cast<dai::IMUData*>(callback.get()) != nullptr)
 			{
-				auto& acceleroValues = imuPacket.acceleroMeter;
-				auto& gyroValues = imuPacket.gyroscope;
-				double accStamp = std::chrono::duration<double>(acceleroValues.getTimestampDevice().time_since_epoch()).count();
-				double gyroStamp = std::chrono::duration<double>(gyroValues.getTimestampDevice().time_since_epoch()).count();
+				dai::IMUData* imuData = static_cast<dai::IMUData*>(callback.get());
+				auto imuPackets = imuData->packets;

-				if(publishInterIMU_)
+				for(auto& imuPacket : imuPackets)
 				{
-					IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1),
-							cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1),
-							imuLocalTransform_);
-					UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2));
-				}
-				else
-				{
-					UScopeMutex lock(imuMutex_);
-					accBuffer_.emplace_hint(accBuffer_.end(), accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z));
-					gyroBuffer_.emplace_hint(gyroBuffer_.end(), gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z));
+					auto& acceleroValues = imuPacket.acceleroMeter;
+					auto& gyroValues = imuPacket.gyroscope;
+					double accStamp = std::chrono::duration<double>(acceleroValues.getTimestampDevice().time_since_epoch()).count();
+					double gyroStamp = std::chrono::duration<double>(gyroValues.getTimestampDevice().time_since_epoch()).count();
+
+					if(publishInterIMU_)
+					{
+						IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1),
+								cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1),
+								imuLocalTransform_);
+						UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2));
+					}
+					else
+					{
+						UScopeMutex lock(imuMutex_);
+						accBuffer_.emplace_hint(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
+						gyroBuffer_.emplace_hint(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
+					}
 				}
 			}
 		});
 	}
-	leftOrColorQueue_ = device_->getOutputQueue(outputMode_<2?"rectified_left":"rectified_color", 8, false);
-	rightOrDepthQueue_ = device_->getOutputQueue(outputMode_?"depth":"rectified_right", 8, false);
-	if(detectFeatures_)
-		featuresQueue_ = device_->getOutputQueue("features", 8, false);
+	leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
+	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);

 	std::vector<std::tuple<std::string, int, int>> irDrivers = device_->getIrDrivers();
 	if(!irDrivers.empty())
@@ -567,7 +368,7 @@ bool CameraDepthAI::isCalibrated() const
 std::string CameraDepthAI::getSerial() const
 {
 #ifdef RTABMAP_DEPTHAI
-	return device_->getMxId();
+	return deviceSerial_;
 #endif
 	return "";
 }
@@ -577,37 +378,23 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	SensorData data;
 #ifdef RTABMAP_DEPTHAI

-	cv::Mat leftOrColor, depthOrRight;
-	auto rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
+	cv::Mat left, depthOrRight;
+	auto rectifL = leftQueue_->get<dai::ImgFrame>();
 	auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();

-	while(rectifLeftOrColor->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
-		rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
-	while(rectifLeftOrColor->getSequenceNum() > rectifRightOrDepth->getSequenceNum())
+	while(rectifL->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
+		rectifL = leftQueue_->get<dai::ImgFrame>();
+	while(rectifL->getSequenceNum() > rectifRightOrDepth->getSequenceNum())
 		rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();

-	double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
-	if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
-	{
-		leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
-		depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
-		if(outputMode_)
-		{
-			cv::Mat disp;
-			depthOrRight.convertTo(disp, CV_16UC1);
-			cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight);
-		}
-	}
-	else
-	{
-		leftOrColor = rectifLeftOrColor->getCvFrame();
-		depthOrRight = rectifRightOrDepth->getCvFrame();
-	}
+	double stamp = std::chrono::duration<double>(rectifL->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
+	left = rectifL->getCvFrame();
+	depthOrRight = rectifRightOrDepth->getCvFrame();

-	if(outputMode_)
-		data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
+	if(depthOrRight.type() == CV_8UC1)
+		data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
 	else
-		data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
+		data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);

 	if(imuPublished_ && !publishInterIMU_)
 	{
@@ -658,84 +445,6 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 		data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
 	}

-	if(detectFeatures_ == 1)
-	{
-		auto features = featuresQueue_->get<dai::TrackedFeatures>();
-		while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
-			features = featuresQueue_->get<dai::TrackedFeatures>();
-		auto detectedFeatures = features->trackedFeatures;
-
-		std::vector<cv::KeyPoint> keypoints;
-		for(auto& feature : detectedFeatures)
-			keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3));
-		data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
-	}
-	else if(detectFeatures_ == 2)
-	{
-		auto features = featuresQueue_->get<dai::NNData>();
-		while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
-			features = featuresQueue_->get<dai::NNData>();
-
-		auto heatmap = features->getLayerFp16("heatmap");
-		auto desc = features->getLayerFp16("desc");
-
-		cv::Mat scores(200, 320, CV_32FC1, heatmap.data());
-		cv::resize(scores, scores, targetSize_, 0, 0, cv::INTER_CUBIC);
-
-		if(nms_)
-		{
-			cv::Mat dilated_scores(targetSize_, CV_32FC1);
-			cv::dilate(scores, dilated_scores, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1)));
-			cv::Mat max_mask = scores == dilated_scores;
-			cv::dilate(scores, dilated_scores, cv::Mat());
-			cv::Mat max_mask_r1 = scores == dilated_scores;
-			cv::Mat supp_mask(targetSize_, CV_8UC1);
-			for(size_t i=0; i<2; i++)
-			{
-				cv::dilate(max_mask, supp_mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1)));
-				cv::Mat supp_scores = scores.clone();
-				supp_scores.setTo(0, supp_mask);
-				cv::dilate(supp_scores, dilated_scores, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1)));
-				cv::Mat new_max_mask = cv::Mat::zeros(targetSize_, CV_8UC1);
-				cv::bitwise_not(supp_mask, supp_mask);
-				cv::bitwise_and(supp_scores == dilated_scores, supp_mask, new_max_mask, max_mask_r1);
-				cv::bitwise_or(max_mask, new_max_mask, max_mask);
-			}
-			cv::bitwise_not(max_mask, supp_mask);
-			scores.setTo(0, supp_mask);
-		}
-
-		std::vector<cv::Point> kpts;
-		cv::findNonZero(scores > threshold_, kpts);
-		std::vector<cv::KeyPoint> keypoints;
-		for(auto& kpt : kpts)
-		{
-			float response = scores.at<float>(kpt);
-			keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
-		}
-
-		cv::Mat coarse_desc(25, 40, CV_32FC(256), desc.data());
-		coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
-			cv::normalize(descriptor, descriptor);
-		});
-		cv::Mat mapX(keypoints.size(), 1, CV_32FC1);
-		cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
-		for(size_t i=0; i<keypoints.size(); ++i)
-		{
-			mapX.at<float>(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2;
-			mapY.at<float>(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2;
-		}
-		cv::Mat map1, map2, descriptors;
-		cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2);
-		cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR);
-		descriptors.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
-			cv::normalize(descriptor, descriptor);
-		});
-		descriptors = descriptors.reshape(1);
-
-		data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
-	}
-
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
@@ -1501,7 +1501,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 						getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
 						if(!imuTmp.empty())
 						{
-							UEventsManager::post(new IMUEvent(imuTmp, stamps[i]/1000.0));
+							UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
 							pub++;
 						}
 						else
@@ -501,47 +501,32 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 #endif
 	sl::Resolution res = stereoParams->left_cam.image_size;

+	stereoModel_ = StereoCameraModel(
+		stereoParams->left_cam.fx, 
+		stereoParams->left_cam.fy, 
+		stereoParams->left_cam.cx, 
+		stereoParams->left_cam.cy, 
 #if ZED_SDK_MAJOR_VERSION < 4
-	stereoModel_ = StereoCameraModel(
-		stereoParams->left_cam.fx,
-		stereoParams->left_cam.fy,
-		stereoParams->left_cam.cx,
-		stereoParams->left_cam.cy,
 		stereoParams->T[0],//baseline
-		this->getLocalTransform(),
-		cv::Size(res.width, res.height));
 #else
-	stereoModel_ = StereoCameraModel(
-		stereoParams->left_cam.fx, 
-		stereoParams->left_cam.fy, 
-		stereoParams->left_cam.cx, 
-		stereoParams->left_cam.cy, 
 		stereoParams->getCameraBaseline(),
+#endif
 		this->getLocalTransform(),
 		cv::Size(res.width, res.height));
-#endif

-#if ZED_SDK_MAJOR_VERSION < 4
-	UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
-		stereoParams->left_cam.fx,
-		stereoParams->left_cam.fy,
-		stereoParams->left_cam.cx,
-		stereoParams->left_cam.cy,
-		stereoParams->T[0],//baseline
-		(int)res.width,
-		(int)res.height,
-		this->getLocalTransform().prettyPrint().c_str());
-#else
 	UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
 			stereoParams->left_cam.fx,
 			stereoParams->left_cam.fy,
 			stereoParams->left_cam.cx,
 			stereoParams->left_cam.cy,
+#if ZED_SDK_MAJOR_VERSION < 4
+			stereoParams->T[0],//baseline
+#else
 			stereoParams->getCameraBaseline(),
+#endif
 			(int)res.width,
 			(int)res.height,
 			this->getLocalTransform().prettyPrint().c_str());
-#endif

 #if ZED_SDK_MAJOR_VERSION < 3
 	if(infos.camera_model == sl::MODEL_ZED_M)
@@ -554,15 +539,12 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 #else
 		imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).inverse();
 #endif
-
+		UINFO("IMU local transform: %s (imu2cam=%s))",
+		imuLocalTransform_.prettyPrint().c_str(),
 #if ZED_SDK_MAJOR_VERSION < 4
-		UINFO("IMU local transform: %s (imu2cam=%s))",
-			imuLocalTransform_.prettyPrint().c_str(),
-			zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
+		zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
 #else
-		UINFO("IMU local transform: %s (imu2cam=%s))",
-			imuLocalTransform_.prettyPrint().c_str(),
-			zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
+		zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
 #endif
 		if(publishInterIMU_)
 		{
@@ -1,153 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include <rtabmap/core/global_map/CloudMap.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/util3d_filtering.h>
-#include <rtabmap/core/util3d_mapping.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UTimer.h>
-
-#include <pcl/io/pcd_io.h>
-
-namespace rtabmap {
-
-CloudMap::CloudMap(const LocalGridCache * cache, const ParametersMap & parameters) :
-	GlobalMap(cache, parameters),
-	assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
-	assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
-	assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZ>)
-{
-}
-
-void CloudMap::clear()
-{
-	assembledGround_->clear();
-	assembledObstacles_->clear();
-	assembledEmptyCells_->clear();
-	GlobalMap::clear();
-}
-
-void CloudMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
-{
-	UTimer timer;
-
-	bool assembledGroundUpdated = false;
-	bool assembledObstaclesUpdated = false;
-	bool assembledEmptyCellsUpdated = false;
-
-	if(!cache().empty())
-	{
-		UDEBUG("Updating from cache");
-		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
-		{
-			if(uContains(cache(), iter->first))
-			{
-				const LocalGrid & localGrid = cache().at(iter->first);
-
-				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
-
-				addAssembledNode(iter->first, iter->second);
-
-				//ground
-				if(localGrid.groundCells.cols)
-				{
-					if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
-					}
-
-					*assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.groundCells), iter->second, 0, 255, 0);
-					assembledGroundUpdated = true;
-				}
-
-				//empty
-				if(localGrid.emptyCells.cols)
-				{
-					if(localGrid.emptyCells.rows > 1 && localGrid.emptyCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.emptyCells.rows, localGrid.emptyCells.cols);
-					}
-
-					*assembledEmptyCells_ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localGrid.emptyCells), iter->second);
-					assembledEmptyCellsUpdated = true;
-				}
-
-				//obstacles
-				if(localGrid.obstacleCells.cols)
-				{
-					if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
-					}
-
-					*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.obstacleCells), iter->second, 255, 0, 0);
-					assembledObstaclesUpdated = true;
-				}
-			}
-		}
-	}
-
-
-	if(assembledGroundUpdated && assembledGround_->size() > 1)
-	{
-		assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
-	}
-	if(assembledObstaclesUpdated && assembledGround_->size() > 1)
-	{
-		assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
-	}
-	if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
-	{
-		assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
-	}
-
-	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
-}
-
-unsigned long CloudMap::getMemoryUsed() const
-{
-	unsigned long memoryUsage = GlobalMap::getMemoryUsed();
-
-	if(assembledGround_.get())
-	{
-		memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
-	}
-	if(assembledObstacles_.get())
-	{
-		memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
-	}
-	if(assembledEmptyCells_.get())
-	{
-		memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZ);
-	}
-	return memoryUsage;
-}
-
-}
@@ -1,485 +0,0 @@
-/*
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include <rtabmap/core/global_map/GridMap.h>
-#include <rtabmap/core/util3d_transforms.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/util3d_surface.h>
-#include <rtabmap/core/CameraModel.h>
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UStl.h>
-#include <list>
-
-#include <opencv2/photo.hpp>
-#include <grid_map_core/iterators/GridMapIterator.hpp>
-#include <pcl/io/pcd_io.h>
-
-namespace rtabmap {
-
-GridMap::GridMap(const LocalGridCache * cache, const ParametersMap & parameters) :
-	GlobalMap(cache, parameters),
-	minMapSize_(Parameters::defaultGridGlobalMinSize())
-{
-	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
-}
-
-void GridMap::clear()
-{
-	gridMap_ = grid_map::GridMap();
-	GlobalMap::clear();
-}
-
-cv::Mat GridMap::createHeightMap(float & xMin, float & yMin, float & cellSize) const
-{
-	return toImage("elevation", xMin, yMin, cellSize);
-}
-
-cv::Mat GridMap::createColorMap(float & xMin, float & yMin, float & cellSize) const
-{
-	return toImage("colors", xMin, yMin, cellSize);
-}
-
-cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const
-{
-	if( gridMap_.hasBasicLayers())
-	{
-		const grid_map::Matrix& data = gridMap_[layer];
-
-		cv::Mat image;
-		if(layer.compare("elevation") == 0)
-		{
-			image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_32FC1);
-			for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
-				const grid_map::Index index(*iterator);
-				const float& value = data(index(0), index(1));
-				const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
-				if (std::isfinite(value))
-				{
-					image.at<float>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0)) = value;
-				}
-			}
-		}
-		else if(layer.compare("colors") == 0)
-		{
-			image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_8UC3);
-			for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
-				const grid_map::Index index(*iterator);
-				const float& value = data(index(0), index(1));
-				const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
-				if (std::isfinite(value))
-				{
-					const int * ptr = (const int *)&value;
-					cv::Vec3b & color = image.at<cv::Vec3b>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0));
-					color[0] = (unsigned char)(*ptr & 0xFF); // B
-					color[1] = (unsigned char)((*ptr >> 8) & 0xFF); // G
-					color[2] = (unsigned char)((*ptr >> 16) & 0xFF); // R
-				}
-			}
-		}
-		else
-		{
-			UFATAL("Unknown layer \"%s\"", layer.c_str());
-		}
-
-		xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
-		yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
-		cellSize = gridMap_.getResolution();
-
-		return image;
-
-	}
-	return cv::Mat();
-}
-
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr GridMap::createTerrainCloud() const
-{
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	if( gridMap_.hasBasicLayers())
-	{
-		const grid_map::Matrix& dataElevation = gridMap_["elevation"];
-		const grid_map::Matrix& dataColors = gridMap_["colors"];
-
-		cloud->width = gridMap_.getSize()(0);
-		cloud->height = gridMap_.getSize()(1);
-		cloud->resize(cloud->width * cloud->height);
-		cloud->is_dense = false;
-
-		float xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
-		float yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
-		float cellSize = gridMap_.getResolution();
-
-		for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator)
-		{
-			const grid_map::Index index(*iterator);
-			const float& value = dataElevation(index(0), index(1));
-			const int* color = (const int*)&dataColors(index(0), index(1));
-			const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
-			pcl::PointXYZRGB & pt = cloud->at(cloud->width-1-imageIndex(0), imageIndex(1));
-			if (std::isfinite(value))
-			{
-				pt.x = xMin + (cloud->width-1-imageIndex(0)) * cellSize;
-				pt.y = yMin + (cloud->height-1-imageIndex(1)) * cellSize;
-				pt.z = value;
-				pt.b = (unsigned char)(*color & 0xFF);
-				pt.g = (unsigned char)((*color >> 8) & 0xFF);
-				pt.r = (unsigned char)((*color >> 16) & 0xFF);
-			}
-			else
-			{
-				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
-			}
-		}
-	}
-	return cloud;
-}
-
-pcl::PolygonMesh::Ptr GridMap::createTerrainMesh() const
-{
-	pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTerrainCloud();
-	if(!cloud->empty())
-	{
-		mesh->polygons = util3d::organizedFastMesh(
-				cloud,
-				M_PI,
-				true,
-				1);
-
-		pcl::toPCLPointCloud2(*cloud, mesh->cloud);
-	}
-
-	return mesh;
-}
-
-void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
-{
-	UTimer timer;
-
-	float margin = cellSize_*10.0f;
-
-	float minX=-minMapSize_/2.0f;
-	float minY=-minMapSize_/2.0f;
-	float maxX=minMapSize_/2.0f;
-	float maxY=minMapSize_/2.0f;
-	bool undefinedSize = minMapSize_ == 0.0f;
-	std::map<int, cv::Mat> occupiedLocalMaps;
-
-	if(gridMap_.hasBasicLayers())
-	{
-		// update
-		minX=minValues_[0]+margin+cellSize_/2.0f;
-		minY=minValues_[1]+margin+cellSize_/2.0f;
-		maxX=minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - margin;
-		maxY=minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - margin;
-		undefinedSize = false;
-	}
-
-	for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
-	{
-		UASSERT(!iter->second.isNull());
-
-		float x = iter->second.x();
-		float y  =iter->second.y();
-		if(undefinedSize)
-		{
-			minX = maxX = x;
-			minY = maxY = y;
-			undefinedSize = false;
-		}
-		else
-		{
-			if(minX > x)
-				minX = x;
-			else if(maxX < x)
-				maxX = x;
-
-			if(minY > y)
-				minY = y;
-			else if(maxY < y)
-				maxY = y;
-		}
-	}
-
-	if(!cache().empty())
-	{
-		UDEBUG("Updating from cache");
-		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
-		{
-			if(uContains(cache(), iter->first))
-			{
-				const LocalGrid & localGrid = cache().at(iter->first);
-
-				if(!localGrid.is3D())
-				{
-					UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)",
-							localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type());
-					continue;
-				}
-
-				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
-
-				//ground
-				cv::Mat occupied;
-				if(localGrid.groundCells.cols || localGrid.obstacleCells.cols)
-				{
-					occupied = cv::Mat(1, localGrid.groundCells.cols+localGrid.obstacleCells.cols, CV_32FC4);
-				}
-				if(localGrid.groundCells.cols)
-				{
-					if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
-					}
-					for(int i=0; i<localGrid.groundCells.cols; ++i)
-					{
-						const float * vi = localGrid.groundCells.ptr<float>(0,i);
-						float * vo = occupied.ptr<float>(0,i);
-						cv::Point3f vt;
-						vo[3] = 0xFFFFFFFF; // RGBA
-						if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5)
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
-							if(localGrid.groundCells.channels() == 4)
-							{
-								vo[3] = vi[3];
-							}
-						}
-						else
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
-						}
-						vo[0] = vt.x;
-						vo[1] = vt.y;
-						vo[2] = vt.z;
-						if(minX > vo[0])
-							minX = vo[0];
-						else if(maxX < vo[0])
-							maxX = vo[0];
-
-						if(minY > vo[1])
-							minY = vo[1];
-						else if(maxY < vo[1])
-							maxY = vo[1];
-					}
-				}
-
-				//obstacles
-				if(localGrid.obstacleCells.cols)
-				{
-					if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
-					}
-					for(int i=0; i<localGrid.obstacleCells.cols; ++i)
-					{
-						const float * vi = localGrid.obstacleCells.ptr<float>(0,i);
-						float * vo = occupied.ptr<float>(0,i+localGrid.groundCells.cols);
-						cv::Point3f vt;
-						vo[3] = 0xFFFFFFFF; // RGBA
-						if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5)
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
-							if(localGrid.obstacleCells.channels() == 4)
-							{
-								vo[3] = vi[3];
-							}
-						}
-						else
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
-						}
-						vo[0] = vt.x;
-						vo[1] = vt.y;
-						vo[2] = vt.z;
-						if(minX > vo[0])
-							minX = vo[0];
-						else if(maxX < vo[0])
-							maxX = vo[0];
-
-						if(minY > vo[1])
-							minY = vo[1];
-						else if(maxY < vo[1])
-							maxY = vo[1];
-					}
-				}
-				uInsert(occupiedLocalMaps, std::make_pair(iter->first, occupied));
-			}
-		}
-	}
-
-	if(minX != maxX && minY != maxY)
-	{
-		//Get map size
-		float xMin = minX-margin;
-		xMin -= cellSize_/2.0f;
-		float yMin = minY-margin;
-		yMin -= cellSize_/2.0f;
-		float xMax = maxX+margin;
-		float yMax = maxY+margin;
-
-		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
-		   fabs((xMax - xMin) / cellSize_) > 99999)
-		{
-			UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
-					"There's maybe an error with the poses provided! The map will not be created!",
-					xMin, yMin, xMax, yMax);
-		}
-		else
-		{
-			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
-			cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
-			if(!gridMap_.hasBasicLayers())
-			{
-				UDEBUG("Map empty!");
-				grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
-				grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
-
-				UDEBUG("length: %f, %f position: %f, %f", length[0], length[1], position[0], position[1]);
-				gridMap_.setGeometry(length, cellSize_, position);
-				UDEBUG("size: %d, %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
-				// Add elevation layer
-				gridMap_.add("elevation");
-				gridMap_.add("node_ids");
-				gridMap_.add("colors");
-				gridMap_.setBasicLayers({"elevation"});
-			}
-			else
-			{
-				if(xMin == minValues_[0] && yMin == minValues_[1] &&
-						newMapSize.width == gridMap_.getSize()[0] &&
-						newMapSize.height == gridMap_.getSize()[1])
-				{
-					// same map size and origin, don't do anything
-					UDEBUG("Map same size!");
-				}
-				else
-				{
-					UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
-					UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
-					UASSERT_MSG(xMax >= minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], gridMap_.getSize()[0], cellSize_).c_str());
-					UASSERT_MSG(yMax >= minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], gridMap_.getSize()[1], cellSize_).c_str());
-
-					UDEBUG("Copy map");
-					// copy the old map in the new map
-					// make sure the translation is cellSize
-					int deltaX = 0;
-					if(xMin < minValues_[0])
-					{
-						deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f;
-						xMin = minValues_[0]-float(deltaX)*cellSize_;
-					}
-					int deltaY = 0;
-					if(yMin < minValues_[1])
-					{
-						deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f;
-						yMin = minValues_[1]-float(deltaY)*cellSize_;
-					}
-					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
-					newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
-					newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
-					UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], newMapSize.width, newMapSize.height);
-					UASSERT(newMapSize.width >= gridMap_.getSize()[0] && newMapSize.height >= gridMap_.getSize()[1]);
-					UASSERT(newMapSize.width >= gridMap_.getSize()[0]+deltaX && newMapSize.height >= gridMap_.getSize()[1]+deltaY);
-					UASSERT(deltaX>=0 && deltaY>=0);
-
-					grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
-					grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
-
-					grid_map::GridMap tmpExtendedMap;
-					tmpExtendedMap.setGeometry(length, cellSize_, position);
-
-					UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], tmpExtendedMap.getSize()[0], tmpExtendedMap.getSize()[1]);
-
-					UDEBUG("extendToInclude (%f,%f,%f,%f) -> (%f,%f,%f,%f)",
-							gridMap_.getLength()[0], gridMap_.getLength()[1],
-							gridMap_.getPosition()[0], gridMap_.getPosition()[1],
-							tmpExtendedMap.getLength()[0], tmpExtendedMap.getLength()[1],
-							tmpExtendedMap.getPosition()[0], tmpExtendedMap.getPosition()[1]);
-					if(!gridMap_.extendToInclude(tmpExtendedMap))
-					{
-						UERROR("Failed to update size of the grid map");
-					}
-					UDEBUG("Updated side: %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
-				}
-			}
-			UDEBUG("map %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
-			if(newPoses.size())
-			{
-				UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
-			}
-			grid_map::Matrix& gridMapData = gridMap_["elevation"];
-			grid_map::Matrix& gridMapNodeIds = gridMap_["node_ids"];
-			grid_map::Matrix& gridMapColors = gridMap_["colors"];
-			for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
-			{
-				std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(kter->first);
-				if(iter!=occupiedLocalMaps.end())
-				{
-					addAssembledNode(kter->first, kter->second);
-
-					for(int i=0; i<iter->second.cols; ++i)
-					{
-						float * ptf = iter->second.ptr<float>(0,i);
-						grid_map::Position position(ptf[0], ptf[1]);
-						grid_map::Index index;
-						if(gridMap_.getIndex(position, index))
-						{
-							// If no elevation has been set, use current elevation.
-							if (!gridMap_.isValid(index))
-							{
-								gridMapData(index(0), index(1)) = ptf[2];
-								gridMapNodeIds(index(0), index(1)) = kter->first;
-								gridMapColors(index(0), index(1)) = ptf[3];
-							}
-							else
-							{
-								if ((gridMapData(index(0), index(1)) < ptf[2] && (gridMapNodeIds(index(0), index(1)) <= kter->first || kter->first == -1)) ||
-									gridMapNodeIds(index(0), index(1)) < kter->first)
-								{
-									gridMapData(index(0), index(1)) = ptf[2];
-									gridMapNodeIds(index(0), index(1)) = kter->first;
-									gridMapColors(index(0), index(1)) = ptf[3];
-								}
-							}
-						}
-						else
-						{
-							UERROR("Outside map!? (%d) (%f,%f) -> (%d,%d)", i, ptf[0], ptf[1], index[0], index[1]);
-						}
-					}
-				}
-			}
-
-			minValues_[0] = xMin;
-			minValues_[1] = yMin;
-		}
-	}
-	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
-}
-
-}
@@ -1,682 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include <rtabmap/core/global_map/OccupancyGrid.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/util3d_filtering.h>
-#include <rtabmap/core/util3d_mapping.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UTimer.h>
-
-#include <pcl/io/pcd_io.h>
-
-namespace rtabmap {
-
-OccupancyGrid::OccupancyGrid(const LocalGridCache * cache, const ParametersMap & parameters) :
-	GlobalMap(cache, parameters),
-	minMapSize_(Parameters::defaultGridGlobalMinSize()),
-	erode_(Parameters::defaultGridGlobalEroded()),
-	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius())
-{
-	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
-	Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
-	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
-
-	UASSERT(minMapSize_ >= 0.0f);
-}
-
-void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses)
-{
-	UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d",
-			map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size());
-	this->clear();
-	if(!poses.empty() && !map.empty())
-	{
-		UASSERT(cellSize > 0.0f);
-		UASSERT(map.type() == CV_8SC1);
-		map_ = map.clone();
-		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
-		for(int i=0; i<map_.rows; ++i)
-		{
-			for(int j=0; j<map_.cols; ++j)
-			{
-				const char value = map_.at<char>(i,j);
-				float * info = mapInfo_.ptr<float>(i,j);
-				if(value == 0)
-				{
-					info[3] = logOddsClampingMin_;
-				}
-				else if(value == 100)
-				{
-					info[3] = logOddsClampingMax_;
-				}
-			}
-		}
-		minValues_[0] = xMin;
-		minValues_[1] = yMin;
-		cellSize_ = cellSize;
-		addAssembledNode(poses.lower_bound(1)->first, poses.lower_bound(1)->second);
-	}
-}
-
-void OccupancyGrid::clear()
-{
-	map_ = cv::Mat();
-	mapInfo_ = cv::Mat();
-	cellCount_.clear();
-	GlobalMap::clear();
-}
-
-cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
-{
-	xMin = minValues_[0];
-	yMin = minValues_[1];
-
-	cv::Mat map = map_;
-
-	UTimer t;
-	if(occupancyThr_ != 0.0f && !map.empty())
-	{
-		float occThr = logodds(occupancyThr_);
-		map = cv::Mat(map.size(), map.type());
-		UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
-		for(int i=0; i<map.rows; ++i)
-		{
-			for(int j=0; j<map.cols; ++j)
-			{
-				const float * info = mapInfo_.ptr<float>(i, j);
-				if(info[3] == 0.0f)
-				{
-					map.at<char>(i, j) = -1; // unknown
-				}
-				else if(info[3] >= occThr)
-				{
-					map.at<char>(i, j) = 100; // unknown
-				}
-				else
-				{
-					map.at<char>(i, j) = 0; // empty
-				}
-			}
-		}
-		UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
-	}
-
-	if(erode_ && !map.empty())
-	{
-		map = util3d::erodeMap(map);
-		UDEBUG("Eroding map = %fs", t.ticks());
-	}
-	return map;
-}
-
-cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
-{
-	xMin = minValues_[0];
-	yMin = minValues_[1];
-
-	cv::Mat map;
-	if(!mapInfo_.empty())
-	{
-		map = cv::Mat(mapInfo_.size(), map_.type());
-		for(int i=0; i<map.rows; ++i)
-		{
-			for(int j=0; j<map.cols; ++j)
-			{
-				const float * info = mapInfo_.ptr<float>(i, j);
-				if(info[3] == 0.0f)
-				{
-					map.at<char>(i, j) = -1; // unknown
-				}
-				else
-				{
-					map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
-				}
-			}
-		}
-	}
-	else
-	{
-		UWARN("Map info is empty, cannot generate probabilistic occupancy grid");
-	}
-	return map;
-}
-
-void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPoses)
-{
-	UTimer timer;
-
-	float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_;
-
-	float minX=-minMapSize_/2.0f;
-	float minY=-minMapSize_/2.0f;
-	float maxX=minMapSize_/2.0f;
-	float maxY=minMapSize_/2.0f;
-	bool undefinedSize = minMapSize_ == 0.0f;
-	std::map<int, cv::Mat> emptyLocalMaps;
-	std::map<int, cv::Mat> occupiedLocalMaps;
-
-	if(!map_.empty())
-	{
-		// update
-		minX=minValues_[0]+margin+cellSize_/2.0f;
-		minY=minValues_[1]+margin+cellSize_/2.0f;
-		maxX=minValues_[0]+float(map_.cols)*cellSize_ - margin;
-		maxY=minValues_[1]+float(map_.rows)*cellSize_ - margin;
-		undefinedSize = false;
-	}
-
-	for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
-	{
-		UASSERT(!iter->second.isNull());
-
-		float x = iter->second.x();
-		float y  =iter->second.y();
-		if(undefinedSize)
-		{
-			minX = maxX = x;
-			minY = maxY = y;
-			undefinedSize = false;
-		}
-		else
-		{
-			if(minX > x)
-				minX = x;
-			else if(maxX < x)
-				maxX = x;
-
-			if(minY > y)
-				minY = y;
-			else if(maxY < y)
-				maxY = y;
-		}
-	}
-
-	if(!cache().empty())
-	{
-		UDEBUG("Updating from cache");
-		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
-		{
-			if(uContains(cache(), iter->first))
-			{
-				const LocalGrid & localGrid = cache().at(iter->first);
-
-				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
-
-				//ground
-				cv::Mat ground;
-				if(localGrid.groundCells.cols || localGrid.emptyCells.cols)
-				{
-					ground = cv::Mat(1, localGrid.groundCells.cols+localGrid.emptyCells.cols, CV_32FC2);
-				}
-				if(localGrid.groundCells.cols)
-				{
-					if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
-					}
-					for(int i=0; i<localGrid.groundCells.cols; ++i)
-					{
-						const float * vi = localGrid.groundCells.ptr<float>(0,i);
-						float * vo = ground.ptr<float>(0,i);
-						cv::Point3f vt;
-						if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5)
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
-						}
-						else
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
-						}
-						vo[0] = vt.x;
-						vo[1] = vt.y;
-						if(minX > vo[0])
-							minX = vo[0];
-						else if(maxX < vo[0])
-							maxX = vo[0];
-
-						if(minY > vo[1])
-							minY = vo[1];
-						else if(maxY < vo[1])
-							maxY = vo[1];
-					}
-				}
-
-				//empty
-				if(localGrid.emptyCells.cols)
-				{
-					if(localGrid.emptyCells.rows > 1 && localGrid.emptyCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.emptyCells.rows, localGrid.emptyCells.cols);
-					}
-					for(int i=0; i<localGrid.emptyCells.cols; ++i)
-					{
-						const float * vi = localGrid.emptyCells.ptr<float>(0,i);
-						float * vo = ground.ptr<float>(0,i+localGrid.groundCells.cols);
-						cv::Point3f vt;
-						if(localGrid.emptyCells.channels() != 2 && localGrid.emptyCells.channels() != 5)
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
-						}
-						else
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
-						}
-						vo[0] = vt.x;
-						vo[1] = vt.y;
-						if(minX > vo[0])
-							minX = vo[0];
-						else if(maxX < vo[0])
-							maxX = vo[0];
-
-						if(minY > vo[1])
-							minY = vo[1];
-						else if(maxY < vo[1])
-							maxY = vo[1];
-					}
-				}
-				uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
-
-				//obstacles
-				if(localGrid.obstacleCells.cols)
-				{
-					if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
-					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
-					}
-					cv::Mat obstacles(1, localGrid.obstacleCells.cols, CV_32FC2);
-					for(int i=0; i<obstacles.cols; ++i)
-					{
-						const float * vi = localGrid.obstacleCells.ptr<float>(0,i);
-						float * vo = obstacles.ptr<float>(0,i);
-						cv::Point3f vt;
-						if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5)
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
-						}
-						else
-						{
-							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
-						}
-						vo[0] = vt.x;
-						vo[1] = vt.y;
-						if(minX > vo[0])
-							minX = vo[0];
-						else if(maxX < vo[0])
-							maxX = vo[0];
-
-						if(minY > vo[1])
-							minY = vo[1];
-						else if(maxY < vo[1])
-							maxY = vo[1];
-					}
-					uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
-				}
-			}
-		}
-	}
-
-	cv::Mat map;
-	cv::Mat mapInfo;
-	if(minX != maxX && minY != maxY)
-	{
-		//Get map size
-		float xMin = minX-margin;
-		xMin -= cellSize_/2.0f;
-		float yMin = minY-margin;
-		yMin -= cellSize_/2.0f;
-		float xMax = maxX+margin;
-		float yMax = maxY+margin;
-
-		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
-		   fabs((xMax - xMin) / cellSize_) > 99999)
-		{
-			UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
-					"There's maybe an error with the poses provided! The map will not be created!",
-					xMin, yMin, xMax, yMax);
-		}
-		else
-		{
-			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
-			cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
-			if(map_.empty())
-			{
-				UDEBUG("Map empty!");
-				map = cv::Mat::ones(newMapSize, CV_8S)*-1;
-				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
-			}
-			else
-			{
-				if(xMin == minValues_[0] && yMin == minValues_[1] &&
-						newMapSize.width == map_.cols &&
-						newMapSize.height == map_.rows)
-				{
-					// same map size and origin, don't do anything
-					UDEBUG("Map same size!");
-					map = map_;
-					mapInfo = mapInfo_;
-				}
-				else
-				{
-					UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
-					UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
-					UASSERT_MSG(xMax >= minValues_[0]+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], map_.cols, cellSize_).c_str());
-					UASSERT_MSG(yMax >= minValues_[1]+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], map_.rows, cellSize_).c_str());
-
-					UDEBUG("Copy map");
-					// copy the old map in the new map
-					// make sure the translation is cellSize
-					int deltaX = 0;
-					if(xMin < minValues_[0])
-					{
-						deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f;
-						xMin = minValues_[0]-float(deltaX)*cellSize_;
-					}
-					int deltaY = 0;
-					if(yMin < minValues_[1])
-					{
-						deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f;
-						yMin = minValues_[1]-float(deltaY)*cellSize_;
-					}
-					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
-					newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
-					newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
-					UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
-					UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
-					UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
-					UASSERT(deltaX>=0 && deltaY>=0);
-					map = cv::Mat::ones(newMapSize, CV_8S)*-1;
-					mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
-					map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
-					mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
-				}
-			}
-			UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
-			UDEBUG("map %d %d", map.cols, map.rows);
-			if(newPoses.size())
-			{
-				UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
-			}
-			for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
-			{
-				std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
-				std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
-				if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
-				{
-					addAssembledNode(kter->first, kter->second);
-					std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
-					if(cter == cellCount_.end() && kter->first > 0)
-					{
-						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)
-						{
-							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",
-											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).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.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] += logOddsMiss_;
-									if (info[3] < logOddsClampingMin_)
-									{
-										info[3] = logOddsClampingMin_;
-									}
-									if (info[3] > logOddsClampingMax_)
-									{
-										info[3] = logOddsClampingMax_;
-									}
-								}
-							}
-						}
-					}
-
-					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)
-						{
-							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] = logOddsClampingMin_;
-									cter->second.first+=1;
-								}
-								value = -2; // free space (footprint)
-							}
-						}
-					}
-
-					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",
-												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).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] += logOddsHit_;
-									if (info[3] < logOddsClampingMin_)
-									{
-										info[3] = logOddsClampingMin_;
-									}
-									if (info[3] > logOddsClampingMax_)
-									{
-										info[3] = logOddsClampingMax_;
-									}
-								}
-
-								value = 100; // obstacles
-							}
-						}
-					}
-				}
-			}
-
-			if(footprintRadius_ >= cellSize_*1.5f)
-			{
-				for(int i=1; i<map.rows-1; ++i)
-				{
-					for(int j=1; j<map.cols-1; ++j)
-					{
-						char & value = map.at<char>(i, j);
-						if(value == -2)
-						{
-							value = 0;
-						}
-					}
-				}
-			}
-
-			map_ = map;
-			mapInfo_ = mapInfo;
-			minValues_[0] = xMin;
-			minValues_[1] = yMin;
-
-			// clean cellCount_
-			for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
-			{
-				UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
-				if(iter->second.first == 0 && iter->second.second == 0)
-				{
-					cellCount_.erase(iter++);
-				}
-				else
-				{
-					++iter;
-				}
-			}
-		}
-	}
-
-	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
-}
-
-unsigned long OccupancyGrid::getMemoryUsed() const
-{
-	unsigned long memoryUsage = GlobalMap::getMemoryUsed();
-
-	memoryUsage += map_.total() * map_.elemSize();
-	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
-	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
-
-	return memoryUsage;
-}
-
-}
@@ -513,28 +513,16 @@ public:
 			for (int i = 0; i < pointsCount; ++i)
 			{
 				float minDistance = std::numeric_limits<float>::max();
-				bool minDistFound = false;
 				for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
 				{
-					int matchesIdsCoeff = matches.ids.coeff(k, i);
-					if (matchesIdsCoeff!=-1)
+					float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
+					if(distIntensity < minDistance)
 					{
-						float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matchesIdsCoeff));
-						if(distIntensity < minDistance)
-						{
-							matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
-							matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
-							minDistance = distIntensity;
-							minDistFound = true;
-						}
+						matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
+						matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
+						minDistance = distIntensity;
 					}
 				}
-
-				if (!minDistFound)
-				{
-					matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(0, i);
-					matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(0, i);
-				}
 			}
 			matches = matchesOrderedByIntensity;
 		}
@@ -74,15 +74,15 @@ Transform OdometryF2F::computeTransform(
 	UTimer timer;
 	Transform output;
 	if(!data.rightRaw().empty() &&
-	   (data.stereoCameraModels().empty() || !data.stereoCameraModels()[0].isValidForProjection()))
+	   (data.stereoCameraModels().size() != 1 || !data.stereoCameraModels()[0].isValidForProjection()))
 	{
-		UERROR("Calibrated stereo camera required.");
+		UERROR("Calibrated stereo camera required (multi-cameras not supported)");
 		return output;
 	}
 	if(!data.depthRaw().empty() &&
-		(data.cameraModels().empty() || !data.cameraModels()[0].isValidForProjection()))
+		(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValidForProjection()))
 	{
-		UERROR("Calibrated camera required.");
+		UERROR("Calibrated camera required (multi-cameras not supported).");
 		return output;
 	}

@@ -672,17 +672,18 @@ public:
 		T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
 		T_i_w.translation() = imu_state.position;

-		Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse();
+		Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w *
+				msckf_vio::IMUState::T_imu_body.inverse();
 		Eigen::Vector3d body_velocity =
 				msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;

 		// Publish tf
 		/*if (publish_tf) {
-			tf::Transform T_b_w_tf;
-			tf::transformEigenToTF(T_b_w, T_b_w_tf);
-			tf_pub.sendTransform(tf::StampedTransform(
-				T_b_w_tf, time, fixed_frame_id, child_frame_id));
-		}*/
+	    tf::Transform T_b_w_tf;
+	    tf::transformEigenToTF(T_b_w, T_b_w_tf);
+	    tf_pub.sendTransform(tf::StampedTransform(
+	          T_b_w_tf, time, fixed_frame_id, child_frame_id));
+	  }*/

 		// Publish the odometry
 		nav_msgs::Odometry odom_msg;
@@ -724,18 +725,20 @@ public:
 		// Publish the 3D positions of the features that
 		// has been initialized.
 		feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
-		feature_msg_ptr->header.frame_id = fixed_frame_id;
-		feature_msg_ptr->height = 1;
-		for (const auto& item : map_server) {
-			const auto& feature = item.second;
-			if (feature.is_initialized) {
-				feature_msg_ptr->points.push_back(pcl::PointXYZ(
-					feature.position(0), feature.position(1), feature.position(2)));
-			}
-		}
-		feature_msg_ptr->width = feature_msg_ptr->points.size();
+	  feature_msg_ptr->header.frame_id = fixed_frame_id;
+	  feature_msg_ptr->height = 1;
+	  for (const auto& item : map_server) {
+	    const auto& feature = item.second;
+	    if (feature.is_initialized) {
+	      Eigen::Vector3d feature_position =
+	        msckf_vio::IMUState::T_imu_body.linear() * feature.position;
+	      feature_msg_ptr->points.push_back(pcl::PointXYZ(
+	            feature_position(0), feature_position(1), feature_position(2)));
+	    }
+	  }
+	  feature_msg_ptr->width = feature_msg_ptr->points.size();

-		// feature_pub.publish(feature_msg_ptr);
+	  //feature_pub.publish(feature_msg_ptr);

 		return odom_msg;
 	}
@@ -752,7 +755,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
 imageProcessor_(0),
 msckf_(0),
 parameters_(parameters),
-fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0),
+fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0),
 previousPose_(Transform::getIdentity()),
 initGravity_(false)
 #endif
@@ -34,21 +34,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UDirectory.h"
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/types_c.h>
-#include <rtabmap/core/odometry/OdometryORBSLAM2.h>
+#include <rtabmap/core/odometry/OdometryORBSLAM.h>

-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+#ifdef RTABMAP_ORB_SLAM
 #include <System.h>
 #include <thread>

 using namespace std;

+#if RTABMAP_ORB_SLAM == 3
+namespace ORB_SLAM3 {
+#else
 namespace ORB_SLAM2 {
-
+#endif
 // Override original Tracking object to comment all rendering stuff
 class Tracker: public Tracking
 {
 public:
+#if RTABMAP_ORB_SLAM == 3
+	Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Atlas* pMap,
+#else
 	Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
+#endif
 	             KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor, long unsigned int maxFeatureMapSize) :
 	            	 Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor),
 	            	 maxFeatureMapSize_(maxFeatureMapSize)
@@ -60,6 +67,9 @@ private:
 protected:
 	void Track()
 	{
+#if RTABMAP_ORB_SLAM == 3
+		Map* mpMap = mpAtlas->GetCurrentMap();
+#endif
 	    if(mState==NO_IMAGES_YET)
 	    {
 	        mState = NOT_INITIALIZED;
@@ -81,8 +91,17 @@ protected:

 	        if(mState!=OK)
 	        {
+#if RTABMAP_ORB_SLAM == 3
+	        	mLastFrame = Frame(mCurrentFrame);
+#endif
 	            return;
 	        }
+#if RTABMAP_ORB_SLAM == 3
+	        if(mpAtlas->GetAllMaps().size() == 1)
+			{
+				mnFirstFrameId = mCurrentFrame.mnId;
+			}
+#endif
 	    }
 	    else
 	    {
@@ -365,6 +384,9 @@ protected:
 	        // Set Frame pose to the origin
 	        mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));

+#if RTABMAP_ORB_SLAM == 3
+			Map* mpMap = mpAtlas->GetCurrentMap();
+#endif
 	        // Create KeyFrame
 	        KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

@@ -462,9 +484,11 @@ public:
 				cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
 			}
 		}
-
+#if RTABMAP_ORB_SLAM == 3
+	    mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera);
+#else
 	    mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
-
+#endif
 	    Track();

 	    return mCurrentFrame.mTcw.clone();
@@ -492,8 +516,11 @@ public:

 	    UASSERT(imDepth.type()==CV_32F);

+#if RTABMAP_ORB_SLAM == 3
+	    mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera);
+#else
 	    mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
-
+#endif
 	    Track();

 	    return mCurrentFrame.mTcw.clone();
@@ -504,7 +531,11 @@ public:
 class LoopCloser: public LoopClosing
 {
 public:
+#if RTABMAP_ORB_SLAM == 3
+	LoopCloser(Atlas* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) :
+#else
 	LoopCloser(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) :
+#endif
 		LoopClosing(pMap, pDB, pVoc, bFixScale)
 	{}

@@ -535,12 +566,16 @@ public:

 } // namespace ORB_SLAM

+#if RTABMAP_ORB_SLAM == 3
+using namespace ORB_SLAM3;
+#else
 using namespace ORB_SLAM2;
+#endif

-class ORBSLAM2System
+class ORBSLAMSystem
 {
 public:
-	ORBSLAM2System(const rtabmap::ParametersMap & parameters) :
+	ORBSLAMSystem(const rtabmap::ParametersMap & parameters) :
 		mpVocabulary(0),
 		mpKeyFrameDatabase(0),
 		mpMap(0),
@@ -578,7 +613,7 @@ public:
 		}
 	}

-	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline)
+	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline, const rtabmap::Transform & localIMUTransform)
 	{
 		if(!mpVocabulary)
 		{
@@ -674,6 +709,31 @@ public:
 		ofs << "DepthMapFactor: " << 1000.0 << std::endl;
 		ofs << std::endl;

+		if(!localIMUTransform.isNull())
+		{
+			//#--------------------------------------------------------------------------------------------
+			//# IMU Parameters TODO: hard-coded, not used
+			//#--------------------------------------------------------------------------------------------
+			//  Transformation from camera 0 to body-frame (imu)
+			rtabmap::Transform camImuT = model.localTransform()*localIMUTransform;
+			ofs << "Tbc: !!opencv-matrix" << std::endl;
+			ofs << "   rows: 4" << std::endl;
+			ofs << "   cols: 4" << std::endl;
+			ofs << "   dt: f" << std::endl;
+			ofs << "   data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2]  << ", " << camImuT.data()[3]  << ", " << std::endl;
+			ofs << "         "  << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6]  << ", " << camImuT.data()[7]  << ", " << std::endl;
+			ofs << "         "  << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl;
+			ofs << "         0.0, 0.0, 0.0, 1.0]" << std::endl;
+			ofs << std::endl;
+
+			ofs << "IMU.NoiseGyro: " << 1.7e-4 << std::endl;
+			ofs << "IMU.NoiseAcc: " << 2.0e-3 << std::endl;
+			ofs << "IMU.GyroWalk: " << 1.9393e-5 << std::endl;
+			ofs << "IMU.AccWalk: " << 3.e-3 << std::endl;
+			ofs << "IMU.Frequency: " << 200 << std::endl;
+			ofs << std::endl;
+		}
+
 		//#--------------------------------------------------------------------------------------------
 		//# ORB Parameters
 		//#--------------------------------------------------------------------------------------------
@@ -716,15 +776,22 @@ public:
 		mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

 		//Create the Map
+#if RTABMAP_ORB_SLAM == 3
+		mpMap = new Atlas(0);
+#else
 		mpMap = new ORB_SLAM2::Map();
+#endif

 		//Initialize the Tracking thread
 		//(it will live in the main thread of execution, the one that called this constructor)
 		mpTracker = new Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?System::STEREO:System::RGBD, maxFeatureMapSize);

 		//Initialize the Local Mapping thread and launch
+#if RTABMAP_ORB_SLAM == 3
+		mpLocalMapper = new LocalMapping(0, mpMap, false, stereo && !localIMUTransform.isNull());
+#else
 		mpLocalMapper = new LocalMapping(mpMap, false);
-
+#endif
 		//Initialize the Loop Closing thread and launch
 		mpLoopCloser = new LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);

@@ -745,10 +812,17 @@ public:
 		// Reset all static variables
 		Frame::mbInitialComputations = true;

+#if RTABMAP_ORB_SLAM == 3
+		if(ULogger::level() > ULogger::kInfo)
+			Verbose::SetTh(Verbose::VERBOSITY_QUIET);
+
+		mpTracker->Reset(true);
+#endif
+
 		return true;
 	}

-	virtual ~ORBSLAM2System()
+	virtual ~ORBSLAMSystem()
 	{
 		shutdown();
 		delete mpVocabulary;
@@ -795,7 +869,11 @@ public:
 	KeyFrameDatabase* mpKeyFrameDatabase;

 	// Map structure that stores the pointers to all KeyFrames and MapPoints.
+#if RTABMAP_ORB_SLAM == 3
+	Atlas* mpMap;
+#else
 	Map* mpMap;
+#endif

 	// Tracker. It receives a frame and computes the associated camera pose.
 	// It also decides when to insert a new keyframe, create some new MapPoints and
@@ -820,23 +898,24 @@ public:

 namespace rtabmap {

-OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
+OdometryORBSLAM::OdometryORBSLAM(const ParametersMap & parameters) :
 	Odometry(parameters)
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+#ifdef RTABMAP_ORB_SLAM
    ,
 	orbslam_(0),
 	firstFrame_(true),
-	previousPose_(Transform::getIdentity())
+	previousPose_(Transform::getIdentity()),
+	useIMU_(false) // TODO: Not yet supported with ORB_SLAM3
 #endif
 {
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
-	orbslam_ = new ORBSLAM2System(parameters);
+#ifdef RTABMAP_ORB_SLAM
+	orbslam_ = new ORBSLAMSystem(parameters);
 #endif
 }

-OdometryORBSLAM2::~OdometryORBSLAM2()
+OdometryORBSLAM::~OdometryORBSLAM()
 {
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+#ifdef RTABMAP_ORB_SLAM
 	if(orbslam_)
 	{
 		delete orbslam_;
@@ -844,10 +923,10 @@ OdometryORBSLAM2::~OdometryORBSLAM2()
 #endif
 }

-void OdometryORBSLAM2::reset(const Transform & initialPose)
+void OdometryORBSLAM::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+#ifdef RTABMAP_ORB_SLAM
 	if(orbslam_)
 	{
 		orbslam_->shutdown();
@@ -855,20 +934,60 @@ void OdometryORBSLAM2::reset(const Transform & initialPose)
 	firstFrame_ = true;
 	originLocalTransform_.setNull();
 	previousPose_.setIdentity();
+	imuLocalTransform_.setNull();
+#endif
+}
+
+bool OdometryORBSLAM::canProcessAsyncIMU() const
+{
+#ifdef RTABMAP_ORB_SLAM
+	return useIMU_;
+#else
+	return false;
 #endif
 }

 // return not null transform if odometry is correctly computed
-Transform OdometryORBSLAM2::computeTransform(
+Transform OdometryORBSLAM::computeTransform(
 		SensorData & data,
 		const Transform & guess,
 		OdometryInfo * info)
 {
 	Transform t;

-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+#ifdef RTABMAP_ORB_SLAM
 	UTimer timer;

+#if RTABMAP_ORB_SLAM == 3
+	if(useIMU_)
+	{
+		if(orbslam_->mpTracker == 0)
+		{
+			if(!data.imu().empty())
+			{
+				imuLocalTransform_ = data.imu().localTransform();
+			}
+		}
+		else if(!data.imu().empty())
+		{
+			ORB_SLAM3::IMU::Point pt(
+					data.imu().linearAcceleration().val[0],
+					data.imu().linearAcceleration().val[1],
+					data.imu().linearAcceleration().val[2],
+					data.imu().angularVelocity().val[0],
+					data.imu().angularVelocity().val[1],
+					data.imu().angularVelocity().val[2],
+					data.stamp());
+			orbslam_->mpTracker->GrabImuData(pt);
+		}
+
+		if(data.imageRaw().empty() || imuLocalTransform_.isNull())
+		{
+			return Transform();
+		}
+	}
+#endif
+
 	if(data.imageRaw().empty() ||
 		data.imageRaw().rows != data.depthOrRightRaw().rows ||
 		data.imageRaw().cols != data.depthOrRightRaw().cols)
@@ -888,12 +1007,18 @@ Transform OdometryORBSLAM2::computeTransform(
 	}

 	bool stereo = data.cameraModels().size() == 0;
+	if(!stereo && useIMU_)
+	{
+		UWARN("Disabling IMU support (ORB_SLAM3 doesn't support IMU with RGB-D mode).");
+		useIMU_ = false;
+		imuLocalTransform_.setNull();
+	}

 	cv::Mat covariance;
 	if(orbslam_->mpTracker == 0)
 	{
 		CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left();
-		if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline()))
+		if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline(), imuLocalTransform_))
 		{
 			return t;
 		}
@@ -1,589 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include "rtabmap/core/OdometryInfo.h"
-#include "rtabmap/core/util2d.h"
-#include "rtabmap/core/util3d_transforms.h"
-#include "rtabmap/utilite/ULogger.h"
-#include "rtabmap/utilite/UTimer.h"
-#include "rtabmap/utilite/UStl.h"
-#include "rtabmap/utilite/UDirectory.h"
-#include <pcl/common/transforms.h>
-#include <opencv2/imgproc/types_c.h>
-#include <rtabmap/core/odometry/OdometryORBSLAM3.h>
-
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-#include <thread>
-#include <Converter.h>
-
-using namespace std;
-
-#endif
-
-namespace rtabmap {
-
-OdometryORBSLAM3::OdometryORBSLAM3(const ParametersMap & parameters) :
-	Odometry(parameters)
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-    ,
-	orbslam_(0),
-	firstFrame_(true),
-	previousPose_(Transform::getIdentity()),
-	useIMU_(Parameters::defaultOdomORBSLAMInertial()),
-	parameters_(parameters),
-	lastImuStamp_(0.0),
-	lastImageStamp_(0.0)
-#endif
-{
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-	Parameters::parse(parameters, Parameters::kOdomORBSLAMInertial(), useIMU_);
-#endif
-}
-
-OdometryORBSLAM3::~OdometryORBSLAM3()
-{
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-	if(orbslam_)
-	{
-		orbslam_->Shutdown();
-		delete orbslam_;
-	}
-#endif
-}
-
-void OdometryORBSLAM3::reset(const Transform & initialPose)
-{
-	Odometry::reset(initialPose);
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-	if(orbslam_)
-	{
-		orbslam_->Shutdown();
-		delete orbslam_;
-		orbslam_=0;
-	}
-	firstFrame_ = true;
-	originLocalTransform_.setNull();
-	previousPose_.setIdentity();
-	imuLocalTransform_.setNull();
-	lastImuStamp_ = 0.0;
-	lastImageStamp_ = 0.0;
-#endif
-}
-
-bool OdometryORBSLAM3::canProcessAsyncIMU() const
-{
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-	return useIMU_;
-#else
-	return false;
-#endif
-}
-
-bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::CameraModel & model2, double stamp,  bool stereo, double baseline)
-{
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-	std::string vocabularyPath;
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMVocPath(), vocabularyPath);
-
-	if(vocabularyPath.empty())
-	{
-		UERROR("ORB_SLAM vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
-		return false;
-	}
-	//Load ORB Vocabulary
-	vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
-	UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
-
-	// Create configuration file
-	std::string workingDir;
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir);
-	if(workingDir.empty())
-	{
-		workingDir = ".";
-	}
-	std::string configPath = workingDir+"/rtabmap_orbslam.yaml";
-	std::ofstream ofs (configPath, std::ofstream::out);
-	ofs << "%YAML:1.0" << std::endl;
-	ofs << std::endl;
-
-	ofs << "File.version: \"1.0\"" << std::endl;
-	ofs << std::endl;
-
-	ofs << "Camera.type: \"PinHole\"" << std::endl;
-	ofs << std::endl;
-
-	ofs << fixed << setprecision(13);
-
-	for(int i=1; i<(stereo?3:2); ++i)
-	{
-		const CameraModel & model = i==1?model1:model2;
-
-		//# Camera calibration and distortion parameters (OpenCV)
-		ofs << "Camera" << i << ".fx: " << model.fx() << std::endl;
-		ofs << "Camera" << i << ".fy: " << model.fy() << std::endl;
-		ofs << "Camera" << i << ".cx: " << model.cx() << std::endl;
-		ofs << "Camera" << i << ".cy: " << model.cy() << std::endl;
-		ofs << std::endl;
-
-		if(model.D().cols < 4)
-		{
-			ofs << "Camera" << i << ".k1: " << 0.0 << std::endl;
-			ofs << "Camera" << i << ".k2: " << 0.0 << std::endl;
-			ofs << "Camera" << i << ".p1: " << 0.0 << std::endl;
-			ofs << "Camera" << i << ".p2: " << 0.0 << std::endl;
-			if(!stereo)
-			{
-				ofs << "Camera" << i << ".k3: " << 0.0 << std::endl;
-			}
-		}
-		if(model.D().cols >= 4)
-		{
-			ofs << "Camera" << i << ".k1: " << model.D().at<double>(0,0) << std::endl;
-			ofs << "Camera" << i << ".k2: " << model.D().at<double>(0,1) << std::endl;
-			ofs << "Camera" << i << ".p1: " << model.D().at<double>(0,2) << std::endl;
-			ofs << "Camera" << i << ".p2: " << model.D().at<double>(0,3) << std::endl;
-		}
-		if(model.D().cols >= 5)
-		{
-			ofs << "Camera" << i << ".k3: " << model.D().at<double>(0,4) << std::endl;
-		}
-		if(model.D().cols > 5)
-		{
-			UWARN("Unhandled camera distortion size %d, only 5 first coefficients used", model.D().cols);
-		}
-		ofs << std::endl;
-	}
-
-	//# IR projector baseline times fx (aprox.)
-	if(baseline <= 0.0)
-	{
-		baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
-	}
-	else
-	{
-		// # Transformation matrix from right camera to left camera
-		ofs << "Stereo.T_c1_c2: !!opencv-matrix" << std::endl;
-		ofs << "   rows: 4" << std::endl;
-		ofs << "   cols: 4" << std::endl;
-		ofs << "   dt: f" << std::endl;
-		ofs << "   data: [" << 1 << ", " << 0 << ", " << 0  << ", " << baseline  << ", " << std::endl;
-		ofs << "         "  << 0 << ", " << 1 << ", " << 0  << ", " << 0  << ", " << std::endl;
-		ofs << "         "  << 0 << ", " << 0 << ", " << 1 << ", " << 0 << ", " << std::endl;
-		ofs << "         0.0, 0.0, 0.0, 1.0]" << std::endl;
-		ofs << std::endl;
-	}
-	ofs << "Camera.bf: " << model1.fx()*baseline << std::endl;
-
-	ofs << "Camera.width: " << model1.imageWidth() << std::endl;
-	ofs << "Camera.height: " << model1.imageHeight() << std::endl;
-	ofs << std::endl;
-
-	//# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
-	//Camera.RGB: 1
-	ofs << "Camera.RGB: 0" << std::endl;
-	ofs << std::endl;
-
-	float fps = rtabmap::Parameters::defaultOdomORBSLAMFps();
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMFps(), fps);
-	if(fps == 0)
-	{
-		UASSERT(stamp > lastImageStamp_);
-		fps = std::round(1./(stamp - lastImageStamp_));
-		UWARN("Camera FPS estimated at %d Hz. If this doesn't look good, "
-			  "set explicitly parameter %s to expected frequency.",
-			  int(fps), Parameters::kOdomORBSLAMFps().c_str());
-	}
-	ofs << "Camera.fps: " << (int)fps << std::endl;
-	ofs << std::endl;
-
-	//# Close/Far threshold. Baseline times.
-	double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth();
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMThDepth(), thDepth);
-	ofs << "Stereo.ThDepth: " << thDepth << std::endl;
-	ofs << "Stereo.b: " << baseline << std::endl;
-	ofs << std::endl;
-
-	//# Deptmap values factor
-	ofs << "RGBD.DepthMapFactor: " << 1.0 << std::endl;
-	ofs << std::endl;
-
-	bool withIMU = false;
-	if(!imuLocalTransform_.isNull())
-	{
-		withIMU = true;
-		//#--------------------------------------------------------------------------------------------
-		//# IMU Parameters TODO: hard-coded, not used
-		//#--------------------------------------------------------------------------------------------
-		//  Transformation from camera 0 to body-frame (imu)
-		rtabmap::Transform camImuT = model1.localTransform()*imuLocalTransform_;
-		ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl;
-		ofs << "   rows: 4" << std::endl;
-		ofs << "   cols: 4" << std::endl;
-		ofs << "   dt: f" << std::endl;
-		ofs << "   data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2]  << ", " << camImuT.data()[3]  << ", " << std::endl;
-		ofs << "         "  << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6]  << ", " << camImuT.data()[7]  << ", " << std::endl;
-		ofs << "         "  << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl;
-		ofs << "         0.0, 0.0, 0.0, 1.0]" << std::endl;
-		ofs << std::endl;
-
-		ofs << "IMU.InsertKFsWhenLost: " << 0 << std::endl;
-		ofs << std::endl;
-
-		double gyroNoise = rtabmap::Parameters::defaultOdomORBSLAMGyroNoise();
-		double accNoise = rtabmap::Parameters::defaultOdomORBSLAMAccNoise();
-		double gyroWalk = rtabmap::Parameters::defaultOdomORBSLAMGyroWalk();
-		double accWalk = rtabmap::Parameters::defaultOdomORBSLAMAccWalk();
-		double samplingRate = rtabmap::Parameters::defaultOdomORBSLAMSamplingRate();
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroNoise(), gyroNoise);
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccNoise(), accNoise);
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroWalk(), gyroWalk);
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccWalk(), accWalk);
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMSamplingRate(), samplingRate);
-
-		ofs << "IMU.NoiseGyro: " << gyroNoise << std::endl; // 1e-2
-		ofs << "IMU.NoiseAcc: " << accNoise << std::endl; // 1e-1
-		ofs << "IMU.GyroWalk: " << gyroWalk << std::endl; // 1e-6
-		ofs << "IMU.AccWalk: " << accWalk << std::endl; // 1e-4
-		if(samplingRate == 0)
-		{
-			// estimate rate from imu received.
-			UASSERT(orbslamImus_.size() > 1 && orbslamImus_[0].t < orbslamImus_[1].t);
-			samplingRate = 1./(orbslamImus_[1].t - orbslamImus_[0].t);
-			samplingRate = std::round(samplingRate);
-			UWARN("IMU sampling rate estimated at %.0f Hz. If this doesn't look good, "
-				  "set explicitly parameter %s to expected frequency.",
-				  samplingRate,	Parameters::kOdomORBSLAMSamplingRate().c_str());
-		}
-		ofs << "IMU.Frequency: " << samplingRate << std::endl; // 200
-		ofs << std::endl;
-	}
-
-
-
-	//#--------------------------------------------------------------------------------------------
-	//# ORB Parameters
-	//#--------------------------------------------------------------------------------------------
-	//# ORB Extractor: Number of features per image
-	int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures();
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMaxFeatures(), features);
-	ofs << "ORBextractor.nFeatures: " << features << std::endl;
-	ofs << std::endl;
-
-	//# ORB Extractor: Scale factor between levels in the scale pyramid
-	double scaleFactor = rtabmap::Parameters::defaultORBScaleFactor();
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBScaleFactor(), scaleFactor);
-	ofs << "ORBextractor.scaleFactor: " << scaleFactor << std::endl;
-	ofs << std::endl;
-
-	//# ORB Extractor: Number of levels in the scale pyramid
-	int levels = rtabmap::Parameters::defaultORBNLevels();
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBNLevels(), levels);
-	ofs << "ORBextractor.nLevels: " << levels << std::endl;
-	ofs << std::endl;
-
-	//# ORB Extractor: Fast threshold
-	//# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
-	//# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
-	//# You can lower these values if your images have low contrast
-	int iniThFAST = rtabmap::Parameters::defaultFASTThreshold();
-	int minThFAST = rtabmap::Parameters::defaultFASTMinThreshold();
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTThreshold(), iniThFAST);
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTMinThreshold(), minThFAST);
-	ofs << "ORBextractor.iniThFAST: " << iniThFAST << std::endl;
-	ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
-	ofs << std::endl;
-
-	int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize();
-	rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMapSize(), maxFeatureMapSize);
-
-	//# Disable loop closure detection
-	ofs << "loopClosing: " << 0 << std::endl;
-	ofs << std::endl;
-
-	//# Set dummy Viewer parameters
-	ofs << "Viewer.KeyFrameSize: " << 0.05 << std::endl;
-	ofs << "Viewer.KeyFrameLineWidth: " << 1.0 << std::endl;
-	ofs << "Viewer.GraphLineWidth: " << 0.9 << std::endl;
-	ofs << "Viewer.PointSize: " << 2.0 << std::endl;
-	ofs << "Viewer.CameraSize: " << 0.08 << std::endl;
-	ofs << "Viewer.CameraLineWidth: " << 3.0 << std::endl;
-	ofs << "Viewer.ViewpointX: " << 0.0 << std::endl;
-	ofs << "Viewer.ViewpointY: " << -0.7 << std::endl;
-	ofs << "Viewer.ViewpointZ: " << -3.5 << std::endl;
-	ofs << "Viewer.ViewpointF: " << 500.0 << std::endl;
-	ofs << std::endl;
-
-	ofs.close();
-
-	orbslam_ = new ORB_SLAM3::System(
-			vocabularyPath,
-			configPath,
-			stereo && withIMU?ORB_SLAM3::System::IMU_STEREO:
-			stereo?ORB_SLAM3::System::STEREO:
-			withIMU?ORB_SLAM3::System::IMU_RGBD:
-					ORB_SLAM3::System::RGBD,
-			false);
-	return true;
-#else
-	UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
-#endif
-	return false;
-}
-
-// return not null transform if odometry is correctly computed
-Transform OdometryORBSLAM3::computeTransform(
-		SensorData & data,
-		const Transform & guess,
-		OdometryInfo * info)
-{
-	Transform t;
-
-#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
-	UTimer timer;
-
-	if(useIMU_)
-	{
-		bool added = false;
-		if(!data.imu().empty())
-		{
-			if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
-			{
-				orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
-						data.imu().linearAcceleration().val[0],
-						data.imu().linearAcceleration().val[1],
-						data.imu().linearAcceleration().val[2],
-						data.imu().angularVelocity().val[0],
-						data.imu().angularVelocity().val[1],
-						data.imu().angularVelocity().val[2],
-						data.stamp()));
-				lastImuStamp_ = data.stamp();
-				added = true;
-			}
-			else
-			{
-				UERROR("Received IMU with stamp (%f) <= than the previous IMU (%f), ignoring it!", data.stamp(), lastImuStamp_);
-			}
-		}
-
-		if(orbslam_ == 0)
-		{
-			// We need two samples to estimate imu frame rate
-			if(orbslamImus_.size()>1 && added)
-			{
-				imuLocalTransform_ = data.imu().localTransform();
-			}
-		}
-
-		if(data.imageRaw().empty() || imuLocalTransform_.isNull())
-		{
-			return Transform();
-		}
-	}
-
-	if(data.imageRaw().empty() ||
-		data.imageRaw().rows != data.depthOrRightRaw().rows ||
-		data.imageRaw().cols != data.depthOrRightRaw().cols)
-	{
-		UERROR("Not supported input! RGB (%dx%d) and depth (%dx%d) should have the same size.",
-				data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
-		return t;
-	}
-
-	if(!((data.cameraModels().size() == 1 &&
-			data.cameraModels()[0].isValidForReprojection()) ||
-		(data.stereoCameraModels().size() == 1 &&
-			data.stereoCameraModels()[0].isValidForProjection())))
-	{
-		UERROR("Invalid camera model!");
-		return t;
-	}
-
-	bool stereo = data.cameraModels().size() == 0;
-
-	cv::Mat covariance;
-	if(orbslam_ == 0)
-	{
-		// We need two frames to estimate camera frame rate
-		if(lastImageStamp_ == 0.0)
-		{
-			lastImageStamp_ = data.stamp();
-			return t;
-		}
-
-		CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left();
-		if(!init(model, !stereo?model:data.stereoCameraModels()[0].right(), data.stamp(), stereo, data.cameraModels().size()==1?0.0:data.stereoCameraModels()[0].baseline()))
-		{
-			return t;
-		}
-	}
-
-	Sophus::SE3f Tcw;
-	Transform localTransform;
-	if(stereo)
-	{
-		localTransform = data.stereoCameraModels()[0].localTransform();
-
-		Tcw = orbslam_->TrackStereo(data.imageRaw(), data.rightRaw(), data.stamp(), orbslamImus_);
-		orbslamImus_.clear();
-	}
-	else
-	{
-		localTransform = data.cameraModels()[0].localTransform();
-		cv::Mat depth;
-		if(data.depthRaw().type() == CV_32FC1)
-		{
-			depth = data.depthRaw();
-		}
-		else if(data.depthRaw().type() == CV_16UC1)
-		{
-			depth = util2d::cvtDepthToFloat(data.depthRaw());
-		}
-		Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_);
-		orbslamImus_.clear();
-	}
-
-	Transform previousPoseInv = previousPose_.inverse();
-	std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints();
-	if(orbslam_->isLost() || mapPoints.empty())
-	{
-		covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
-	}
-	else
-	{
-		cv::Mat TcwMat = ORB_SLAM3::Converter::toCvMat(ORB_SLAM3::Converter::toSE3Quat(Tcw)).clone();
-		UASSERT(TcwMat.cols == 4 && TcwMat.rows == 4);
-		Transform p = Transform(cv::Mat(TcwMat, cv::Range(0,3), cv::Range(0,4)));
-
-		if(!p.isNull())
-		{
-			if(!localTransform.isNull())
-			{
-				if(originLocalTransform_.isNull())
-				{
-					originLocalTransform_ = localTransform;
-				}
-				// transform in base frame
-				p = originLocalTransform_ * p.inverse() * localTransform.inverse();
-			}
-			t = previousPoseInv*p;
-		}
-		previousPose_ = p;
-
-		if(firstFrame_)
-		{
-			// just recovered of being lost, set high covariance
-			covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
-			firstFrame_ = false;
-		}
-		else
-		{
-			float baseline = data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline();
-			if(baseline <= 0.0f)
-			{
-				baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
-				rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
-			}
-			double linearVar = 0.0001;
-			if(baseline > 0.0f)
-			{
-				linearVar = baseline/8.0;
-				linearVar *= linearVar;
-			}
-
-			covariance = cv::Mat::eye(6,6, CV_64FC1);
-			covariance.at<double>(0,0) = linearVar;
-			covariance.at<double>(1,1) = linearVar;
-			covariance.at<double>(2,2) = linearVar;
-			covariance.at<double>(3,3) = 0.0001;
-			covariance.at<double>(4,4) = 0.0001;
-			covariance.at<double>(5,5) = 0.0001;
-		}
-	}
-
-	if(info)
-	{
-		info->lost = t.isNull();
-		info->type = (int)kTypeORBSLAM;
-		info->reg.covariance = covariance;
-		info->localMapSize = mapPoints.size();
-		info->localKeyFrames = 0;
-
-		if(this->isInfoDataFilled())
-		{
-			std::vector<cv::KeyPoint> kpts = orbslam_->GetTrackedKeyPointsUn();
-			info->reg.matchesIDs.resize(kpts.size());
-			info->reg.inliersIDs.resize(kpts.size());
-			int oi = 0;
-
-			UASSERT(mapPoints.size() == kpts.size());
-			for (unsigned int i = 0; i < kpts.size(); ++i)
-			{
-				int wordId;
-				if(mapPoints[i] != 0)
-				{
-					wordId = mapPoints[i]->mnId;
-				}
-				else
-				{
-					wordId = -(i+1);
-				}
-				info->words.insert(std::make_pair(wordId, kpts[i]));
-				if(mapPoints[i] != 0)
-				{
-					info->reg.matchesIDs[oi] = wordId;
-					info->reg.inliersIDs[oi] = wordId;
-					++oi;
-				}
-			}
-			info->reg.matchesIDs.resize(oi);
-			info->reg.inliersIDs.resize(oi);
-			info->reg.inliers = oi;
-			info->reg.matches = oi;
-
-			Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
-			for (unsigned int i = 0; i < mapPoints.size(); ++i)
-			{
-				if(mapPoints[i])
-				{
-					Eigen::Vector3f pt = mapPoints[i]->GetWorldPos();
-					pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt[0], pt[1], pt[2]), fixRot);
-					info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
-				}
-			}
-		}
-	}
-
-	UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false");
-
-#else
-	UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
-#endif
-	return t;
-}
-
-} // namespace rtabmap
@@ -28,17 +28,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d_transforms.h"
-#include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
-#include <opencv2/core/eigen.hpp>
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UThread.h"
+#include "rtabmap/utilite/UDirectory.h"
 #include <opencv2/imgproc/types_c.h>

 #ifdef RTABMAP_OPENVINS
 #include "core/VioManager.h"
-#include "state/Propagator.h"
+#include "core/VioManagerOptions.h"
+#include "core/RosVisualizer.h"
+#include "utils/dataset_reader.h"
+#include "utils/parse_ros.h"
+#include "utils/sensor_data.h"
 #include "state/State.h"
-#include "state/StateHelper.h"
+#include "types/Type.h"
 #endif

 namespace rtabmap {
@@ -47,111 +52,17 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
 	Odometry(parameters)
 #ifdef RTABMAP_OPENVINS
    ,
+	vioManager_(0),
 	initGravity_(false),
-	previousPoseInv_(Transform::getIdentity())
+	previousPose_(Transform::getIdentity())
 #endif
 {
+}
+
+OdometryOpenVINS::~OdometryOpenVINS()
+{
 #ifdef RTABMAP_OPENVINS
-	ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel(ULogger::level()+1));
-	int enum_index;
-	std::string left_mask_path, right_mask_path;
-	params_ = std::make_unique<ov_msckf::VioManagerOptions>();
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSNumPts(), params_->num_pts);
-	Parameters::parse(parameters, Parameters::kFASTThreshold(), params_->fast_threshold);
-	Parameters::parse(parameters, Parameters::kVisGridCols(), params_->grid_x);
-	Parameters::parse(parameters, Parameters::kVisGridRows(), params_->grid_y);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist);
-	Parameters::parse(parameters, Parameters::kVisCorNNDR(), params_->knn_ratio);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSFiTriangulate1d(), params_->featinit_options.triangulate_1d);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSFiRefineFeatures(), params_->featinit_options.refine_features);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxRuns(), params_->featinit_options.max_runs);
-	Parameters::parse(parameters, Parameters::kVisMinDepth(), params_->featinit_options.min_dist);
-	Parameters::parse(parameters, Parameters::kVisMaxDepth(), params_->featinit_options.max_dist);
-	if(params_->featinit_options.max_dist == 0)
-		params_->featinit_options.max_dist = std::numeric_limits<double>::infinity();
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxBaseline(), params_->featinit_options.max_baseline);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxCondNumber(), params_->featinit_options.max_cond_number);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index);
-	params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamExtrinsics(), params_->state_options.do_calib_camera_pose);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamIntrinsics(), params_->state_options.do_calib_camera_intrinsics);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamTimeoffset(), params_->state_options.do_calib_camera_timeoffset);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUIntrinsics(), params_->state_options.do_calib_imu_intrinsics);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUGSensitivity(), params_->state_options.do_calib_imu_g_sensitivity);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxClones(), params_->state_options.max_clone_size);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAM(), params_->state_options.max_slam_features);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAMInUpdate(), params_->state_options.max_slam_in_update);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxMSCKFInUpdate(), params_->state_options.max_msckf_in_update);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepMSCKF(), enum_index);
-	params_->state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::Representation(enum_index);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepSLAM(), enum_index);
-	params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag);
-	Parameters::parse(parameters, Parameters::kVisDepthAsMask(), params_->use_mask);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSLeftMaskPath(), left_mask_path);
-	if(!left_mask_path.empty())
-	{
-		if(!UFile::exists(left_mask_path))
-			UWARN("OpenVINS: invalid left mask path: %s", left_mask_path.c_str());
-		else
-			params_->masks.emplace(0, cv::imread(left_mask_path, cv::IMREAD_GRAYSCALE));
-	}
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSRightMaskPath(), right_mask_path);
-	if(!right_mask_path.empty())
-	{
-		if(!UFile::exists(right_mask_path))
-			UWARN("OpenVINS: invalid right mask path: %s", right_mask_path.c_str());
-		else
-			params_->masks.emplace(1, cv::imread(right_mask_path, cv::IMREAD_GRAYSCALE));
-	}
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitWindowTime(), params_->init_options.init_window_time);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxFeatures(), params_->init_options.init_max_features);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynUse(), params_->init_options.init_dyn_use);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEOptCalib(), params_->init_options.init_dyn_mle_opt_calib);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxIter(), params_->init_options.init_dyn_mle_max_iter);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxTime(), params_->init_options.init_dyn_mle_max_time);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxThreads(), params_->init_options.init_dyn_mle_max_threads);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynNumPose(), params_->init_options.init_dyn_num_pose);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinDeg(), params_->init_options.init_dyn_min_deg);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationOri(), params_->init_options.init_dyn_inflation_orientation);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationVel(), params_->init_options.init_dyn_inflation_velocity);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBg(), params_->init_options.init_dyn_inflation_bias_gyro);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBa(), params_->init_options.init_dyn_inflation_bias_accel);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinRecCond(), params_->init_options.init_dyn_min_rec_cond);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSTryZUPT(), params_->try_zupt);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTChi2Multiplier(), params_->zupt_options.chi2_multipler);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxVelodicy(), params_->zupt_max_velocity);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTNoiseMultiplier(), params_->zupt_noise_multiplier);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxDisparity(), params_->zupt_max_disparity);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTOnlyAtBeginning(), params_->zupt_only_at_beginning);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerNoiseDensity(), params_->imu_noises.sigma_a);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerRandomWalk(), params_->imu_noises.sigma_ab);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeNoiseDensity(), params_->imu_noises.sigma_w);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeRandomWalk(), params_->imu_noises.sigma_wb);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFSigmaPx(), params_->msckf_options.sigma_pix);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier(), params_->msckf_options.chi2_multipler);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMSigmaPx(), params_->slam_options.sigma_pix);
-	Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMChi2Multiplier(), params_->slam_options.chi2_multipler);
-	params_->vec_dw << 1, 0, 0, 1, 0, 1;
-	params_->vec_da << 1, 0, 0, 1, 0, 1;
-	params_->vec_tg << 0, 0, 0, 0, 0, 0, 0, 0, 0;
-	params_->q_ACCtoIMU << 0, 0, 0, 1;
-	params_->q_GYROtoIMU << 0, 0, 0, 1;
-	params_->use_aruco = false;
-	params_->num_opencv_threads = -1;
-	params_->histogram_method = ov_core::TrackBase::HistogramMethod::NONE;
-	params_->init_options.sigma_a = params_->imu_noises.sigma_a;
-	params_->init_options.sigma_ab = params_->imu_noises.sigma_ab;
-	params_->init_options.sigma_w = params_->imu_noises.sigma_w;
-	params_->init_options.sigma_wb = params_->imu_noises.sigma_wb;
-	params_->init_options.sigma_pix = params_->slam_options.sigma_pix;
-	params_->init_options.gravity_mag = params_->gravity_mag;
+	delete vioManager_;
 #endif
 }

@@ -161,9 +72,11 @@ void OdometryOpenVINS::reset(const Transform & initialPose)
 #ifdef RTABMAP_OPENVINS
 	if(!initGravity_)
 	{
-		vioManager_.reset();
-		previousPoseInv_.setIdentity();
-		imuLocalTransformInv_.setNull();
+		delete vioManager_;
+		vioManager_ = 0;
+		previousPose_.setIdentity();
+		previousLocalTransform_.setNull();
+		imuBuffer_.clear();
 	}
 	initGravity_ = false;
 #endif
@@ -177,306 +90,395 @@ Transform OdometryOpenVINS::computeTransform(
 {
 	Transform t;
 #ifdef RTABMAP_OPENVINS
+	UTimer timer;

-	if(!vioManager_)
+	// Buffer imus;
+	if(!data.imu().empty())
 	{
-		if(!data.imu().empty())
+		imuBuffer_.insert(std::make_pair(data.stamp(), data.imu()));
+	}
+
+	// OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp
+	if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1)
+	{
+		if(imuBuffer_.empty())
 		{
-			imuLocalTransformInv_ = data.imu().localTransform().inverse();
-			Phi_.setZero();
-			Phi_.block(0,0,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3);
-			Phi_.block(3,3,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3);
+			UWARN("Waiting IMU for initialization...");
+			return t;
 		}
-
-		if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull())
+		if(vioManager_ == 0)
 		{
-			Transform T_imu_left;
-			Eigen::VectorXd left_calib(8), right_calib(8);
-			if(!data.rightRaw().empty())
+			UINFO("OpenVINS Initialization");
+
+			// intialize
+			ov_msckf::VioManagerOptions params;
+
+			// ESTIMATOR ======================================================================
+
+			// Main EKF parameters
+			//params.state_options.do_fej = true;
+			//params.state_options.imu_avg =false;
+			//params.state_options.use_rk4_integration;
+			//params.state_options.do_calib_camera_pose = false;
+			//params.state_options.do_calib_camera_intrinsics = false;
+			//params.state_options.do_calib_camera_timeoffset = false;
+			//params.state_options.max_clone_size = 11;
+			//params.state_options.max_slam_features = 25;
+			//params.state_options.max_slam_in_update = INT_MAX;
+			//params.state_options.max_msckf_in_update = INT_MAX;
+			//params.state_options.max_aruco_features = 1024;
+			params.state_options.num_cameras = 2;
+			//params.dt_slam_delay = 2;
+
+			// Set what representation we should be using
+			//params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
+			//params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH");  // default GLOBAL_3D
+			//params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
+			if( params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN ||
+				params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN ||
+				params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN)
 			{
-				params_->state_options.num_cameras = params_->init_options.num_cameras = 2;
-				T_imu_left = imuLocalTransformInv_ * data.stereoCameraModels()[0].localTransform();
+				printf(RED "VioManager(): invalid feature representation specified:\n" RESET);
+				printf(RED "\t- GLOBAL_3D\n" RESET);
+				printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
+				printf(RED "\t- ANCHORED_3D\n" RESET);
+				printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
+				printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
+				printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
+				std::exit(EXIT_FAILURE);
+			}

-				bool is_fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
-				if(is_fisheye)
-				{
-					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
-						data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
-					params_->camera_intrinsics.emplace(1, std::make_shared<ov_core::CamEqui>(
-						data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight()));
-				}
-				else
-				{
-					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
-						data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
-					params_->camera_intrinsics.emplace(1, std::make_shared<ov_core::CamRadtan>(
-						data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight()));
-				}
+			// Filter initialization
+			//params.init_window_time = 1;
+			//params.init_imu_thresh = 1;

-				if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
-				{
-					left_calib << data.stereoCameraModels()[0].left().fx(),
-						data.stereoCameraModels()[0].left().fy(),
-					 	data.stereoCameraModels()[0].left().cx(),
-					 	data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
-					right_calib << data.stereoCameraModels()[0].right().fx(),
-						data.stereoCameraModels()[0].right().fy(),
-					 	data.stereoCameraModels()[0].right().cx(),
-					 	data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
-				}
-				else
-				{
-					UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
-					UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
-					UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
-					left_calib << data.stereoCameraModels()[0].left().K_raw().at<double>(0,0),
-					 	data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
-					 	data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
-					 	data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
-					 	data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
-					 	data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
-					 	data.stereoCameraModels()[0].left().D_raw().at<double>(0,is_fisheye?4:2),
-					 	data.stereoCameraModels()[0].left().D_raw().at<double>(0,is_fisheye?5:3);
-					right_calib << data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
-					 	data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
-					 	data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
-					 	data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
-					 	data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
-					 	data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
-					 	data.stereoCameraModels()[0].right().D_raw().at<double>(0,is_fisheye?4:2),
-					 	data.stereoCameraModels()[0].right().D_raw().at<double>(0,is_fisheye?5:3);
-				}
+			// Zero velocity update
+			//params.try_zupt = false;
+			//params.zupt_options.chi2_multipler = 5;
+			//params.zupt_max_velocity = 1;
+			//params.zupt_noise_multiplier = 1;
+
+			// NOISE ======================================================================
+
+			// Our noise values for inertial sensor
+			//params.imu_noises.sigma_w = 1.6968e-04;
+			//params.imu_noises.sigma_a = 2.0000e-3;
+			//params.imu_noises.sigma_wb = 1.9393e-05;
+			//params.imu_noises.sigma_ab = 3.0000e-03;
+
+			// Read in update parameters
+			//params.msckf_options.sigma_pix = 1;
+			//params.msckf_options.chi2_multipler = 5;
+			//params.slam_options.sigma_pix = 1;
+			//params.slam_options.chi2_multipler = 5;
+			//params.aruco_options.sigma_pix = 1;
+			//params.aruco_options.chi2_multipler = 5;
+
+
+			// STATE ======================================================================
+
+			// Timeoffset from camera to IMU
+			//params.calib_camimu_dt = 0.0;
+
+			// Global gravity
+			//params.gravity[2] = 9.81;
+
+
+			// TRACKERS ======================================================================
+
+			// Tracking flags
+			params.use_stereo = true;
+			//params.use_klt = true;
+			params.use_aruco = false;
+			//params.downsize_aruco = true;
+			//params.downsample_cameras = false;
+			//params.use_multi_threading = true;
+
+			// General parameters
+			//params.num_pts = 200;
+			//params.fast_threshold = 10;
+			//params.grid_x = 10;
+			//params.grid_y = 5;
+			//params.min_px_dist = 8;
+			//params.knn_ratio = 0.7;
+
+			// Feature initializer parameters
+			//nh.param<bool>("fi_triangulate_1d", params.featinit_options.triangulate_1d, params.featinit_options.triangulate_1d);
+			//nh.param<bool>("fi_refine_features", params.featinit_options.refine_features, params.featinit_options.refine_features);
+			//nh.param<int>("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs);
+			//nh.param<double>("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda);
+			//nh.param<double>("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda);
+			//nh.param<double>("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx);
+			///nh.param<double>("fi_min_dcost", params.featinit_options.min_dcost, params.featinit_options.min_dcost);
+			//nh.param<double>("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult);
+			//nh.param<double>("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist);
+			//params.featinit_options.max_dist = 75;
+			//params.featinit_options.max_baseline = 500;
+			//params.featinit_options.max_cond_number = 5000;
+
+
+			// CAMERA ======================================================================
+			bool fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
+			params.camera_fisheye.insert(std::make_pair(0, fisheye));
+			params.camera_fisheye.insert(std::make_pair(1, fisheye));
+
+			Eigen::VectorXd camLeft(8), camRight(8);
+			if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
+			{
+				camLeft << data.stereoCameraModels()[0].left().fx(),
+					 data.stereoCameraModels()[0].left().fy(),
+					 data.stereoCameraModels()[0].left().cx(),
+					 data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
+				camRight << data.stereoCameraModels()[0].right().fx(),
+					 data.stereoCameraModels()[0].right().fy(),
+					 data.stereoCameraModels()[0].right().cx(),
+					 data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
 			}
 			else
 			{
-				params_->state_options.num_cameras = params_->init_options.num_cameras = 1;
-				T_imu_left = imuLocalTransformInv_ * data.cameraModels()[0].localTransform();
+				UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
+				UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
+				UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);

-				bool is_fisheye = data.cameraModels()[0].isFisheye() && !this->imagesAlreadyRectified();
-				if(is_fisheye)
-				{
-					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
-						data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
-				}
-				else
-				{
-					params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
-						data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
-				}
+				//https://github.com/ethz-asl/kalibr/wiki/supported-models
+				///	    radial-tangential (radtan)
+				//		(distortion_coeffs: [k1 k2 r1 r2])
+				///		equidistant (equi)
+				//		(distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4)

-				if(this->imagesAlreadyRectified() || data.cameraModels()[0].D_raw().empty())
-				{
-					left_calib << data.cameraModels()[0].fx(),
-						data.cameraModels()[0].fy(),
-					 	data.cameraModels()[0].cx(),
-					 	data.cameraModels()[0].cy(), 0, 0, 0, 0;
-				}
-				else
-				{
-					UASSERT(data.cameraModels()[0].D_raw().cols >= 4);
-					left_calib << data.cameraModels()[0].K_raw().at<double>(0,0),
-					 	data.cameraModels()[0].K_raw().at<double>(1,1),
-					 	data.cameraModels()[0].K_raw().at<double>(0,2),
-					 	data.cameraModels()[0].K_raw().at<double>(1,2),
-					 	data.cameraModels()[0].D_raw().at<double>(0,0),
-					 	data.cameraModels()[0].D_raw().at<double>(0,1),
-					 	data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?4:2),
-					 	data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?5:3);
-				}
+				camLeft <<
+					 data.stereoCameraModels()[0].left().K_raw().at<double>(0,0),
+					 data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
+					 data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
+					 data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
+					 data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
+					 data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
+					 data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?4:2),
+					 data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?5:3);
+				camRight <<
+					 data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
+					 data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
+					 data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
+					 data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
+					 data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
+					 data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
+					 data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?4:2),
+					 data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?5:3);
 			}
+			params.camera_intrinsics.insert(std::make_pair(0, camLeft));
+			params.camera_intrinsics.insert(std::make_pair(1, camRight));

-			Eigen::Matrix4d T_LtoI = T_imu_left.toEigen4d();
-			Eigen::Matrix<double,7,1> left_eigen;
-			left_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_LtoI.block(0,0,3,3).transpose());
-			left_eigen.block(4,0,3,1) = -T_LtoI.block(0,0,3,3).transpose()*T_LtoI.block(0,3,3,1);
-			params_->camera_intrinsics.at(0)->set_value(left_calib);
-			params_->camera_extrinsics.emplace(0, left_eigen);
-			if(!data.rightRaw().empty())
+			const IMU & imu = imuBuffer_.begin()->second;
+			imuLocalTransform_ = imu.localTransform();
+			Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModels()[0].localTransform();
+			Transform cam0cam1;
+			if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
 			{
-				Transform T_left_right;
-				if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
-				{
-					T_left_right = Transform(
+				cam0cam1 = Transform(
 						1, 0, 0, data.stereoCameraModels()[0].baseline(),
 						0, 1, 0, 0,
 						0, 0, 1, 0);
-				}
-				else
-				{
-					T_left_right = data.stereoCameraModels()[0].stereoTransform().inverse();
-				}
-				UASSERT(!T_left_right.isNull());
-				Transform T_imu_right = T_imu_left * T_left_right;
-				Eigen::Matrix4d T_RtoI = T_imu_right.toEigen4d();
-				Eigen::Matrix<double,7,1> right_eigen;
-				right_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_RtoI.block(0,0,3,3).transpose());
-				right_eigen.block(4,0,3,1) = -T_RtoI.block(0,0,3,3).transpose()*T_RtoI.block(0,3,3,1);
-				params_->camera_intrinsics.at(1)->set_value(right_calib);
-				params_->camera_extrinsics.emplace(1, right_eigen);
 			}
-			params_->init_options.camera_intrinsics = params_->camera_intrinsics;
-			params_->init_options.camera_extrinsics = params_->camera_extrinsics;
-			vioManager_ = std::make_unique<ov_msckf::VioManager>(*params_);
+			else
+			{
+				cam0cam1 = data.stereoCameraModels()[0].stereoTransform().inverse();
+			}
+			UASSERT(!cam0cam1.isNull());
+			Transform imuCam1 = imuCam0 * cam0cam1;
+			Eigen::Matrix4d cam0_eigen = imuCam0.toEigen4d();
+			Eigen::Matrix4d cam1_eigen = imuCam1.toEigen4d();
+			Eigen::Matrix<double,7,1> cam_eigen0;
+			cam_eigen0.block(0,0,4,1) = rot_2_quat(cam0_eigen.block(0,0,3,3).transpose());
+			cam_eigen0.block(4,0,3,1) = -cam0_eigen.block(0,0,3,3).transpose()*cam0_eigen.block(0,3,3,1);
+			Eigen::Matrix<double,7,1> cam_eigen1;
+			cam_eigen1.block(0,0,4,1) = rot_2_quat(cam1_eigen.block(0,0,3,3).transpose());
+			cam_eigen1.block(4,0,3,1) = -cam1_eigen.block(0,0,3,3).transpose()*cam1_eigen.block(0,3,3,1);
+			params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0));
+			params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1));
+
+			params.camera_wh.insert({0, std::make_pair(data.stereoCameraModels()[0].left().imageWidth(),data.stereoCameraModels()[0].left().imageHeight())});
+			params.camera_wh.insert({1, std::make_pair(data.stereoCameraModels()[0].right().imageWidth(),data.stereoCameraModels()[0].right().imageHeight())});
+
+			vioManager_ = new ov_msckf::VioManager(params);
 		}
-	}
-	else
-	{
-		if(!data.imu().empty())
+
+		cv::Mat left;
+		cv::Mat right;
+		if(data.imageRaw().type() == CV_8UC3)
 		{
+			cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY);
+		}
+		else if(data.imageRaw().type() == CV_8UC1)
+		{
+			left = data.imageRaw().clone();
+		}
+		else
+		{
+			UFATAL("Not supported color type!");
+		}
+		if(data.rightRaw().type() == CV_8UC3)
+		{
+			cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
+		}
+		else if(data.rightRaw().type() == CV_8UC1)
+		{
+			right = data.rightRaw().clone();
+		}
+		else
+		{
+			UFATAL("Not supported color type!");
+		}
+
+		// Create the measurement
+		ov_core::CameraData message;
+		message.timestamp = data.stamp();
+		message.sensor_ids.push_back(0);
+		message.sensor_ids.push_back(1);
+		message.images.push_back(left);
+		message.images.push_back(right);
+		message.masks.push_back(cv::Mat::zeros(left.size(), CV_8UC1));
+		message.masks.push_back(cv::Mat::zeros(right.size(), CV_8UC1));
+
+		// send it to our VIO system
+		vioManager_->feed_measurement_camera(message);
+		UDEBUG("Image update stamp=%f", data.stamp());
+
+		double lastIMUstamp = 0.0;
+		while(!imuBuffer_.empty())
+		{
+			std::map<double, IMU>::iterator iter = imuBuffer_.begin();
+
+			// Process IMU data until stamp is over image stamp
 			ov_core::ImuData message;
-			message.timestamp = data.stamp();
-			message.wm << data.imu().angularVelocity().val[0], data.imu().angularVelocity().val[1], data.imu().angularVelocity().val[2];
-			message.am << data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[1], data.imu().linearAcceleration().val[2];
+			message.timestamp = iter->first;
+			message.wm << iter->second.angularVelocity().val[0], iter->second.angularVelocity().val[1], iter->second.angularVelocity().val[2];
+			message.am << iter->second.linearAcceleration().val[0], iter->second.linearAcceleration().val[1], iter->second.linearAcceleration().val[2];
+
+			UDEBUG("IMU update stamp=%f", message.timestamp);
+
+			// send it to our VIO system
 			vioManager_->feed_measurement_imu(message);
+
+			lastIMUstamp = iter->first;
+
+			imuBuffer_.erase(iter);
+
+			if(lastIMUstamp > data.stamp())
+			{
+				break;
+			}
 		}

-		if(!data.imageRaw().empty())
+		if(vioManager_->initialized())
 		{
-			bool covFilled = false;
-			Eigen::Matrix<double, 13, 1> state_plus = Eigen::Matrix<double, 13, 1>::Zero();
-			Eigen::Matrix<double, 12, 12> cov_plus = Eigen::Matrix<double, 12, 12>::Zero();
-			if(vioManager_->initialized())
-				covFilled = vioManager_->get_propagator()->fast_state_propagate(vioManager_->get_state(), data.stamp(), state_plus, cov_plus);
-
-			cv::Mat image;
-			if(data.imageRaw().type() == CV_8UC3)
-				cv::cvtColor(data.imageRaw(), image, CV_BGR2GRAY);
-			else if(data.imageRaw().type() == CV_8UC1)
-				image = data.imageRaw().clone();
-			else
-				UFATAL("Not supported color type!");
-			ov_core::CameraData message;
-			message.timestamp = data.stamp();
-			message.sensor_ids.emplace_back(0);
-			message.images.emplace_back(image);
-			if(params_->masks.find(0) != params_->masks.end())
-			{
-				message.masks.emplace_back(params_->masks[0]);
-			}
-			else if(!data.depthRaw().empty() && params_->use_mask)
-			{
-				cv::Mat mask;
-				if(data.depthRaw().type() == CV_32FC1)
-					cv::inRange(data.depthRaw(), params_->featinit_options.min_dist,
-						std::isinf(params_->featinit_options.max_dist)?std::numeric_limits<float>::max():params_->featinit_options.max_dist, mask);
-				else if(data.depthRaw().type() == CV_16UC1)
-					cv::inRange(data.depthRaw(), params_->featinit_options.min_dist*1000,
-						std::isinf(params_->featinit_options.max_dist)?std::numeric_limits<uint16_t>::max():params_->featinit_options.max_dist*1000, mask);
-				message.masks.emplace_back(255-mask);
-			}
-			else
-			{
-				message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
-			}
-			if(!data.rightRaw().empty())
-			{
-				if(data.rightRaw().type() == CV_8UC3)
-					cv::cvtColor(data.rightRaw(), image, CV_BGR2GRAY);
-				else if(data.rightRaw().type() == CV_8UC1)
-					image = data.rightRaw().clone();
-				else
-					UFATAL("Not supported color type!");
-				message.sensor_ids.emplace_back(1);
-				message.images.emplace_back(image);
-				if(params_->masks.find(1) != params_->masks.end())
-					message.masks.emplace_back(params_->masks[1]);
-				else
-					message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
-			}
-			vioManager_->feed_measurement_camera(message);
-
+			// Get the current state
 			std::shared_ptr<ov_msckf::State> state = vioManager_->get_state();
-			Transform p((float)state->_imu->pos()(0),
-						(float)state->_imu->pos()(1),
-						(float)state->_imu->pos()(2),
-						(float)state->_imu->quat()(0),
-						(float)state->_imu->quat()(1),
-						(float)state->_imu->quat()(2),
-						(float)state->_imu->quat()(3));
-			if(!p.isNull() && !p.isIdentity())
+
+			if(state->_timestamp != data.stamp())
 			{
-				p = p * imuLocalTransformInv_;
+				UWARN("OpenVINS: Stamp of the current state %f is not the same "
+						"than last image processed %f (last IMU stamp=%f). There could be "
+						"a synchronization issue between camera and IMU. ",
+						state->_timestamp,
+						data.stamp(),
+						lastIMUstamp);
+			}
+
+			Transform p(
+					(float)state->_imu->pos()(0),
+					(float)state->_imu->pos()(1),
+					(float)state->_imu->pos()(2),
+					(float)state->_imu->quat()(0),
+					(float)state->_imu->quat()(1),
+					(float)state->_imu->quat()(2),
+					(float)state->_imu->quat()(3));
+
+
+			// Finally set the covariance in the message (in the order position then orientation as per ros convention)
+			std::vector<std::shared_ptr<ov_type::Type>> statevars;
+			statevars.push_back(state->_imu->pose()->p());
+			statevars.push_back(state->_imu->pose()->q());
+
+			cv::Mat covariance = cv::Mat::eye(6,6, CV_64FC1);
+			if(this->framesProcessed() == 0)
+			{
+				covariance *= 9999;
+			}
+			else
+			{
+				Eigen::Matrix<double,6,6> covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(),statevars);
+				for(int r=0; r<6; r++) {
+					for(int c=0; c<6; c++) {
+						((double *)covariance.data)[6*r+c] = covariance_posori(r,c);
+					}
+				}
+			}
+
+			if(!p.isNull())
+			{
+				p =  p * imuLocalTransform_.inverse();

 				if(this->getPose().rotation().isIdentity())
 				{
 					initGravity_ = true;
-					this->reset(this->getPose() * p.rotation());
+					this->reset(this->getPose()*p.rotation());
 				}

-				if(previousPoseInv_.isIdentity())
-					previousPoseInv_ = p.inverse();
+				if(previousPose_.isIdentity())
+				{
+					previousPose_ = p;
+				}

-				t = previousPoseInv_ * p;
+				// make it incremental
+				Transform previousPoseInv = previousPose_.inverse();
+				t = previousPoseInv*p;
+				previousPose_ = p;

 				if(info)
 				{
-					double timestamp;
-					std::unordered_map<size_t, Eigen::Vector3d> feat_posinG, feat_tracks_uvd;
-					vioManager_->get_active_tracks(timestamp, feat_posinG, feat_tracks_uvd);
-					auto features_SLAM = vioManager_->get_features_SLAM();
-					auto good_features_MSCKF = vioManager_->get_good_features_MSCKF();
-
 					info->type = this->getType();
-					info->localMapSize = feat_posinG.size();
-					info->features = features_SLAM.size() + good_features_MSCKF.size();
-					info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1);
-					if(covFilled)
-					{
-						Eigen::Matrix<double, 6, 6> covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose();
-						cv::eigen2cv(covariance, info->reg.covariance);
-					}
+					info->reg.covariance = covariance;

-					if(this->isInfoDataFilled())
+					// feature map
+					Transform fixT = this->getPose()*previousPoseInv;
+					Transform camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse()*this->getPose().inverse();
+					for (auto &it_per_id : vioManager_->get_features_SLAM())
 					{
-						Transform fixT = this->getPose() * previousPoseInv_;
-						Transform camT;
-						if(!data.rightRaw().empty())
-							camT = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT;
-						else
-							camT = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT;
-						
-						for(auto &feature : feat_posinG)
-						{
-							cv::Point3f pt3d(feature.second[0], feature.second[1], feature.second[2]);
-							pt3d = util3d::transformPoint(pt3d, fixT);
-							info->localMap.emplace(feature.first, pt3d);
-						}
+						cv::Point3f pt3d;
+						pt3d.x = it_per_id[0];
+						pt3d.y = it_per_id[1];
+						pt3d.z = it_per_id[2];
+						pt3d = util3d::transformPoint(pt3d, fixT);
+						info->localMap.insert(std::make_pair(info->localMap.size(), pt3d));

 						if(this->imagesAlreadyRectified())
 						{
-							for(auto &feature : features_SLAM)
-							{
-								cv::Point3f pt3d(feature[0], feature[1], feature[2]);
-								pt3d = util3d::transformPoint(pt3d, camT);
-								cv::Point2f pt;
-								if(!data.rightRaw().empty())
-									data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
-								else
-									data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
-								info->reg.inliersIDs.emplace_back(info->newCorners.size());
-								info->newCorners.emplace_back(pt);
-							}
-
-							for(auto &feature : good_features_MSCKF)
-							{
-								cv::Point3f pt3d(feature[0], feature[1], feature[2]);
-								pt3d = util3d::transformPoint(pt3d, camT);
-								cv::Point2f pt;
-								if(!data.rightRaw().empty())
-									data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
-								else
-									data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
-								info->reg.matchesIDs.emplace_back(info->newCorners.size());
-								info->newCorners.emplace_back(pt);
-							}
+							cv::Point2f pt;
+							pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
+							data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
+							info->reg.inliersIDs.push_back(info->newCorners.size());
+							info->newCorners.push_back(pt);
 						}
 					}
+					info->features = info->newCorners.size();
+					info->localMapSize = info->localMap.size();
 				}
-
-				previousPoseInv_ = p.inverse();
+				UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
 			}
 		}
 	}
+	else if(!data.imageRaw().empty() && !data.depthRaw().empty())
+	{
+		UERROR("OpenVINS doesn't work with RGB-D data, stereo images are required!");
+	}
+	else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
+	{
+		UERROR("OpenVINS requires stereo images!");
+	}
+	else
+	{
+		UERROR("OpenVINS requires stereo images (only one stereo camera and should be calibrated)!");
+	}

 #else
 	UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach.");
@@ -2054,43 +2054,7 @@ bool OptimizerG2O::saveGraph(
 				q.w());
 		}

-		// For landmarks, determinate which one has observation with orientation
-		std::map<int, bool> isLandmarkWithRotation;
-		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
-		{
-			int landmarkId = iter->second.from() < 0?iter->second.from():iter->second.to() < 0?iter->second.to():0;
-			if(landmarkId != 0 && isLandmarkWithRotation.find(landmarkId) == isLandmarkWithRotation.end())
-			{
-				if(isSlam2d())
-				{
-					if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
-					{
-						isLandmarkWithRotation.insert(std::make_pair(landmarkId, false));
-						UDEBUG("Tag %d has no orientation", landmarkId);
-					}
-					else
-					{
-						isLandmarkWithRotation.insert(std::make_pair(landmarkId, true));
-						UDEBUG("Tag %d has orientation", landmarkId);
-					}
-				}
-				else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
-						 1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
-						 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
-				{
-					isLandmarkWithRotation.insert(std::make_pair(landmarkId, false));
-					UDEBUG("Tag %d has no orientation", landmarkId);
-				}
-				else
-				{
-					isLandmarkWithRotation.insert(std::make_pair(landmarkId, true));
-					UDEBUG("Tag %d has orientation", landmarkId);
-				}
-
-			}
-		}
-
-		int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first:0;
+		int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first+1:0;
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			if (isSlam2d())
@@ -2099,30 +2063,18 @@ bool OptimizerG2O::saveGraph(
 				{
 					// VERTEX_SE2 id x y theta
 					fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
-						iter->first,
+						landmarkOffset-iter->first,
 						iter->second.x(),
 						iter->second.y(),
 						iter->second.theta());
 				}
 				else if(!landmarksIgnored())
 				{
-					if(uValue(isLandmarkWithRotation, iter->first, false))
-					{
-						// VERTEX_SE2 id x y theta
-						fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
-							landmarkOffset-iter->first,
-							iter->second.x(),
-							iter->second.y(),
-							iter->second.theta());
-					}
-					else
-					{
-						// VERTEX_XY id x y
-						fprintf(file, "VERTEX_XY %d %f %f\n",
-							landmarkOffset-iter->first,
-							iter->second.x(),
-							iter->second.y());
-					}
+					// VERTEX_XY id x y
+					fprintf(file, "VERTEX_XY %d %f %f\n",
+						iter->first,
+						iter->second.x(),
+						iter->second.y());
 				}
 			}
 			else
@@ -2143,29 +2095,12 @@ bool OptimizerG2O::saveGraph(
 				}
 				else if(!landmarksIgnored())
 				{
-					if(uValue(isLandmarkWithRotation, iter->first, false))
-					{
-						// VERTEX_SE3 id x y z qw qx qy qz
-						Eigen::Quaternionf q = iter->second.getQuaternionf();
-						fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
-							landmarkOffset-iter->first,
-							iter->second.x(),
-							iter->second.y(),
-							iter->second.z(),
-							q.x(),
-							q.y(),
-							q.z(),
-							q.w());
-					}
-					else
-					{
-						// VERTEX_TRACKXYZ id x y z
-						fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n",
-							landmarkOffset-iter->first,
-							iter->second.x(),
-							iter->second.y(),
-							iter->second.z());
-					}
+					// VERTEX_TRACKXYZ id x y z
+					fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n",
+						landmarkOffset-iter->first,
+						iter->second.x(),
+						iter->second.y(),
+						iter->second.z());
 				}
 			}
 		}
@@ -2181,90 +2116,32 @@ bool OptimizerG2O::saveGraph(
 				}
 				if(isSlam2d())
 				{
-					if(uValue(isLandmarkWithRotation, iter->first, false))
-					{
-						// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
-						fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n",
-								iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
-								iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
-								iter->second.transform().x(),
-								iter->second.transform().y(),
-								iter->second.transform().theta(),
-								iter->second.infMatrix().at<double>(0, 0),
-								iter->second.infMatrix().at<double>(0, 1),
-								iter->second.infMatrix().at<double>(0, 5),
-								iter->second.infMatrix().at<double>(1, 1),
-								iter->second.infMatrix().at<double>(1, 5),
-								iter->second.infMatrix().at<double>(5, 5));
-					}
-					else
-					{
-						// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
-						fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
-							iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
-							iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
-							iter->second.transform().x(),
-							iter->second.transform().y(),
-							iter->second.infMatrix().at<double>(0, 0),
-							iter->second.infMatrix().at<double>(0, 1),
-							iter->second.infMatrix().at<double>(1, 1));
-					}
+					// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
+					fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
+						iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
+						iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
+						iter->second.transform().x(),
+						iter->second.transform().y(),
+						iter->second.infMatrix().at<double>(0, 0),
+						iter->second.infMatrix().at<double>(0, 1),
+						iter->second.infMatrix().at<double>(1, 1));
 				}
 				else
 				{
-					if(uValue(isLandmarkWithRotation, iter->first, false))
-					{
-						// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
-						Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
-						fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
-								iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
-								iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
-								iter->second.transform().x(),
-								iter->second.transform().y(),
-								iter->second.transform().z(),
-								q.x(),
-								q.y(),
-								q.z(),
-								q.w(),
-								iter->second.infMatrix().at<double>(0, 0),
-								iter->second.infMatrix().at<double>(0, 1),
-								iter->second.infMatrix().at<double>(0, 2),
-								iter->second.infMatrix().at<double>(0, 3),
-								iter->second.infMatrix().at<double>(0, 4),
-								iter->second.infMatrix().at<double>(0, 5),
-								iter->second.infMatrix().at<double>(1, 1),
-								iter->second.infMatrix().at<double>(1, 2),
-								iter->second.infMatrix().at<double>(1, 3),
-								iter->second.infMatrix().at<double>(1, 4),
-								iter->second.infMatrix().at<double>(1, 5),
-								iter->second.infMatrix().at<double>(2, 2),
-								iter->second.infMatrix().at<double>(2, 3),
-								iter->second.infMatrix().at<double>(2, 4),
-								iter->second.infMatrix().at<double>(2, 5),
-								iter->second.infMatrix().at<double>(3, 3),
-								iter->second.infMatrix().at<double>(3, 4),
-								iter->second.infMatrix().at<double>(3, 5),
-								iter->second.infMatrix().at<double>(4, 4),
-								iter->second.infMatrix().at<double>(4, 5),
-								iter->second.infMatrix().at<double>(5, 5));
-					}
-					else
-					{
-						// EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
-						fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
-							iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
-							iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
-							PARAM_OFFSET,
-							iter->second.transform().x(),
-							iter->second.transform().y(),
-							iter->second.transform().z(),
-							iter->second.infMatrix().at<double>(0, 0),
-							iter->second.infMatrix().at<double>(0, 1),
-							iter->second.infMatrix().at<double>(0, 2),
-							iter->second.infMatrix().at<double>(1, 1),
-							iter->second.infMatrix().at<double>(1, 2),
-							iter->second.infMatrix().at<double>(2, 2));
-					}
+					// EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
+					fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
+						iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
+						iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
+						PARAM_OFFSET,
+						iter->second.transform().x(),
+						iter->second.transform().y(),
+						iter->second.transform().z(),
+						iter->second.infMatrix().at<double>(0, 0),
+						iter->second.infMatrix().at<double>(0, 1),
+						iter->second.infMatrix().at<double>(0, 2),
+						iter->second.infMatrix().at<double>(1, 1),
+						iter->second.infMatrix().at<double>(1, 2),
+						iter->second.infMatrix().at<double>(2, 2));
 				}
 				continue;
 			}
@@ -527,7 +527,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				float x,y,z,roll,pitch,yaw;
 				std::map<int, Transform> tmpPoses;
-#if GTSAM_VERSION_NUMERIC >= 40200
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
 				for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
 #else
 				for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
@@ -634,7 +634,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());

 		float x,y,z,roll,pitch,yaw;
-#if GTSAM_VERSION_NUMERIC >= 40200
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
 		for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
 #else
 		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
@@ -36,8 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/optimizer/OptimizerTORO.h>

 #ifdef RTABMAP_TORO
-#include "toro3d/treeoptimizer3.h"
-#include "toro3d/treeoptimizer2.h"
+#include "toro3d/treeoptimizer3.hh"
+#include "toro3d/treeoptimizer2.hh"
 #endif

 namespace rtabmap {
@@ -63,14 +63,14 @@ public:

  /** vector of errors */
  Vector attitudeError(const Rot3& p,
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
      OptionalJacobian<2,3> H = {}) const;
 #else
  	  OptionalJacobian<2,3> H = boost::none) const;
 #endif

  /** Serialization function */
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
@@ -91,7 +91,7 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
 public:

  /// shorthand for a smart pointer to a factor
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
  typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
  #else
  typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
@@ -121,7 +121,7 @@ public:

  /// @return a deep copy of this factor
  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
 #else
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
@@ -138,7 +138,7 @@ public:

  /** vector of errors */
  virtual Vector evaluateError(const Rot3& nRb, //
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
 	OptionalMatrixType H = OptionalNone) const {
 #else
 	boost::optional<Matrix&> H = boost::none) const {
@@ -153,7 +153,7 @@ public:
  }

 private:
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
@@ -182,7 +182,7 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
 public:

  /// shorthand for a smart pointer to a factor
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
  typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
 #else
  typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
@@ -211,7 +211,7 @@ public:

  /// @return a deep copy of this factor
  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
 #else
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
@@ -228,7 +228,7 @@ public:

  /** vector of errors */
  virtual Vector evaluateError(const Pose3& nTb, //
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
      OptionalMatrixType H = OptionalNone) const {
 #else
      boost::optional<Matrix&> H = boost::none) const {
@@ -249,7 +249,7 @@ public:
  }

 private:
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
@@ -42,7 +42,7 @@ public:
  // @param p    the pose in Pose2
  // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
  gtsam::Vector evaluateError(const VALUE& p,
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
 		  OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
@@ -42,7 +42,7 @@ public:
  // @param p    the pose in Pose
  // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
  gtsam::Vector evaluateError(const gtsam::Pose3& p,
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
 		  OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
@@ -54,7 +54,7 @@ public:
    return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
  }
  gtsam::Vector evaluateError(const gtsam::Point3& p,
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
 		  OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
@@ -40,8 +40,7 @@
 * such as loading, saving, merging constraints, and etc.
 **/

-#include "posegraph2.h"
-
+#include "posegraph2.hh"
 #include <fstream>
 #include <sstream>
 #include <string>
@@ -43,10 +43,10 @@
 #ifndef _POSEGRAPH2_HH_
 #define _POSEGRAPH2_HH_

+#include "posegraph.hh"
+#include "transformation2.hh"
 #include <iostream>
 #include <vector>
-#include "posegraph.h"
-#include "transformation2.h"

 namespace AISNavigation {

@@ -39,8 +39,7 @@
 * such as loading, saving, merging constraints, and etc.
 **/

-#include "posegraph3.h"
-
+#include "posegraph3.hh"
 #include <fstream>
 #include <sstream>
 #include <string>
@@ -43,10 +43,10 @@
 #ifndef _POSEGRAPH3_HH_
 #define _POSEGRAPH3_HH_

+#include "posegraph.hh"
+#include "transformation3.hh"
 #include <iostream>
 #include <vector>
-#include "posegraph.h"
-#include "transformation3.h"

 typedef unsigned int uint;
 #ifndef M_PI
@@ -39,8 +39,7 @@

 #include <assert.h>
 #include <cmath>
-
-#include "dmatrix.h"
+#include "dmatrix.hh"

 namespace AISNavigation {

@@ -41,8 +41,7 @@
 *
 **/

-#include "treeoptimizer2.h"
-
+#include "treeoptimizer2.hh"
 #include <fstream>
 #include <sstream>
 #include <string>
@@ -44,7 +44,7 @@
 #ifndef _TREEOPTIMIZER2_HH_
 #define _TREEOPTIMIZER2_HH_

-#include "posegraph2.h"
+#include "posegraph2.hh"

 namespace AISNavigation {

@@ -41,8 +41,7 @@
 *
 **/

-#include "treeoptimizer3.h"
-
+#include "treeoptimizer3.hh"
 #include <fstream>
 #include <sstream>
 #include <string>
@@ -44,7 +44,7 @@
 #ifndef _TREEOPTIMIZER3_HH_
 #define _TREEOPTIMIZER3_HH_

-#include "posegraph3.h"
+#include "posegraph3.hh"

 namespace AISNavigation {

@@ -34,9 +34,9 @@
 * PURPOSE.  
 **********************************************************************/

+#include "treeoptimizer3.hh"
 #include <fstream>
 #include <string>
-#include "treeoptimizer3.h"

 using namespace std;

@@ -72,7 +72,7 @@ public:
  /**
   * Clone this value (normal clone on the heap, delete with 'delete' operator)
   */
-#if GTSAM_VERSION_NUMERIC >= 40300
+#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
  virtual std::shared_ptr<gtsam::Value> clone() const {
    return std::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
  }
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	0fcf562042	latest update	2023-06-07 17:14:48 -07:00
matlabbe	d48e2093f5	added fast normal estimation on obstacle segmentation	2023-06-02 16:05:05 -07:00