Fixed warning with vtk 7.1. Dont add target "uninstall" if already added from some cmake dependencies

* GridMap integration

* Removed GridGlobal/FullUpdate parameter. Bump version 0.21.3. Mvoed specialized global map classes under global_map sub dir. Renamed Map -> GlobalMap.

* UI: Added elevation map visualization

* Added LocalGridCache class to share cache between global maps

* Fixed OctoMap nans. DbViewer: Added frontiers visualization.

* convenient functions for ros

* Small fix

* fixed build without GridMap

* CI disabled fail-fast

* CI updated checkout action to v4

.appveyor.yml

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

.devcontainer/devcontainer.json

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

.github/workflows/cmake-ros.yml

@@ -21,28 +21,23 @@ jobs:
     name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
+      fail-fast: false
       matrix:
-        ros_distribution: [ noetic, foxy, humble, rolling]
+        ros_distribution: [ noetic, humble, iron]
         include:
         - ros_distribution: 'noetic'
           os: ubuntu-20.04
-        - ros_distribution: 'foxy'
-          os: ubuntu-20.04
         - ros_distribution: 'humble'
           os: ubuntu-22.04
-        - ros_distribution: 'rolling'
+        - ros_distribution: 'iron'
           os: ubuntu-22.04
 
-    steps:
-    - name: Workaround dpkg grub-efi-amd64-signed error
-      run: |
-        sudo apt-mark hold grub-efi-amd64-signed
-    
-    - uses: ros-tooling/setup-ros@v0.5
+    steps:   
+    - uses: ros-tooling/setup-ros@v0.6
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
     
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v4
 
     - name: Install dependencies
       run: |

.github/workflows/cmake.yml

@@ -16,6 +16,7 @@ jobs:
     name: ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
+      fail-fast: false
       matrix:
         os: [ubuntu-22.04, ubuntu-20.04]
 
@@ -26,7 +27,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@v2
+    - uses: actions/checkout@v4
 
     - name: Configure CMake
       run: |

.github/workflows/docker.yml

@@ -6,22 +6,74 @@ on:
       - 'master'
 
 jobs:
-  docker:
+  docker_deps:
     runs-on: ubuntu-latest
     
     strategy:
+      fail-fast: false
       matrix:
-        docker_tag: [xenial, bionic, focal, focal-foxy, jammy, android23, android24, android26, android30]
+        docker_tag: [focal-deps, jammy-deps, jammy-iron-deps]
         include:
-        - docker_tag: xenial
+        - docker_tag: focal-deps
           docker_tags: |
-            introlab3it/rtabmap:xenial
-            introlab3it/rtabmap:16.04
-          docker_args: |
-            NOT_USED=0
+            introlab3it/rtabmap:focal-deps
           docker_platforms: |
             linux/amd64
-          docker_path: 'xenial'
+            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]
+        include:
         - docker_tag: bionic
           docker_tags: |
             introlab3it/rtabmap:bionic
@@ -43,16 +95,6 @@ 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
@@ -63,6 +105,14 @@ 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

CMakeLists.txt

@@ -1,5 +1,5 @@
 # Top-Level CmakeLists.txt
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.10)
 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 1)
+SET(RTABMAP_PATCH_VERSION 3)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -203,11 +203,12 @@ 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_OCTOMAP       "Include OctoMap support"              ON)
+option(WITH_GRIDMAP       "Include GridMap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
 option(WITH_OPENCHISEL    "Include open_chisel support"          OFF)
 option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)
-option(WITH_FOVIS         "Include FOVIS support"                OFF)
+option(WITH_FOVIS         "Include FOVIS supp++ort"                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)
@@ -218,7 +219,7 @@ option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
-option(WITH_OPENGV        "Include OpenGV support"               OFF)
+option(WITH_OPENGV        "Include OpenGV support"               ON)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -228,7 +229,7 @@ ENDIF()
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
 set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)
 
-FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
+FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
 
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
@@ -325,6 +326,11 @@ 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()
@@ -389,6 +395,7 @@ 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)
 
@@ -518,7 +525,14 @@ ENDIF(WITH_CVSBA)
 IF(WITH_POINTMATCHER)
     find_package(libpointmatcher QUIET)
     IF(libpointmatcher_FOUND)
-       MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
+        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)
     ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)
 
@@ -647,6 +661,13 @@ 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)
@@ -766,7 +787,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))
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND)
       # Qt6 requires c++17
       include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
@@ -777,8 +798,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 ((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
+    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
      include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
       IF(COMPILER_SUPPORTS_CXX14)
@@ -789,22 +810,7 @@ IF(NOT MSVC)
       ENDIF()
     ENDIF()
 
-    IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "17") AND NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
-           G2O_FOUND OR
-           GTSAM_FOUND OR
-           CERES_FOUND OR
-           ZED_FOUND OR
-           ZEDOC_FOUND OR
-           ANDROID OR
-           RealSense_FOUND OR
-           realsense2_FOUND OR
-           ORB_SLAM_FOUND OR
-           okvis_FOUND OR
-           open_chisel_FOUND OR
-           msckf_vio_FOUND OR
-           vins_FOUND OR
-           ov_msckf_FOUND OR
-           libpointmatcher_FOUND))
+    IF(NOT ("${CMAKE_CXX_STANDARD}" STREQUAL "17") AND NOT ("${CMAKE_CXX_STANDARD}" STREQUAL "14"))
      #Newest versions require std11
      include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
@@ -819,6 +825,7 @@ 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))
@@ -903,9 +910,9 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
-IF(NOT opengv_FOUND)
+IF(NOT opengv_FOUND OR NOT WITH_OPENGV)
    SET(OPENGV "//")
-ENDIF(NOT opengv_FOUND)
+ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
 IF(NOT PDAL_FOUND)
    SET(PDAL "//")
 ENDIF(NOT PDAL_FOUND)
@@ -977,10 +984,10 @@ IF(NOT mynteye_FOUND)
    SET(MYNTEYE "//")
 ENDIF(NOT mynteye_FOUND)
 IF(NOT depthai_FOUND)
-   SET(CONF_DEPTH_AI OFF)
+   SET(CONF_WITH_DEPTH_AI 0)
    SET(DEPTHAI "//")
 ELSE()
-   SET(CONF_DEPTH_AI ON) 
+   SET(CONF_WITH_DEPTH_AI 1)
 ENDIF()
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
@@ -988,6 +995,12 @@ 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()
@@ -1029,6 +1042,9 @@ 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)
@@ -1067,6 +1083,7 @@ 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"
@@ -1074,6 +1091,7 @@ CONFIGURE_FILE(
 
 ADD_CUSTOM_TARGET(uninstall
   "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
+ENDIF()
 
 ####
 # Global Export Target
@@ -1446,7 +1464,7 @@ ELSE()
 MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()
 
-IF(opengv_FOUND)
+IF(opengv_FOUND AND WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV ${opengv_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")
@@ -1457,11 +1475,19 @@ 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)")
+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)")
 ENDIF()
 
 IF(CPUTSDF_FOUND)

README.md

@@ -4,11 +4,15 @@ rtabmap
 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
 
 [![Release][release-image]][releases]
+[![Downloads][downloads-image]][downloads]
 [![License][license-image]][license]
 
-[release-image]: https://img.shields.io/badge/release-0.20.16-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
+[downloads]: https://github.com/introlab/rtabmap/releases
+
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
 [license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
 
@@ -50,27 +54,29 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
  <table>
     <tbody>
         <tr>
-           <td rowspan="2">ROS 1</td>
-           <td>Melodic</td>
-            <td><a href="http://build.ros.org/job/Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
+           <td rowspan="1">ROS 1</td>
             <td>Noetic</td>
             <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
         </tr>
         <tr>
             <td rowspan="3">ROS 2</td>
-            <td>Foxy</td>
-            <td><a href="http://build.ros2.org/job/Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
             <td>Humble</td>
             <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
         </tr>
+        <tr>
+            <td>Iron</td>
+            <td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
         <tr>
             <td>Rolling</td>
             <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
         </tr>
+        <tr>
+           <td>Docker</td>
+           <td>
+             <a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
+           </td>
+           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap.svg?label=pulls" alt="Docker Pulls"/></td>
+        </tr>
     </tbody>
 </table>
- 

RTABMapConfig.cmake.in

@@ -42,10 +42,22 @@ IF(@CONF_WITH_K4A@)
     ENDIF()
 ENDIF()
 
+IF(@CONF_WITH_DEPTH_AI@)
+ find_dependency(depthai 2)
+ENDIF()
+
 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 "")

Version.h.in

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

app/android/jni/RTABMapApp.cpp

@@ -649,9 +649,9 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 								UWARN("Cloud %d is empty", id);
 							}
 						}
-						else
+						else if(!data.depthOrRightCompressed().empty() || !data.laserScanCompressed().isEmpty())
 						{
-							UERROR("Failed to uncompress data!");
+							UERROR("Failed to uncompress data! (rgb=%d, depth=%d, scan=%d)", data.imageCompressed().cols, data.depthOrRightCompressed().cols, data.laserScanCompressed().size());
 							status=-2;
 						}
 					}

app/ios/RTABMapApp/install_deps.sh

@@ -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/4a66ebb9faa1dfe997a0860d733bc5473cff20ee/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
+curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
 git apply pcl_1_11_1_vtk_ios_support.patch
 mkdir build
 cd build

cmake_modules/FindORB_SLAM.cmake

@@ -12,6 +12,7 @@ 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)
 
@@ -21,6 +22,9 @@ 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)

corelib/include/rtabmap/core/CameraInfo.h

@@ -45,6 +45,7 @@ public:
 		timeMirroring(0.0f),
 		timeStereoExposureCompensation(0.0f),
 		timeImageDecimation(0.0f),
+		timeHistogramEqualization(0.0f),
 		timeScanFromDepth(0.0f),
 		timeUndistortDepth(0.0f),
 		timeBilateralFiltering(0.0f),
@@ -62,6 +63,7 @@ public:
 	float timeMirroring;
 	float timeStereoExposureCompensation;
 	float timeImageDecimation;
+	float timeHistogramEqualization;
 	float timeScanFromDepth;
 	float timeUndistortDepth;
 	float timeBilateralFiltering;

corelib/include/rtabmap/core/CameraThread.h

@@ -47,10 +47,11 @@ class CameraInfo;
 class SensorData;
 class StereoDense;
 class IMUFilter;
+class Feature2D;
 
 /**
  * Class CameraThread
- *
+ *
  */
 class RTABMAP_CORE_EXPORT CameraThread :
 	public UThread,
@@ -80,6 +81,7 @@ 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);
@@ -87,6 +89,8 @@ 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(
@@ -134,6 +138,7 @@ private:
 	bool _stereoExposureCompensation;
 	bool _colorOnly;
 	int _imageDecimation;
+	int _histogramMethod;
 	bool _stereoToDepth;
 	bool _scanFromDepth;
 	int _scanDownsampleStep;
@@ -150,6 +155,8 @@ private:
 	float _bilateralSigmaR;
 	IMUFilter * _imuFilter;
 	bool _imuBaseFrameConversion;
+	Feature2D * _featureDetector;
+	bool _depthAsMask;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/DBDriver.h

@@ -96,6 +96,10 @@ 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);
 
@@ -231,6 +235,11 @@ 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;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -96,6 +96,11 @@ 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;
@@ -153,6 +158,7 @@ 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;
@@ -165,6 +171,7 @@ 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;

corelib/include/rtabmap/core/GlobalMap.h

@@ -0,0 +1,102 @@
+/*
+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_ */

corelib/include/rtabmap/core/Graph.h

@@ -136,6 +136,12 @@ 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,
@@ -147,6 +153,12 @@ 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,

corelib/include/rtabmap/core/IMUThread.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -39,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+class IMUFilter;
+
 /**
  * Class IMUThread
  *
@@ -53,6 +56,8 @@ public:
 
 	bool init(const std::string & path);
 	void setRate(int rate);
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+	void disableIMUFiltering();
 
 private:
 	virtual void mainLoopBegin();
@@ -65,6 +70,8 @@ private:
 	UTimer frameRateTimer_;
 	double captureDelay_;
 	double previousStamp_;
+	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/LocalGrid.h

@@ -0,0 +1,90 @@
+/*
+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_ */

corelib/include/rtabmap/core/LocalGridMaker.h

@@ -0,0 +1,115 @@
+/*
+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_ */

corelib/include/rtabmap/core/Memory.h

@@ -57,7 +57,7 @@ class RegistrationInfo;
 class RegistrationIcp;
 class RegistrationVis;
 class Stereo;
-class OccupancyGrid;
+class LocalGridMaker;
 class MarkerDetector;
 
 class RTABMAP_CORE_EXPORT Memory
@@ -371,7 +371,7 @@ private:
 	RegistrationIcp * _registrationIcpMulti;
 	RegistrationVis * _registrationVis;
 
-	OccupancyGrid * _occupancy;
+	LocalGridMaker * _localMapMaker;
 
 	MarkerDetector * _markerDetector;
 };

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -25,144 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef CORELIB_SRC_OCCUPANCYGRID_H_
-#define CORELIB_SRC_OCCUPANCYGRID_H_
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_
 
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+/*
+ * Deprecated header, use the one below directly!
+ */
 
-#include <pcl/point_cloud.h>
-#include <pcl/pcl_base.h>
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/global_map/OccupancyGrid.h>
 
-namespace rtabmap {
 
-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_;
-	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_ */
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_ */

corelib/include/rtabmap/core/OctoMap.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -25,223 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef SRC_OCTOMAP_H_
-#define SRC_OCTOMAP_H_
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_
 
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+/*
+ * Deprecated header, use the one below directly!
+ */
 
-#include <octomap/ColorOcTree.h>
-#include <octomap/OcTreeKey.h>
+#include <rtabmap/core/global_map/OctoMap.h>
 
-#include <pcl/pcl_base.h>
-#include <pcl/point_types.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_ */
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_ */

corelib/include/rtabmap/core/Optimizer.h

@@ -79,8 +79,7 @@ public:
 			const std::map<int, Transform> & posesIn,
 			const std::multimap<int, Link> & linksIn,
 			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut,
-			bool adjustPosesWithConstraints = true) const;
+			std::multimap<int, Link> & linksOut) const;
 
 public:
 	virtual ~Optimizer() {}

corelib/include/rtabmap/core/Parameters.h

@@ -376,6 +376,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
     RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
     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.");
@@ -525,12 +527,19 @@ 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.");
-    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).");
+    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).");
+
 
     // Odometry OKVIS
     RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
@@ -575,6 +584,68 @@ 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.");
@@ -585,54 +656,56 @@ 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, 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, 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, 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
@@ -763,8 +836,6 @@ class RTABMAP_CORE_EXPORT Parameters
     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).");

corelib/include/rtabmap/core/PythonInterface.h

@@ -11,31 +11,31 @@
 
 #include <string>
 #include <rtabmap/utilite/UMutex.h>
-#include <Python.h>
+
+namespace pybind11 {
+class scoped_interpreter;
+class gil_scoped_release;
+}
 
 namespace rtabmap {
 
+/**
+ * Create a single PythonInterface on main thread at
+ * global scope before any Python classes.
+ */
 class PythonInterface
 {
 public:
 	PythonInterface();
 	virtual ~PythonInterface();
 
-protected:
-	std::string getTraceback(); // should be called between lock() and unlock()
-	void lock();
-	void unlock();
-
 private:
-	static UMutex mutex_;
-	static int refCount_;
-
-protected:
-	static PyThreadState * mainThreadState_;
-	static unsigned long mainThreadID_;
-	PyThreadState * threadState_;
+	pybind11::scoped_interpreter* guard_;
+	pybind11::gil_scoped_release* release_;
 };
 
+std::string getPythonTraceback();
+
 }
 
 #endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */

corelib/include/rtabmap/core/RegistrationVis.h

@@ -82,7 +82,9 @@ private:
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;
+	int  _PnPVarMedianRatio;
 	float _PnPMaxVar;
+	unsigned int _multiSamplingPolicy;
 	int _correspondencesApproach;
 	int _flowWinSize;
 	int _flowIterations;

corelib/include/rtabmap/core/Rtabmap.h

@@ -326,6 +326,8 @@ private:
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;
+	bool _localizationSmoothing;
+	double _localizationPriorInf;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;

corelib/include/rtabmap/core/camera/CameraDepthAI.h

@@ -49,15 +49,21 @@ public:
 
 public:
 	CameraDepthAI(
-			const std::string & deviceSerial = "",
+			const std::string & mxidOrName = "",
 			int resolution = 1, // 0=720p, 1=800p, 2=400p
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraDepthAI();
 
-	void setOutputDepth(bool enabled, int confidence = 200);
-	void setIMUFirmwareUpdate(bool enabled);
-	void setIMUPublished(bool published);
+	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);
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -69,19 +75,34 @@ protected:
 private:
 #ifdef RTABMAP_DEPTHAI
 	StereoCameraModel stereoModel_;
+	cv::Size targetSize_;
 	Transform imuLocalTransform_;
-	std::string deviceSerial_;
-	bool outputDepth_;
-	int depthConfidence_;
+	std::string mxidOrName_;
+	int outputMode_;
+	int confThreshold_;
+	int lrcThreshold_;
 	int resolution_;
-	bool imuFirmwareUpdate_;
+	bool useSpecTranslation_;
+	float alphaScaling_;
 	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> leftQueue_;
+	std::shared_ptr<dai::DataOutputQueue> leftOrColorQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
-	std::shared_ptr<dai::DataOutputQueue> imuQueue_;
+	std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
+	UMutex imuMutex_;
 #endif
 };
 

corelib/include/rtabmap/core/camera/CameraStereoZed.h

@@ -45,11 +45,11 @@ class RTABMAP_CORE_EXPORT CameraStereoZed :
 {
 public:
 	static bool available();
-
+	static int sdkVersion();
 public:
 	CameraStereoZed(
 			int deviceId,
-			int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
+			int resolution = 6, // 0=HD2K, 1=HD1080, 2=HD1200, 3=HD720, 4=SVGA, 5=VGA, 6=AUTO
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,
@@ -61,7 +61,7 @@ public:
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
 	CameraStereoZed(
 			const std::string & svoFilePath,
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL
 			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,
 			bool computeOdometry = false,

corelib/include/rtabmap/core/global_map/CloudMap.h

@@ -0,0 +1,64 @@
+/*
+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_ */

corelib/include/rtabmap/core/global_map/GridMap.h

@@ -0,0 +1,69 @@
+/*
+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_ */

corelib/include/rtabmap/core/global_map/OccupancyGrid.h

@@ -0,0 +1,69 @@
+/*
+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_ */

corelib/include/rtabmap/core/global_map/OctoMap.h

@@ -0,0 +1,227 @@
+/*
+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_ */

corelib/include/rtabmap/core/impl/LocalMapMaker.hpp

@@ -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_OCCUPANCYGRID_HPP_
-#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_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 OccupancyGrid::segmentCloud(
+typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 		const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
 		const pcl::IndicesPtr & indicesIn,
 		const Transform & pose,
@@ -205,4 +205,4 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 }
 
 
-#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ */
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_ */

corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h

@@ -25,48 +25,38 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef ODOMETRYORBSLAM_H_
-#define ODOMETRYORBSLAM_H_
+#ifndef ODOMETRYORBSLAM2_H_
+#define ODOMETRYORBSLAM2_H_
 
 #include <rtabmap/core/Odometry.h>
 
-#if RTABMAP_ORB_SLAM == 3
-namespace ORB_SLAM3 {
-#else
-namespace ORB_SLAM2 {
-#endif
-class System;
-}
 
-class ORBSLAMSystem;
+class ORBSLAM2System;
 
 namespace rtabmap {
 
-class RTABMAP_CORE_EXPORT OdometryORBSLAM : public Odometry
+class RTABMAP_CORE_EXPORT OdometryORBSLAM2 : public Odometry
 {
 public:
-	OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryORBSLAM();
+	OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryORBSLAM2();
 
 	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:
-#ifdef RTABMAP_ORB_SLAM
-	ORBSLAMSystem * orbslam_;
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+	ORBSLAM2System * orbslam_;
 	bool firstFrame_;
 	Transform originLocalTransform_;
 	Transform previousPose_;
-	bool useIMU_;
-	Transform imuLocalTransform_;
 #endif
 
 };
 
 }
 
-#endif /* ODOMETRYORBSLAM_H_ */
+#endif /* ODOMETRYORBSLAM2_H_ */

corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h

@@ -0,0 +1,71 @@
+/*
+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 & model, 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_ */

corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace ov_msckf {
 class VioManager;
+struct VioManagerOptions;
 }
 
 namespace rtabmap {
@@ -40,7 +41,6 @@ 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
-	ov_msckf::VioManager * vioManager_;
+	std::unique_ptr<ov_msckf::VioManager> vioManager_;
+	std::unique_ptr<ov_msckf::VioManagerOptions> params_;
 	bool initGravity_;
-	Transform previousPose_;
-	Transform previousLocalTransform_;
-	Transform imuLocalTransform_;
-	std::map<double, IMU> imuBuffer_;
+	Transform previousPoseInv_;
+	Transform imuLocalTransformInv_;
+	Eigen::Matrix<double, 6, 6> Phi_;
 #endif
 };
 

corelib/include/rtabmap/core/util2d.h

@@ -156,6 +156,13 @@ 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);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d.h

@@ -144,6 +144,43 @@ 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,
@@ -153,6 +190,28 @@ 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
@@ -164,6 +223,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
  * @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.
  */

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -48,6 +48,7 @@ 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>(),
@@ -59,11 +60,13 @@ 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>(),

corelib/src/CMakeLists.txt

@@ -87,7 +87,8 @@ SET(SRC_FILES
     odometry/OdometryViso2.cpp
     odometry/OdometryDVO.cpp
     odometry/OdometryOkvis.cpp
-    odometry/OdometryORBSLAM.cpp
+    odometry/OdometryORBSLAM2.cpp
+    odometry/OdometryORBSLAM3.cpp
     odometry/OdometryLOAM.cpp
     odometry/OdometryFLOAM.cpp
     odometry/OdometryMSCKF.cpp
@@ -106,7 +107,11 @@ SET(SRC_FILES
     stereo/StereoBM.cpp
     stereo/StereoSGBM.cpp
     
-    OccupancyGrid.cpp
+    GlobalMap.cpp
+    LocalGridMaker.cpp
+    LocalGrid.cpp
+    global_map/OccupancyGrid.cpp
+    global_map/CloudMap.cpp
     
     MarkerDetector.cpp
     
@@ -205,11 +210,15 @@ IF(TORCH_FOUND)
 ENDIF(TORCH_FOUND)
 
 IF(WITH_PYTHON AND Python3_FOUND)
-	SET(LIBRARIES
-		${LIBRARIES}
+	SET(PUBLIC_LIBRARIES
+		${PUBLIC_LIBRARIES}
 		Python3::Python
 		Python3::NumPy
 	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		pybind11::embed
+	)
 	SET(SRC_FILES
 		${SRC_FILES}
 		python/PythonInterface.cpp
@@ -585,10 +594,32 @@ IF(octomap_FOUND)
     ENDIF()
 	SET(SRC_FILES
     	${SRC_FILES}
-		OctoMap.cpp
+		global_map/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}
@@ -750,6 +781,7 @@ 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}")

corelib/src/CameraThread.cpp

@@ -36,10 +36,12 @@ 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/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>
 
@@ -57,6 +59,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_stereoExposureCompensation(false),
 		_colorOnly(false),
 		_imageDecimation(1),
+		_histogramMethod(0),
 		_stereoToDepth(false),
 		_scanFromDepth(false),
 		_scanDownsampleStep(1),
@@ -72,7 +75,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_bilateralSigmaS(10),
 		_bilateralSigmaR(0.1),
 		_imuFilter(0),
-		_imuBaseFrameConversion(false)
+		_imuBaseFrameConversion(false),
+		_featureDetector(0),
+		_depthAsMask(Parameters::defaultVisDepthAsMask())
 {
 	UASSERT(_camera != 0);
 }
@@ -96,6 +101,7 @@ CameraThread::CameraThread(
 			_stereoExposureCompensation(false),
 			_colorOnly(false),
 			_imageDecimation(1),
+			_histogramMethod(0),
 			_stereoToDepth(false),
 			_scanFromDepth(false),
 			_scanDownsampleStep(1),
@@ -111,7 +117,9 @@ CameraThread::CameraThread(
 			_bilateralSigmaS(10),
 			_bilateralSigmaR(0.1),
 			_imuFilter(0),
-			_imuBaseFrameConversion(false)
+			_imuBaseFrameConversion(false),
+			_featureDetector(0),
+			_depthAsMask(Parameters::defaultVisDepthAsMask())
 {
 	UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
 	UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
@@ -134,6 +142,7 @@ CameraThread::CameraThread(
 			_stereoExposureCompensation(false),
 			_colorOnly(false),
 			_imageDecimation(1),
+			_histogramMethod(0),
 			_stereoToDepth(false),
 			_scanFromDepth(false),
 			_scanDownsampleStep(1),
@@ -149,7 +158,9 @@ CameraThread::CameraThread(
 			_bilateralSigmaS(10),
 			_bilateralSigmaR(0.1),
 			_imuFilter(0),
-			_imuBaseFrameConversion(false)
+			_imuBaseFrameConversion(false),
+			_featureDetector(0),
+			_depthAsMask(Parameters::defaultVisDepthAsMask())
 {
 	UASSERT(_camera != 0);
 	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
@@ -163,6 +174,7 @@ CameraThread::~CameraThread()
 	delete _distortionModel;
 	delete _stereoDense;
 	delete _imuFilter;
+	delete _featureDetector;
 }
 
 void CameraThread::setImageRate(float imageRate)
@@ -214,6 +226,30 @@ 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,
@@ -455,9 +491,21 @@ 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)
@@ -493,6 +541,50 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 	}
 
+	if(_histogramMethod && !data.imageRaw().empty())
+	{
+		if(data.imageRaw().type() == CV_8UC1)
+		{
+			UDEBUG("");
+			UTimer timer;
+			cv::Mat image;
+			if(_histogramMethod == 1)
+			{
+				cv::equalizeHist(data.imageRaw(), image);
+				if(!data.depthRaw().empty())
+				{
+					data.setRGBDImage(image, data.depthRaw(), data.cameraModels());
+				}
+				else if(!data.rightRaw().empty())
+				{
+					cv::Mat right;
+					cv::equalizeHist(data.rightRaw(), right);
+					data.setStereoImage(image, right, data.stereoCameraModels()[0]);
+				}
+			}
+			else if(_histogramMethod == 2)
+			{
+				cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0);
+				clahe->apply(data.imageRaw(), image);
+				if(!data.depthRaw().empty())
+				{
+					data.setRGBDImage(image, data.depthRaw(), data.cameraModels());
+				}
+				else if(!data.rightRaw().empty())
+				{
+					cv::Mat right;
+					clahe->apply(data.rightRaw(), right);
+					data.setStereoImage(image, right, data.stereoCameraModels()[0]);
+				}
+			}
+			if(info) info->timeHistogramEqualization = timer.ticks();
+		}
+		else
+		{
+			UWARN("Histogram equalization only supports grayscale images...");
+		}
+	}
+
 	if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
 	{
 		if(data.stereoCameraModels().size()==1)
@@ -673,6 +765,50 @@ 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

corelib/src/DBDriver.cpp

@@ -502,6 +502,16 @@ 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();

corelib/src/DBDriverSqlite3.cpp

@@ -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().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();
+			_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();
 
 			stepNode(ppStmt, *i);
 		}
@@ -4615,6 +4615,39 @@ 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
@@ -5771,6 +5804,131 @@ 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)

corelib/src/Features2d.cpp

@@ -732,19 +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> sub_keypoints;
-			sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
-			limitKeypoints(sub_keypoints, maxFeatures);
+			std::vector<cv::KeyPoint> subKeypoints;
+			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
+			limitKeypoints(subKeypoints, maxFeatures);
 			if(roi.x || roi.y)
 			{
 				// Adjust keypoint position to raw image
-				for(std::vector<cv::KeyPoint>::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter)
+				for(std::vector<cv::KeyPoint>::iterator iter=subKeypoints.begin(); iter!=subKeypoints.end(); ++iter)
 				{
 					iter->pt.x += roi.x;
 					iter->pt.y += roi.y;
 				}
 			}
-			keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
+			keypoints.insert( keypoints.end(), subKeypoints.begin(), subKeypoints.end() );
 		}
 	}
 	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
@@ -2119,6 +2119,24 @@ 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());

corelib/src/GlobalMap.cpp

@@ -0,0 +1,169 @@
+/*
+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

corelib/src/Graph.cpp

@@ -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();
@@ -902,7 +902,7 @@ void computeMaxGraphErrors(
 		float & maxAngularError,
 		const Link ** maxLinearErrorLink,
 		const Link ** maxAngularErrorLink,
-		bool for3DoF)
+		bool force3DoF)
 {
 	maxLinearErrorRatio = -1;
 	maxAngularErrorRatio = -1;
@@ -912,17 +912,44 @@ void computeMaxGraphErrors(
 	UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
-		// ignore links with high variance, priors and landmarks
-		if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
+		// ignore priors
+		if(iter->second.from() != iter->second.to())
 		{
 			Transform t1 = uValue(poses, iter->second.from(), Transform());
 			Transform t2 = uValue(poses, iter->second.to(), Transform());
+
+			if( t1.isNull() ||
+				t2.isNull() ||
+				!t1.isInvertible() ||
+				!t2.isInvertible())
+			{
+				UWARN("Poses are null or not invertible, aborting optimized graph max error check! (Pose %d=%s Pose %d=%s)",
+					iter->second.from(),
+					t1.prettyPrint().c_str(),
+					iter->second.to(),
+					t2.prettyPrint().c_str());
+
+				if(maxLinearErrorLink)
+				{
+					*maxLinearErrorLink = 0;
+				}
+				if(maxAngularErrorLink)
+				{
+					*maxAngularErrorLink = 0;
+				}
+				maxLinearErrorRatio = -1;
+				maxAngularErrorRatio = -1;
+				maxLinearError = -1;
+				maxAngularError = -1;
+				return;
+			}
+
 			Transform t = t1.inverse()*t2;
 
 			float linearError = uMax3(
 					fabs(iter->second.transform().x() - t.x()),
 					fabs(iter->second.transform().y() - t.y()),
-					for3DoF?0:fabs(iter->second.transform().z() - t.z()));
+					force3DoF?0:fabs(iter->second.transform().z() - t.z()));
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
@@ -936,25 +963,30 @@ void computeMaxGraphErrors(
 				}
 			}
 
-			float opt_roll,opt_pitch,opt_yaw;
-			float link_roll,link_pitch,link_yaw;
-			t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
-			iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
-			float angularError = uMax3(
-					for3DoF?0:fabs(opt_roll - link_roll),
-					for3DoF?0:fabs(opt_pitch - link_pitch),
-					fabs(opt_yaw - link_yaw));
-			angularError = angularError>M_PI?2*M_PI-angularError:angularError;
-			UASSERT(iter->second.rotVariance(false)>0.0);
-			float stddevAngular = sqrt(iter->second.rotVariance(false));
-			float angularErrorRatio = angularError/stddevAngular;
-			if(angularErrorRatio > maxAngularErrorRatio)
+			// For landmark links, don't compute angular error if it doesn't estimate orientation
+			if(iter->second.type() != Link::kLandmark ||
+				1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
 			{
-				maxAngularError = angularError;
-				maxAngularErrorRatio = angularErrorRatio;
-				if(maxAngularErrorLink)
+				float opt_roll,opt_pitch,opt_yaw;
+				float link_roll,link_pitch,link_yaw;
+				t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
+				iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+				float angularError = uMax3(
+						force3DoF?0:fabs(opt_roll - link_roll),
+						force3DoF?0:fabs(opt_pitch - link_pitch),
+						fabs(opt_yaw - link_yaw));
+				angularError = angularError>M_PI?2*M_PI-angularError:angularError;
+				UASSERT(iter->second.rotVariance(false)>0.0);
+				float stddevAngular = sqrt(iter->second.rotVariance(false));
+				float angularErrorRatio = angularError/stddevAngular;
+				if(angularErrorRatio > maxAngularErrorRatio)
 				{
-					*maxAngularErrorLink = &iter->second;
+					maxAngularError = angularError;
+					maxAngularErrorRatio = angularErrorRatio;
+					if(maxAngularErrorLink)
+					{
+						*maxAngularErrorLink = &iter->second;
+					}
 				}
 			}
 		}
@@ -1022,6 +1054,39 @@ 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,
@@ -1086,6 +1151,39 @@ 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,
@@ -2218,7 +2316,7 @@ std::map<int, Transform> findNearestPoses(
 	{
 		foundPoses.insert(*poses.find(iter->first));
 	}
-	UDEBUG("found nodes=%d", (int)foundPoses.size());
+	UDEBUG("found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (int)foundPoses.size(), (int)poses.size(), radius, angle, k);
 	return foundPoses;
 }
 

corelib/src/IMUThread.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/IMUThread.h"
 #include "rtabmap/core/IMU.h"
+#include "rtabmap/core/IMUFilter.h"
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -38,13 +39,16 @@ IMUThread::IMUThread(int rate, const Transform & localTransform) :
 		rate_(rate),
 		localTransform_(localTransform),
 		captureDelay_(0.0),
-		previousStamp_(0.0)
+		previousStamp_(0.0),
+		_imuFilter(0),
+		_imuBaseFrameConversion(false)
 {
 }
 
 IMUThread::~IMUThread()
 {
 	imuFile_.close();
+	delete _imuFilter;
 }
 
 bool IMUThread::init(const std::string & path)
@@ -81,6 +85,19 @@ void IMUThread::setRate(int rate)
 	rate_ = rate;
 }
 
+void IMUThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
+{
+	delete _imuFilter;
+	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
+	_imuBaseFrameConversion = baseFrameConversion;
+}
+
+void IMUThread::disableIMUFiltering()
+{
+	delete _imuFilter;
+	_imuFilter = 0;
+}
+
 void IMUThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("IMU");
@@ -141,6 +158,60 @@ void IMUThread::mainLoop()
 		previousStamp_ = stamp;
 
 		IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_);
+
+		// IMU filtering
+		if(_imuFilter && !imu.empty())
+		{
+			if(imu.angularVelocity()[0] == 0 &&
+			   imu.angularVelocity()[1] == 0 &&
+			   imu.angularVelocity()[2] == 0 &&
+			   imu.linearAcceleration()[0] == 0 &&
+			   imu.linearAcceleration()[1] == 0 &&
+			   imu.linearAcceleration()[2] == 0)
+			{
+				UWARN("IMU's acc and gyr values are null! Please disable IMU filtering.");
+			}
+			else
+			{
+				// Transform IMU data in base_link to correctly initialize yaw
+				if(_imuBaseFrameConversion)
+				{
+					UASSERT(!imu.localTransform().isNull());
+					imu.convertToBaseFrame();
+
+				}
+				_imuFilter->update(
+						imu.angularVelocity()[0],
+						imu.angularVelocity()[1],
+						imu.angularVelocity()[2],
+						imu.linearAcceleration()[0],
+						imu.linearAcceleration()[1],
+						imu.linearAcceleration()[2],
+						stamp);
+				double qx,qy,qz,qw;
+				_imuFilter->getOrientation(qx,qy,qz,qw);
+
+				imu = IMU(
+						cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
+						imu.angularVelocity(), imu.angularVelocityCovariance(),
+						imu.linearAcceleration(), imu.linearAccelerationCovariance(),
+						imu.localTransform());
+
+				UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
+							imu.orientation()[0],
+							imu.orientation()[1],
+							imu.orientation()[2],
+							imu.orientation()[3],
+							imu.angularVelocity()[0],
+							imu.angularVelocity()[1],
+							imu.angularVelocity()[2],
+							imu.linearAcceleration()[0],
+							imu.linearAcceleration()[1],
+							imu.linearAcceleration()[2],
+							stamp);
+			}
+		}
+
 		this->post(new IMUEvent(imu, stamp));
 	}
 	else if(!this->isKilled())

corelib/src/Link.cpp

@@ -163,7 +163,7 @@ cv::Mat Link::uncompressUserDataConst() const
 
 Link Link::merge(const Link & link, Type outputType) const
 {
-	UASSERT(to_ == link.from());
+	UASSERT_MSG(to_ == link.from(), uFormat("merging this=%d->%d to link=%d->%d", from_, to_, link.from(), link.to()).c_str());
 	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);

corelib/src/LocalGrid.cpp

@@ -0,0 +1,126 @@
+/*
+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

corelib/src/LocalGridMaker.cpp

@@ -0,0 +1,587 @@
+/*
+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

corelib/src/Memory.cpp

@@ -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 {
 
@@ -153,7 +153,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	}
 	_registrationIcpMulti = new RegistrationIcp(paramsMulti);
 
-	_occupancy = new OccupancyGrid(parameters);
+	_localMapMaker = new LocalGridMaker(parameters);
 	_markerDetector = new MarkerDetector(parameters);
 	this->parseParameters(parameters);
 }
@@ -283,6 +283,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
                                           -landmarkId, inserted.first->second, landmarkSize.at<float>(0,0));
                                 }
                             }
+							else
+							{
+								UDEBUG("Caching landmark size %f for %d", landmarkSize.at<float>(0,0), -landmarkId);
+							}
                         }
 
                         std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
@@ -541,7 +545,7 @@ Memory::~Memory()
 	delete _registrationPipeline;
 	delete _registrationIcpMulti;
 	delete _registrationVis;
-	delete _occupancy;
+	delete _localMapMaker;
 }
 
 void Memory::parseParameters(const ParametersMap & parameters)
@@ -745,9 +749,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		}
 	}
 
-	if(_occupancy)
+	if(_localMapMaker)
 	{
-		_occupancy->parseParameters(params);
+		_localMapMaker->parseParameters(params);
 	}
 
 	if(_markerDetector)
@@ -3702,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 += _occupancy->getMemoryUsed();
+	memoryUsage += sizeof(LocalGridMaker);
 	memoryUsage += sizeof(MarkerDetector);
 	memoryUsage += sizeof(DBDriver);
 
@@ -4966,6 +4970,10 @@ 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())
@@ -5834,14 +5842,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// Occupancy grid map stuff
 	if(_createOccupancyGrid && !isIntermediateNode)
 	{
-		if( (_occupancy->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
-			(!_occupancy->isGridFromDepth() && !data.laserScanRaw().empty()))
+		if( (_localMapMaker->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
+			(!_localMapMaker->isGridFromDepth() && !data.laserScanRaw().empty()))
 		{
 			cv::Mat ground, obstacles, empty;
 			float cellSize = 0.0f;
 			cv::Point3f viewPoint(0,0,0);
-			_occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
-			cellSize = _occupancy->getCellSize();
+			_localMapMaker->createLocalMap(*s, ground, obstacles, empty, viewPoint);
+			cellSize = _localMapMaker->getCellSize();
 			s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
 
 			t = timer.ticks();

corelib/src/Odometry.cpp

@@ -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/OdometryORBSLAM.h"
+#include "rtabmap/core/odometry/OdometryORBSLAM3.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
@@ -51,6 +51,7 @@ 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 {
 
@@ -84,7 +85,11 @@ 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 OdometryORBSLAM(parameters);
+#else
+		odometry = new OdometryORBSLAM3(parameters);
+#endif
 		break;
 	case Odometry::kTypeOkvis:
 		odometry = new OdometryOkvis(parameters);
@@ -324,6 +329,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				imus_.erase(imus_.begin());
 			}
 		}
+		else
+		{
+			UWARN("Received IMU doesn't have orientation set! It is ignored.");
+		}
 	}
 
 
@@ -671,12 +680,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->refCorners[i].x *= _imageDecimation;
-				info->refCorners[i].y *= _imageDecimation;
+				info->newCorners[i].x *= _imageDecimation;
+				info->newCorners[i].y *= _imageDecimation;
 				if(!info->refCorners.empty())
 				{
-					info->newCorners[i].x *= _imageDecimation;
-					info->newCorners[i].y *= _imageDecimation;
+					info->refCorners[i].x *= _imageDecimation;
+					info->refCorners[i].y *= _imageDecimation;
 				}
 			}
 			for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)

corelib/src/Optimizer.cpp

@@ -190,8 +190,7 @@ 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,
-		bool adjustPosesWithConstraints) const
+		std::multimap<int, Link> & linksOut) 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);
@@ -202,15 +201,15 @@ void Optimizer::getConnectedGraph(
 
 	std::set<int> nextPoses;
 	nextPoses.insert(fromId);
-	std::multimap<int, int> biLinks;
+	std::multimap<int, std::pair<int, Link::Type> > 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()) == biLinks.end())
+			if(graph::findLink(biLinks, iter->second.from(), iter->second.to(), true, iter->second.type()) == biLinks.end())
 			{
-				biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
-				biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
+				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())));
 			}
 		}
 	}
@@ -234,41 +233,35 @@ void Optimizer::getConnectedGraph(
 			}
 		}
 
-		for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
+		for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
-			int toId = iter->second;
+			int toId = iter->second.first;
+			Link::Type type = iter->second.second;
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
-				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
+				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
 				if(nextPoses.find(toId) == nextPoses.end())
 				{
 					if(!uContains(posesOut, toId))
 					{
-						if(adjustPosesWithConstraints)
+						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(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
+							if(poseToIn.is3DoF())
 							{
-								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
 							{
-								Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
-								posesOut.insert(std::make_pair(toId, t));
+								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
 							}
 						}
 						else
 						{
-							posesOut.insert(*posesIn.find(toId));
+							posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
 						}
+
 						// add prior links
 						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
 						{
@@ -282,7 +275,7 @@ void Optimizer::getConnectedGraph(
 					}
 
 					// only add unique links
-					if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
+					if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
 					{
 						if(kter->second.to() < 0)
 						{

corelib/src/Parameters.cpp

@@ -214,14 +214,14 @@ ParametersMap Parameters::getDefaultParameters(const std::string & groupIn)
 	return parameters;
 }
 
-ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & group, bool remove)
+ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & groupIn, 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(group) == 0;
+		bool sameGroup = group.compare(groupIn) == 0;
 		if((!remove && sameGroup) || (remove && !sameGroup))
 		{
 			output.insert(*iter);
@@ -236,6 +236,9 @@ 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())));
 
@@ -301,7 +304,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(true,  Parameters::kGridGlobalFullUpdate())));
+		removedParameters_.insert(std::make_pair("Grid/FullUpdate",               std::make_pair(false,  "")));
 
 		// 0.12.1
 		removedParameters_.insert(std::make_pair("Grid/3DGroundIsObstacle",       std::make_pair(true,  Parameters::kGridGroundIsObstacle())));
@@ -812,11 +815,17 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 #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

corelib/src/RegistrationVis.cpp

@@ -30,6 +30,7 @@ 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>
@@ -69,7 +70,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
+		_PnPVarMedianRatio(Parameters::defaultVisPnPVarianceMedianRatio()),
 		_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
+		_multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()),
 		_correspondencesApproach(Parameters::defaultVisCorType()),
 		_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -125,7 +128,9 @@ 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::kVisPnPSamplingPolicy(), _multiSamplingPolicy);
 	Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
 	Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -484,6 +489,7 @@ 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;
@@ -506,7 +512,48 @@ Transform RegistrationVis::computeTransformationImpl(
 					}
 					else
 					{
-						UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored...");
+						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());
 					}
 				}
 
@@ -1048,7 +1095,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());
+							index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams(32, 0, false));
 
 							UASSERT(indices.size() == cornersProjectedMat.rows);
 							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
@@ -1578,11 +1625,13 @@ 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,
@@ -1605,6 +1654,7 @@ 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,

corelib/src/Rtabmap.cpp

@@ -147,6 +147,8 @@ Rtabmap::Rtabmap() :
 	_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
 	_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()),
@@ -618,6 +620,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
 	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);
@@ -849,7 +856,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
 		if(!_memory->isIncremental())
 		{
 			_lastLocalizationPose = initialPose;
-			_localizationCovariance = 0;
+			_localizationCovariance = cv::Mat();
 			_lastLocalizationNodeId = 0;
 			_odomCachePoses.clear();
 			_odomCacheConstraints.clear();
@@ -1665,7 +1672,10 @@ bool Rtabmap::process(
 
 			Link tmp = signature->getLinks().begin()->second.inverse();
 
-			_distanceTravelled += tmp.transform().getNorm();
+			if(!smallDisplacement)
+			{
+				_distanceTravelled += tmp.transform().getNorm();
+			}
 
 			// if the previous node is an intermediate node, remove it from the local graph
 			if(_constraints.size() &&
@@ -1690,11 +1700,11 @@ bool Rtabmap::process(
 			odomCovariance.type() == CV_64FC1 &&
 			odomCovariance.at<double>(0,0) < 1)
 		{
-			if(_localizationCovariance.empty() || _lastLocalizationPose.isNull())
+			if( _memory->isIncremental() && _localizationCovariance.empty())
 			{
-				_localizationCovariance = odomCovariance.clone();
+				_localizationCovariance = cv::Mat::zeros(6,6,CV_64FC1);
 			}
-			else
+			if(_localizationCovariance.total() == 36)
 			{
 #ifdef RTABMAP_MRPT
 				// Transform odometry covariance (which in base frame) into global frame
@@ -1712,7 +1722,6 @@ bool Rtabmap::process(
 				// build rtabmap with MRPT to use approach above.
 				_localizationCovariance += odomCovariance;
 #endif
-
 			}
 		}
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
@@ -1722,7 +1731,10 @@ bool Rtabmap::process(
 			if(!_odomCachePoses.empty())
 			{
 				float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm();
-				_distanceTravelled += odomDistance;
+				if(!smallDisplacement)
+				{
+					_distanceTravelled += odomDistance;
+				}
 
 				while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize)
 				{
@@ -2518,7 +2530,8 @@ bool Rtabmap::process(
 	{
 		if(_startNewMapOnLoopClosure &&
 			_memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place)
-			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session)
+			_localizationCovariance.empty() && // if we didn't localize yet
+			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).",
@@ -2694,6 +2707,15 @@ 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());
+						}
 					}
 				}
 
@@ -3120,6 +3142,7 @@ 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());
@@ -3127,16 +3150,11 @@ 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, cv::Mat::eye(6,6, CV_64FC1)*1000000)));
-						UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str());
+						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);
 					}
-					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());
+					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());
 				}
-				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;
@@ -3144,9 +3162,25 @@ 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, !_graphOptimizer->isSlam2d());
+				_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());
+					}
+				}
 				cv::Mat locOptCovariance;
-				std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, 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");
+				}
 				_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
 				for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
 				{
@@ -3158,7 +3192,7 @@ bool Rtabmap::process(
 					UWARN("Optimization failed, rejecting localization!");
 					rejectLocalization = true;
 				}
-				else if(_optimizationMaxError > 0.0f)
+				else
 				{
 					UINFO("Compute max graph errors...");
 					const Link * maxLinearLink = 0;
@@ -3173,10 +3207,10 @@ bool Rtabmap::process(
 							&maxLinearLink,
 							&maxAngularLink,
 							_graphOptimizer->isSlam2d());
-					if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
+					if(maxLinearLink == 0 && maxAngularLink==0)
 					{
-						UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
-						optPoses = posesOut;
+						UWARN("Could not compute graph errors! Rejecting localization!");
+						rejectLocalization = true;
 					}
 
 					if(maxLinearLink)
@@ -3188,7 +3222,7 @@ bool Rtabmap::process(
 								maxLinearLink->transVariance(),
 								maxLinearError/sqrt(maxLinearLink->transVariance()),
 								_optimizationMaxError);
-						if(maxLinearErrorRatio > _optimizationMaxError)
+						if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
 						{
 							UWARN("Rejecting localization (%d <-> %d) in this "
 									"iteration because a wrong loop closure has been "
@@ -3207,6 +3241,19 @@ 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)
 					{
@@ -3217,7 +3264,7 @@ bool Rtabmap::process(
 								maxAngularLink->rotVariance(),
 								maxAngularError/sqrt(maxAngularLink->rotVariance()),
 								_optimizationMaxError);
-						if(maxAngularErrorRatio > _optimizationMaxError)
+						if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
 						{
 							UWARN("Rejecting localization (%d <-> %d) in this "
 									"iteration because a wrong loop closure has been "
@@ -3236,6 +3283,19 @@ 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());
+						}
 					}
 				}
 
@@ -3259,8 +3319,17 @@ 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, !_graphOptimizer->isSlam2d());
-						optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
+						_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->setPriorsIgnored(priorsIgnored); // set back
 						for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
 						{
@@ -3272,7 +3341,7 @@ bool Rtabmap::process(
 							UWARN("Optimization failed, rejecting localization!");
 							rejectLocalization = true;
 						}
-						else if(_optimizationMaxError > 0.0f)
+						else
 						{
 							UINFO("Compute max graph errors...");
 							const Link * maxLinearLink = 0;
@@ -3287,10 +3356,10 @@ bool Rtabmap::process(
 									&maxLinearLink,
 									&maxAngularLink,
 									_graphOptimizer->isSlam2d());
-							if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
+							if(maxLinearLink == 0 && maxAngularLink==0)
 							{
-								UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
-								optPoses = posesOut;
+								UWARN("Could not compute graph errors! Rejecting localization!");
+								rejectLocalization = true;
 							}
 
 							if(maxLinearLink)
@@ -3302,7 +3371,7 @@ bool Rtabmap::process(
 										maxLinearLink->transVariance(),
 										maxLinearError/sqrt(maxLinearLink->transVariance()),
 										_optimizationMaxError);
-								if(maxLinearErrorRatio > _optimizationMaxError)
+								if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
 								{
 									UWARN("Rejecting localization (%d <-> %d) in this "
 											"iteration because a wrong loop closure has been "
@@ -3321,6 +3390,19 @@ 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)
 							{
@@ -3331,7 +3413,7 @@ bool Rtabmap::process(
 										maxAngularLink->rotVariance(),
 										maxAngularError/sqrt(maxAngularLink->rotVariance()),
 										_optimizationMaxError);
-								if(maxAngularErrorRatio > _optimizationMaxError)
+								if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
 								{
 									UWARN("Rejecting localization (%d <-> %d) in this "
 											"iteration because a wrong loop closure has been "
@@ -3350,6 +3432,19 @@ 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());
+								}
 							}
 						}
 					}
@@ -3395,16 +3490,26 @@ bool Rtabmap::process(
 					Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
 					for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
 					{
-						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());
-						UDEBUG("  to %s", newT.prettyPrint().c_str());
-						iter->second.setTransform(newT);
-
-						// Update link in the referred signatures
-						if(iter->first > 0)
-							_memory->updateLink(iter->second, false);
+						if(!_localizationSmoothing)
+						{
+							// Add original link without optimization
+							UDEBUG("Adding new odom cache constraint %d->%d (%s)", 
+								iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str());
+						}
+						else
+						{
+							// Adjust with optimized poses, this will smooth the localization
+							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());
+							UDEBUG("  to %s", newT.prettyPrint().c_str());
+							iter->second.setTransform(newT);
 
+							// Update link in the referred signatures
+							if(iter->first > 0)
+								_memory->updateLink(iter->second, false);
+						}
+						
 						_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
 					}
 
@@ -3576,7 +3681,6 @@ bool Rtabmap::process(
 				rejectedLandmark = true;
 			}
 			else if(_memory->isIncremental() &&
-			  _optimizationMaxError > 0.0f &&
 			  loopClosureLinksAdded.size() &&
 			  optimizationIterations > 0 &&
 			  constraints.size())
@@ -3602,7 +3706,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(maxLinearErrorRatio > _optimizationMaxError)
+					if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
 					{
 						UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
 							  "iteration because a wrong loop closure has been "
@@ -3622,11 +3726,24 @@ 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(maxAngularErrorRatio > _optimizationMaxError)
+					if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
 					{
 						UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
 							  "iteration because a wrong loop closure has been "
@@ -3646,6 +3763,19 @@ 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)
@@ -3977,15 +4107,16 @@ bool Rtabmap::process(
 		ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
 	}
 
-	Signature lastSignatureData(signature->id());
+	Signature lastSignatureData = *signature;
 	Transform lastSignatureLocalizedPose;
 	if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
 	{
 		lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
 	}
-	if(_publishLastSignatureData)
+	if(!_publishLastSignatureData)
 	{
-		lastSignatureData = *signature;
+		lastSignatureData.sensorData().clearCompressedData();
+		lastSignatureData.sensorData().clearRawData();
 	}
 	if(!_rawDataKept)
 	{
@@ -4268,96 +4399,73 @@ bool Rtabmap::process(
 			poses = _optimizedPoses;
 			constraints = _constraints;
 		}
-		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);
+		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())
+		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())
 			{
-				std::multimap<int, int> missingIds;
+				missingIds.insert(std::make_pair(-1, tmpId));
+			}
 
-				// priority to loopId
-				int tmpId = loopId>0?loopId:_highestHypothesis.first;
-				if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end())
+			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)
 				{
-					missingIds.insert(std::make_pair(-1, tmpId));
-				}
-
-				if(!_lastLocalizationPose.isNull())
-				{
-					// Republish data from closest nodes of the current localization
-					std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
-					int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose);
-					if(id>0)
+					std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true, false, true);
+					for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 					{
-						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())
 						{
-							if(iter->first != loopId &&
-									_nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
-							{
-								missingIds.insert(std::make_pair(iter->second, iter->first));
-							}
+							missingIds.insert(std::make_pair(iter->second, iter->first));
 						}
+					}
 
-						if(_nodesToRepublish.size() != missingIds.size())
+					if(_nodesToRepublish.size() != missingIds.size())
+					{
+						// remove requested nodes not anymore in the graph
+						for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
 						{
-							// remove requested nodes not anymore in the graph
-							for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
+							if(ids.find(*iter) == ids.end())
 							{
-								if(ids.find(*iter) == ids.end())
-								{
-									iter = _nodesToRepublish.erase(iter);
-								}
-								else
-								{
-									++iter;
-								}
+								iter = _nodesToRepublish.erase(iter);
+							}
+							else
+							{
+								++iter;
 							}
 						}
 					}
 				}
+			}
 
-				int loaded = 0;
-				std::stringstream stream;
-				for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter)
-				{
-					statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true));
-					_nodesToRepublish.erase(iter->second);
-					++loaded;
-					stream << iter->second << " ";
-				}
-				if(loaded)
-				{
-					UWARN("Republishing data of requested node(s) %s(%s=%d)",
-							stream.str().c_str(),
-							Parameters::kRtabmapMaxRepublished().c_str(),
-							_maxRepublished);
-				}
-			}
-		}
-		else
-		{
-			// 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)
+			int loaded = 0;
+			std::stringstream stream;
+			for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter)
 			{
-				nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]);
+				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);
 			}
-			nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
-			nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
-			statistics_.addSignatureData(nodeInfo);
 		}
+
 		UDEBUG("");
 		localGraphSize = (int)poses.size();
 		if(!lastSignatureLocalizedPose.isNull())
@@ -5501,106 +5609,130 @@ 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
 
-									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)
+								//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)
 									{
-										if(ster->second.mapId() == mapId)
+										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)
 										{
-											fromId = ster->first;
-											break;
+											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());
 										}
 									}
-									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())
+									else if(maxAngularLink)
 									{
-										graph::computeMaxGraphErrors(
-												optimizedPoses,
-												links,
-												maxLinearErrorRatio,
-												maxAngularErrorRatio,
-												maxLinearError,
-												maxAngularError,
-												&maxLinearLink,
-												&maxAngularLink);
-										if(maxLinearLink)
+										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)
 										{
-											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);
-											}
+											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 if(maxAngularLink)
+										else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust())
 										{
-											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);
-											}
+											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());
 										}
 									}
-									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)
@@ -5883,7 +6015,7 @@ bool Rtabmap::addLink(const Link & link)
 		{
 			msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to());
 		}
-		else if(_optimizationMaxError > 0.0f)
+		else
 		{
 			float maxLinearError = 0.0f;
 			float maxLinearErrorRatio = 0.0f;
@@ -5904,7 +6036,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(maxLinearErrorRatio > _optimizationMaxError)
+				if(_optimizationMaxError > 0.0f && 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). "
@@ -5919,11 +6051,24 @@ 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(maxAngularErrorRatio > _optimizationMaxError)
+				if(_optimizationMaxError > 0.0f && 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). "
@@ -5938,6 +6083,19 @@ 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())
@@ -6042,7 +6200,7 @@ bool Rtabmap::addLink(const Link & link)
 			UWARN("Optimization failed, rejecting localization!");
 			rejectLocalization = true;
 		}
-		else if(_optimizationMaxError > 0.0f)
+		else
 		{
 			UINFO("Compute max graph errors...");
 			float maxLinearError = 0.0f;
@@ -6069,7 +6227,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(maxLinearErrorRatio > _optimizationMaxError)
+				if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
 				{
 					UWARN("Rejecting localization (%d <-> %d) in this "
 							"iteration because a wrong loop closure has been "
@@ -6088,11 +6246,24 @@ 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(maxAngularErrorRatio > _optimizationMaxError)
+				if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
 				{
 					UWARN("Rejecting localization (%d <-> %d) in this "
 							"iteration because a wrong loop closure has been "
@@ -6111,6 +6282,19 @@ 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());
+				}
 			}
 		}
 

corelib/src/SensorData.cpp

@@ -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.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()+
+			(_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())+
 			_keypoints.size() * sizeof(cv::KeyPoint) +
 			_keypoints3D.size() * sizeof(cv::Point3f) +
-			_descriptors.total()*_descriptors.elemSize();
+			(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
 }
 
 void SensorData::clearCompressedData(bool images, bool scan, bool userData)

corelib/src/Signature.cpp

@@ -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.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
+	total += _wordsDescriptors.empty()?0:_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)
 	{

corelib/src/Transform.cpp

@@ -211,14 +211,24 @@ Transform Transform::to3DoF() const
 {
 	float x,y,z,roll,pitch,yaw;
 	this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-	return Transform(x,y,0, 0,0,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);
 }
 
 Transform Transform::to4DoF() const
 {
 	float x,y,z,roll,pitch,yaw;
 	this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-	return Transform(x,y,z, 0,0,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);
 }
 
 bool Transform::is3DoF() const
@@ -232,7 +242,7 @@ bool Transform::is4DoF() const
 		   r23() == 0.0 &&
 		   r31() == 0.0 &&
 		   r32() == 0.0 &&
-		   r33() == 0.0;
+		   r33() == 1.0;
 }
 
 cv::Mat Transform::rotationMatrix() const

corelib/src/camera/CameraDepthAI.cpp

@@ -26,6 +26,7 @@ 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>
@@ -45,19 +46,31 @@ bool CameraDepthAI::available()
 }
 
 CameraDepthAI::CameraDepthAI(
-		const std::string & deviceSerial,
+		const std::string & mxidOrName,
 		int resolution,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_DEPTHAI
 	,
-	deviceSerial_(deviceSerial),
-	outputDepth_(false),
-	depthConfidence_(200),
+	mxidOrName_(mxidOrName),
+	outputMode_(0),
+	confThreshold_(200),
+	lrcThreshold_(5),
 	resolution_(resolution),
-	imuFirmwareUpdate_(false),
-	imuPublished_(true)
+	useSpecTranslation_(false),
+	alphaScaling_(0.0),
+	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)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
@@ -75,32 +88,90 @@ CameraDepthAI::~CameraDepthAI()
 #endif
 }
 
-void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
+void CameraDepthAI::setOutputMode(int outputMode)
 {
 #ifdef RTABMAP_DEPTHAI
-	outputDepth_ = enabled;
-	if(outputDepth_)
-	{
-		depthConfidence_ = confidence;
-	}
+	outputMode_ = outputMode;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }
 
-void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
+void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold)
 {
 #ifdef RTABMAP_DEPTHAI
-	imuFirmwareUpdate_ = enabled;
+	confThreshold_ = confThreshold;
+	lrcThreshold_ = lrcThreshold;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
 }
 
-void CameraDepthAI::setIMUPublished(bool published)
+void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling)
 {
 #ifdef RTABMAP_DEPTHAI
-	imuPublished_ = published;
+	useSpecTranslation_ = useSpecTranslation;
+	alphaScaling_ = alphaScaling;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
+{
+#ifdef RTABMAP_DEPTHAI
+	imuPublished_ = imuPublished;
+	publishInterIMU_ = publishInterIMU;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setIrBrightness(float dotProjectormA, float floodLightmA)
+{
+#ifdef RTABMAP_DEPTHAI
+	dotProjectormA_ = dotProjectormA;
+	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
@@ -112,107 +183,204 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 #ifdef RTABMAP_DEPTHAI
 
 	std::vector<dai::DeviceInfo> devices = dai::Device::getAllAvailableDevices();
-	if(devices.empty())
+	if(devices.empty() && mxidOrName_.empty())
 	{
+		UERROR("No DepthAI device found or specified");
 		return false;
 	}
 
 	if(device_.get())
-	{
 		device_->close();
-	}
+
 	accBuffer_.clear();
 	gyroBuffer_.clear();
 
-	dai::DeviceInfo deviceToUse;
-	if(deviceSerial_.empty())
-		deviceToUse = devices[0];
-	for(size_t i=0; i<devices.size(); ++i)
-	{
-		UINFO("DepthAI device found: %s", devices[i].getMxId().c_str());
-		if(!deviceSerial_.empty() && deviceSerial_.compare(devices[i].getMxId()) == 0)
-		{
-			deviceToUse = devices[i];
-		}
-	}
+	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(deviceToUse.getMxId().empty())
+	if(!deviceFound)
 	{
-		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());
-		}
+		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());
 		return false;
 	}
-	deviceSerial_ = deviceToUse.getMxId();
 
 	// look for calibration files
 	stereoModel_ = StereoCameraModel();
-	cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
+	targetSize_ = cv::Size(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 xoutLeft = p.create<dai::node::XLinkOut>();
+	auto xoutLeftOrColor = 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
-	xoutLeft->setStreamName("rectified_left");
-	xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
+	xoutLeftOrColor->setStreamName(outputMode_<2?"rectified_left":"rectified_color");
+	xoutDepthOrRight->setStreamName(outputMode_?"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_);
-	monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
-	if(this->getImageRate()>0)
+	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)
 	{
 		monoLeft->setFps(this->getImageRate());
 		monoRight->setFps(this->getImageRate());
 	}
 
 	// StereoDepth
-	stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
-	stereo->initialConfig.setLeftRightCheckThreshold(5);
-	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
-	stereo->setLeftRightCheck(true);
-	stereo->setSubpixel(false);
+	if(outputMode_ == 2)
+		stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
+	else
+		stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
 	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.setLeftRightCheck(true);
+	stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
+	stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
+	auto config = stereo->initialConfig.get();
+	config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
+	config.censusTransform.kernelMask = 0X2AA00AA805540155;
+	config.postProcessing.brightnessFilter.maxBrightness = 255;
+	stereo->initialConfig.set(config);
 
 	// Link plugins CAM -> STEREO -> XLINK
 	monoLeft->out.link(stereo->left);
 	monoRight->out.link(stereo->right);
 
-	if(outputDepth_)
+	if(outputMode_ == 2)
 	{
-		// Depth is registered to right image by default, so subscribe to right image when depth is used
-		if(outputDepth_)
-			stereo->rectifiedRight.link(xoutLeft->input);
+		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
-			stereo->rectifiedLeft.link(xoutLeft->input);
-		stereo->depth.link(xoutDepthOrRight->input);
+		{
+			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);
 	}
 	else
 	{
-		stereo->rectifiedLeft.link(xoutLeft->input);
-		stereo->rectifiedRight.link(xoutDepthOrRight->input);
+		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);
 	}
 
 	if(imuPublished_)
 	{
-		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
-		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
+		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 100 hz rate
+		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100);
 		// 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
@@ -222,39 +390,113 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 		// Link plugins IMU -> XLINK
 		imu->out.link(xoutIMU->input);
+	}
 
-		imu->enableFirmwareUpdate(imuFirmwareUpdate_);
+	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);
 	}
 
 	device_.reset(new dai::Device(p, deviceToUse));
 
 	UINFO("Loading eeprom calibration data");
 	dai::CalibrationHandler calibHandler = device_->readCalibration();
-	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];
-	matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT);
-	double baseline = matrix[0][3]/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);
+
+	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_);
 
 	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::LEFT);
+		//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::CAM_B);
 		//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],
 		//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
-		// Hard-coded: x->down, y->left, z->forward
-		imuLocalTransform_ = Transform(
-				 0, 0, 1, 0,
-				 0, 1, 0, 0,
-				-1 ,0, 0, 0);
-		UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+		auto eeprom = calibHandler.getEepromData();
+		if(eeprom.boardName == "OAK-D" ||
+		   eeprom.boardName == "BW1098OBC")
+		{
+			imuLocalTransform_ = Transform(
+				 0, -1,  0,  0.0525,
+				 1,  0,  0,  0.013662,
+				 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);
+		}
+		else
+		{
+			UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str());
+			imuPublished_ = false;
+		}
 	}
 	else
 	{
@@ -263,10 +505,46 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 	if(imuPublished_)
 	{
-		imuQueue_ = device_->getOutputQueue("imu", 50, false);
+		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)
+			{
+				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(), accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z));
+					gyroBuffer_.emplace_hint(gyroBuffer_.end(), 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);
+
+	std::vector<std::tuple<std::string, int, int>> irDrivers = device_->getIrDrivers();
+	if(!irDrivers.empty())
+	{
+		device_->setIrLaserDotProjectorBrightness(dotProjectormA_);
+		device_->setIrFloodLightBrightness(floodLightmA_);
 	}
-	leftQueue_ = device_->getOutputQueue("rectified_left", 1, false);
-	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false);
 
 	uSleep(2000); // avoid bad frames on start
 
@@ -289,7 +567,7 @@ bool CameraDepthAI::isCalibrated() const
 std::string CameraDepthAI::getSerial() const
 {
 #ifdef RTABMAP_DEPTHAI
-	return deviceSerial_;
+	return device_->getMxId();
 #endif
 	return "";
 }
@@ -299,177 +577,163 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	SensorData data;
 #ifdef RTABMAP_DEPTHAI
 
-	cv::Mat left, depthOrRight;
-	auto rectifL = leftQueue_->get<dai::ImgFrame>();
+	cv::Mat leftOrColor, depthOrRight;
+	auto rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
 	auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
 
-	if(rectifL.get() && rectifRightOrDepth.get())
+	while(rectifLeftOrColor->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
+		rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
+	while(rectifLeftOrColor->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)
 	{
-		auto stampLeft = rectifL->getTimestamp().time_since_epoch().count();
-		auto stampRight = rectifRightOrDepth->getTimestamp().time_since_epoch().count();
-		double stamp = double(stampLeft)/10e8;
-		left = rectifL->getCvFrame();
-		depthOrRight = rectifRightOrDepth->getCvFrame();
-
-		if(!left.empty() && !depthOrRight.empty())
+		leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
+		depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE);
+		if(outputMode_)
 		{
-			if(depthOrRight.type() == CV_8UC1)
-			{
-				if(stereoModel_.isValidForRectification())
-				{
-					left = stereoModel_.left().rectifyImage(left);
-					depthOrRight = stereoModel_.right().rectifyImage(depthOrRight);
-				}
-				data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
-			}
-			else
-			{
-				data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
-			}
-
-			if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms
-			{
-				UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8);
-			}
-
-			//get imu
-			double stampStart = UTimer::now();
-			while(imuPublished_ && imuQueue_.get())
-			{
-				if(imuQueue_->has())
-				{
-					auto imuData = imuQueue_->get<dai::IMUData>();
-
-					auto imuPackets = imuData->packets;
-					double accStamp = 0.0;
-					double gyroStamp = 0.0;
-					for(auto& imuPacket : imuPackets) {
-						auto& acceleroValues = imuPacket.acceleroMeter;
-						auto& gyroValues = imuPacket.gyroscope;
-
-						accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
-						gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
-						accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
-						gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
-						if(accBuffer_.size() > 1000)
-						{
-							accBuffer_.erase(accBuffer_.begin());
-						}
-						if(gyroBuffer_.size() > 1000)
-						{
-							gyroBuffer_.erase(gyroBuffer_.begin());
-						}
-					}
-					if(accStamp >= stamp && gyroStamp >= stamp)
-					{
-						break;
-					}
-				}
-				if((UTimer::now() - stampStart) > 0.01)
-				{
-					UWARN("Could not received IMU after 10 ms! Disabling IMU!");
-					imuPublished_ = false;
-				}
-			}
-
-			cv::Vec3d acc, gyro;
-			bool valid = !accBuffer_.empty() && !gyroBuffer_.empty();
-			//acc
-			if(!accBuffer_.empty())
-			{
-				std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
-				std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
-				if(iterA != accBuffer_.begin())
-				{
-					iterA = --iterA;
-				}
-				if(iterB == accBuffer_.end())
-				{
-					iterB = --iterB;
-				}
-				if(iterA == iterB && stamp == iterA->first)
-				{
-					acc[0] = iterA->second[0];
-					acc[1] = iterA->second[1];
-					acc[2] = iterA->second[2];
-				}
-				else if(stamp >= iterA->first && stamp <= iterB->first)
-				{
-					float t = (stamp-iterA->first) / (iterB->first-iterA->first);
-					acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
-					acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
-					acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
-				}
-				else
-				{
-					valid = false;
-					if(stamp < iterA->first)
-					{
-						UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
-					}
-					else if(stamp > iterB->first)
-					{
-						UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
-					}
-					else
-					{
-						UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
-					}
-				}
-			}
-			//gyro
-			if(!gyroBuffer_.empty())
-			{
-				std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
-				std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
-				if(iterA != gyroBuffer_.begin())
-				{
-					iterA = --iterA;
-				}
-				if(iterB == gyroBuffer_.end())
-				{
-					iterB = --iterB;
-				}
-				if(iterA == iterB && stamp == iterA->first)
-				{
-					gyro[0] = iterA->second[0];
-					gyro[1] = iterA->second[1];
-					gyro[2] = iterA->second[2];
-				}
-				else if(stamp >= iterA->first && stamp <= iterB->first)
-				{
-					float t = (stamp-iterA->first) / (iterB->first-iterA->first);
-					gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
-					gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
-					gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
-				}
-				else
-				{
-					valid = false;
-					if(stamp < iterA->first)
-					{
-						UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
-					}
-					else if(stamp > iterB->first)
-					{
-						UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
-					}
-					else
-					{
-						UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
-					}
-				}
-			}
-
-			if(valid)
-			{
-				data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
-			}
+			cv::Mat disp;
+			depthOrRight.convertTo(disp, CV_16UC1);
+			cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight);
 		}
 	}
 	else
 	{
-		UWARN("Null images received!?");
+		leftOrColor = rectifLeftOrColor->getCvFrame();
+		depthOrRight = rectifRightOrDepth->getCvFrame();
+	}
+
+	if(outputMode_)
+		data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
+	else
+		data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
+
+	if(imuPublished_ && !publishInterIMU_)
+	{
+		cv::Vec3d acc, gyro;
+		std::map<double, cv::Vec3f>::const_iterator iterA, iterB;
+	
+		imuMutex_.lock();
+		while(accBuffer_.empty() || gyroBuffer_.empty() || accBuffer_.rbegin()->first < stamp || gyroBuffer_.rbegin()->first < stamp)
+		{
+			imuMutex_.unlock();
+			uSleep(1);
+			imuMutex_.lock();
+		}
+
+		//acc
+		iterB = accBuffer_.lower_bound(stamp);
+		iterA = iterB;
+		if(iterA != accBuffer_.begin())
+			iterA = --iterA;
+		if(iterA == iterB || stamp == iterB->first)
+		{
+			acc = iterB->second;
+		}
+		else if(stamp > iterA->first && stamp < iterB->first)
+		{
+			float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+			acc = iterA->second + t*(iterB->second - iterA->second);
+		}
+		accBuffer_.erase(accBuffer_.begin(), iterB);
+
+		//gyro
+		iterB = gyroBuffer_.lower_bound(stamp);
+		iterA = iterB;
+		if(iterA != gyroBuffer_.begin())
+			iterA = --iterA;
+		if(iterA == iterB || stamp == iterB->first)
+		{
+			gyro = iterB->second;
+		}
+		else if(stamp > iterA->first && stamp < iterB->first)
+		{
+			float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+			gyro = iterA->second + t*(iterB->second - iterA->second);
+		}
+		gyroBuffer_.erase(gyroBuffer_.begin(), iterB);
+
+		imuMutex_.unlock();
+		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

corelib/src/camera/CameraRealSense2.cpp

@@ -1501,7 +1501,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 						getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
 						if(!imuTmp.empty())
 						{
-							UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
+							UEventsManager::post(new IMUEvent(imuTmp, stamps[i]/1000.0));
 							pub++;
 						}
 						else

corelib/src/camera/CameraStereoZed.cpp

@@ -240,6 +240,17 @@ bool CameraStereoZed::available()
 #endif
 }
 
+
+int CameraStereoZed::sdkVersion()
+{
+#ifdef RTABMAP_ZED
+	return ZED_SDK_MAJOR_VERSION;
+#else
+	return -1;
+#endif
+}
+
+
 CameraStereoZed::CameraStereoZed(
 		int deviceId,
 		int resolution,
@@ -274,6 +285,16 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
+#if ZED_SDK_MAJOR_VERSION < 4
+	if(resolution_ == 3)
+	{
+		resolution_ = 2;
+	}
+	else if(resolution_ == 5)
+	{
+		resolution_ = 3;
+	}
+#endif
 #if ZED_SDK_MAJOR_VERSION < 3
 	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
 	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
@@ -282,11 +303,15 @@ CameraStereoZed::CameraStereoZed(
 #else
     sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
     sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
-    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
 
     UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
+#if ZED_SDK_MAJOR_VERSION < 4
+    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
     UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
+#else
+    UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
+#endif
     UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
     UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
 #endif
@@ -334,11 +359,15 @@ CameraStereoZed::CameraStereoZed(
 #else
     sl::RESOLUTION res = static_cast<sl::RESOLUTION>(resolution_);
     sl::DEPTH_MODE qual = static_cast<sl::DEPTH_MODE>(quality_);
-    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
 
     UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST);
     UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST);
+#if ZED_SDK_MAJOR_VERSION < 4
+    sl::SENSING_MODE sens = static_cast<sl::SENSING_MODE>(sensingMode_);
     UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST);
+#else
+    UASSERT(sensingMode_ >= 0 && sensingMode_ < 2);
+#endif
     UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
     UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100);
 #endif
@@ -465,7 +494,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	}
 
 	sl::CameraInformation infos = zed_->getCameraInformation();
+#if ZED_SDK_MAJOR_VERSION < 4
 	sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
+#else
+	sl::CalibrationParameters *stereoParams = &(infos.camera_configuration.calibration_parameters );
+#endif
 	sl::Resolution res = stereoParams->left_cam.image_size;
 
 	stereoModel_ = StereoCameraModel(
@@ -473,7 +506,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		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
 		this->getLocalTransform(),
 		cv::Size(res.width, res.height));
 
@@ -482,7 +519,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 			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());
@@ -493,11 +534,18 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
     if(infos.camera_model != sl::MODEL::ZED)
 #endif
 	{
+#if ZED_SDK_MAJOR_VERSION < 4
 		imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse();
+#else
+		imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).inverse();
+#endif
 		UINFO("IMU local transform: %s (imu2cam=%s))",
-				imuLocalTransform_.prettyPrint().c_str(),
-				zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
-
+		imuLocalTransform_.prettyPrint().c_str(),
+#if ZED_SDK_MAJOR_VERSION < 4
+		zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str());
+#else
+		zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
+#endif
 		if(publishInterIMU_)
 		{
 			imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true);
@@ -623,8 +671,10 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 #ifdef RTABMAP_ZED
 #if ZED_SDK_MAJOR_VERSION < 3
 	sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
-#else
+#elif ZED_SDK_MAJOR_VERSION < 4
     sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
+#else
+    sl::RuntimeParameters rparam(quality_ > 0, sensingMode_ == 1, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA);
 #endif
 
 	if(zed_)

corelib/src/global_map/CloudMap.cpp

@@ -0,0 +1,153 @@
+/*
+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;
+}
+
+}

corelib/src/global_map/GridMap.cpp

@@ -0,0 +1,485 @@
+/*
+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());
+}
+
+}

corelib/src/global_map/OccupancyGrid.cpp

@@ -0,0 +1,682 @@
+/*
+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;
+}
+
+}

corelib/src/global_map/OctoMap.cpp

@@ -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/OctoMap.h>
+#include <rtabmap/core/global_map/OctoMap.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -288,55 +288,40 @@ RtabmapColorOcTree::StaticMemberInitializer RtabmapColorOcTree::RtabmapColorOcTr
 // OctoMap
 //////////////////////////////////////
 
-OctoMap::OctoMap(const ParametersMap & parameters) :
+OctoMap::OctoMap(const LocalGridCache * cache, const ParametersMap & parameters) :
+		GlobalMap(cache, parameters),
 		hasColor_(false),
-		fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
-		updateError_(Parameters::defaultGridGlobalUpdateError()),
 		rangeMax_(Parameters::defaultGridRangeMax()),
 		rayTracing_(Parameters::defaultGridRayTracing()),
 		emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth())
 {
-	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)
+	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_);
-   
-	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
+	
+	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::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_);
@@ -351,47 +336,17 @@ OctoMap::~OctoMap()
 void OctoMap::clear()
 {
 	octree_->clear();
-	cache_.clear();
-	cacheClouds_.clear();
-	cacheViewPoints_.clear();
-	addedNodes_.clear();
 	hasColor_ = false;
-	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
-	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+	GlobalMap::clear();
 }
 
-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 OctoMap::getMemoryUsed() const
 {
-	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));
+	unsigned long memoryUsage = GlobalMap::getMemoryUsed();
+
+	// Note: size of OctoMap object is missing.
+
+	return memoryUsage;
 }
 
 bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
@@ -509,227 +464,67 @@ std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> OctoMap::fin
 }
 
 
-bool OctoMap::update(const std::map<int, Transform> & poses)
+void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 {
-	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 = addedNodes_.size()?addedNodes_.rbegin()->first:0;
+	int lastId = assembledNodes().size()?assembledNodes().rbegin()->first:0;
 	UDEBUG("Last id = %d", lastId);
 
-	// 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);
-		}
-	}
+	UDEBUG("newPoses = %d", (int)newPoses.size());
 
-	// 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())
+	if(!newPoses.empty())
 	{
 		float rangeMaxSqrd = rangeMax_*rangeMax_;
 		float cellSize = octree_->getResolution();
-		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
+		for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
-			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())
+			std::map<int, LocalGrid>::const_iterator localGridIter;
+			localGridIter = cache().find(iter->first);
+			if(localGridIter != cache().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(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
+				sensorOrigin += octomap::point3d(localGridIter->second.viewPoint.x, localGridIter->second.viewPoint.y, localGridIter->second.viewPoint.z);
 
 				updateMinMax(sensorOrigin);
 
 				octomap::OcTreeKey tmpKey;
-				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
-						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
+				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey))
 				{
 					UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
 				}
 
-				bool computeRays = rayTracing_ && (occupancyIter == cache_.end() || occupancyIter->second.second.empty());
+				bool computeRays = rayTracing_ && emptyCells.empty();
 
 				// instead of direct scan insertion, compute update to filter ground:
 				octomap::KeySet free_cells;
 				// insert ground points only as free:
-				unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
+				unsigned int maxGroundPts = ground.cols;
 				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
 				Eigen::Affine3f t = iter->second.toEigen3f();
-				LaserScan tmpGround;
-				if(occupancyIter != cache_.end())
-				{
-					tmpGround = LaserScan::backwardCompatibility(occupancyIter->second.first.first);
-					UASSERT(tmpGround.size() == (int)maxGroundPts);
-				}
+				LaserScan tmpGround = LaserScan::backwardCompatibility(ground);
+				UASSERT(tmpGround.size() == (int)maxGroundPts);
 				for (unsigned int i=0; i<maxGroundPts; ++i)
 				{
 					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(tmpGround, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
-					}
+					pt = util3d::laserScanToPointRGB(tmpGround, i);
+					pt = pcl::transformPoint(pt, t);
+
 					octomap::point3d point(pt.x, pt.y, pt.z);
 					bool ignoreOccupiedCell = false;
 					if(rangeMaxSqrd > 0.0f)
@@ -793,26 +588,15 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 				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 = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
+				unsigned int maxObstaclePts = obstacles.cols;
 				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
-				LaserScan tmpObstacle;
-				if(occupancyIter != cache_.end())
-				{
-					tmpObstacle = LaserScan::backwardCompatibility(occupancyIter->second.first.second);
-					UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
-				}
+				LaserScan tmpObstacle = LaserScan::backwardCompatibility(obstacles);
+				UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
 				for (unsigned int i=0; i<maxObstaclePts; ++i)
 				{
 					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(tmpObstacle, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
-					}
+					pt = util3d::laserScanToPointRGB(tmpObstacle, i);
+					pt = pcl::transformPoint(pt, t);
 
 					octomap::point3d point(pt.x, pt.y, pt.z);
 
@@ -903,11 +687,11 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 				}
 
 				// all empty cells
-				if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
+				if(emptyCells.cols)
 				{
-					unsigned int maxEmptyPts = occupancyIter->second.second.cols;
+					unsigned int maxEmptyPts = emptyCells.cols;
 					UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
-					LaserScan tmpEmpty = LaserScan::backwardCompatibility(occupancyIter->second.second);
+					LaserScan tmpEmpty = LaserScan::backwardCompatibility(emptyCells);
 					UASSERT(tmpEmpty.size() == (int)maxEmptyPts);
 					for (unsigned int i=0; i<maxEmptyPts; ++i)
 					{
@@ -959,22 +743,18 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 					}
 				}
 
-				if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty())
+				if(emptyCells.cols || !free_cells.empty())
 				{
 					octree_->updateInnerOccupancy();
 				}
 
 				// compress map
-				//if(orderedPoses.size() > 1)
+				//if(newPoses.size() > 1)
 				//{
 				//	octree_->prune();
 				//}
 
-				// ignore negative ids as they are temporary clouds
-				if(iter->first > 0)
-				{
-					addedNodes_.insert(*iter);
-				}
+				addAssembledNode(iter->first, iter->second);
 				UDEBUG("%d: end", iter->first);
 			}
 			else
@@ -1005,21 +785,12 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
             }
         }
 
-
         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)

corelib/src/icp/libpointmatcher.h

@@ -513,16 +513,28 @@ 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)
 				{
-					float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
-					if(distIntensity < minDistance)
+					int matchesIdsCoeff = matches.ids.coeff(k, i);
+					if (matchesIdsCoeff!=-1)
 					{
-						matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
-						matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
-						minDistance = distIntensity;
+						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;
+						}
 					}
 				}
+
+				if (!minDistFound)
+				{
+					matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(0, i);
+					matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(0, i);
+				}
 			}
 			matches = matchesOrderedByIntensity;
 		}

corelib/src/odometry/OdometryF2F.cpp

@@ -74,15 +74,15 @@ Transform OdometryF2F::computeTransform(
 	UTimer timer;
 	Transform output;
 	if(!data.rightRaw().empty() &&
-	   (data.stereoCameraModels().size() != 1 || !data.stereoCameraModels()[0].isValidForProjection()))
+	   (data.stereoCameraModels().empty() || !data.stereoCameraModels()[0].isValidForProjection()))
 	{
-		UERROR("Calibrated stereo camera required (multi-cameras not supported)");
+		UERROR("Calibrated stereo camera required.");
 		return output;
 	}
 	if(!data.depthRaw().empty() &&
-		(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValidForProjection()))
+		(data.cameraModels().empty() || !data.cameraModels()[0].isValidForProjection()))
 	{
-		UERROR("Calibrated camera required (multi-cameras not supported).");
+		UERROR("Calibrated camera required.");
 		return output;
 	}
 

corelib/src/odometry/OdometryF2M.cpp

@@ -214,6 +214,11 @@ Transform OdometryF2M::computeTransform(
 	if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
 	{
 		imuT = Transform::getTransform(imus(), data.stamp());
+		if(data.imu().empty())
+		{
+			Eigen::Quaternionf q = imuT.getQuaternionf();
+			data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat()));
+		}
 	}
 
 	RegistrationInfo regInfo;

corelib/src/odometry/OdometryMSCKF.cpp

@@ -672,18 +672,17 @@ public:
 		T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
 		T_i_w.translation() = imu_state.position;
 
-		Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w *
-				msckf_vio::IMUState::T_imu_body.inverse();
+		Eigen::Isometry3d T_b_w = 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;
@@ -725,20 +724,18 @@ 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) {
-	      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_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_pub.publish(feature_msg_ptr);
+		// feature_pub.publish(feature_msg_ptr);
 
 		return odom_msg;
 	}
@@ -755,7 +752,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
 imageProcessor_(0),
 msckf_(0),
 parameters_(parameters),
-fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0),
+fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0),
 previousPose_(Transform::getIdentity()),
 initGravity_(false)
 #endif

corelib/src/odometry/OdometryORBSLAM2.cpp

@@ -34,28 +34,21 @@ 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/OdometryORBSLAM.h>
+#include <rtabmap/core/odometry/OdometryORBSLAM2.h>
 
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 #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)
@@ -67,9 +60,6 @@ private:
 protected:
 	void Track()
 	{
-#if RTABMAP_ORB_SLAM == 3
-		Map* mpMap = mpAtlas->GetCurrentMap();
-#endif
 	    if(mState==NO_IMAGES_YET)
 	    {
 	        mState = NOT_INITIALIZED;
@@ -91,17 +81,8 @@ 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
 	    {
@@ -384,9 +365,6 @@ 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);
 
@@ -484,11 +462,9 @@ 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();
@@ -516,11 +492,8 @@ 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();
@@ -531,11 +504,7 @@ 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)
 	{}
 
@@ -566,16 +535,12 @@ public:
 
 } // namespace ORB_SLAM
 
-#if RTABMAP_ORB_SLAM == 3
-using namespace ORB_SLAM3;
-#else
 using namespace ORB_SLAM2;
-#endif
 
-class ORBSLAMSystem
+class ORBSLAM2System
 {
 public:
-	ORBSLAMSystem(const rtabmap::ParametersMap & parameters) :
+	ORBSLAM2System(const rtabmap::ParametersMap & parameters) :
 		mpVocabulary(0),
 		mpKeyFrameDatabase(0),
 		mpMap(0),
@@ -613,7 +578,7 @@ public:
 		}
 	}
 
-	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline, const rtabmap::Transform & localIMUTransform)
+	bool init(const rtabmap::CameraModel & model, bool stereo, double baseline)
 	{
 		if(!mpVocabulary)
 		{
@@ -709,31 +674,6 @@ 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
 		//#--------------------------------------------------------------------------------------------
@@ -776,22 +716,15 @@ 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);
 
@@ -812,17 +745,10 @@ 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 ~ORBSLAMSystem()
+	virtual ~ORBSLAM2System()
 	{
 		shutdown();
 		delete mpVocabulary;
@@ -869,11 +795,7 @@ 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
@@ -898,24 +820,23 @@ public:
 
 namespace rtabmap {
 
-OdometryORBSLAM::OdometryORBSLAM(const ParametersMap & parameters) :
+OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
 	Odometry(parameters)
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
     ,
 	orbslam_(0),
 	firstFrame_(true),
-	previousPose_(Transform::getIdentity()),
-	useIMU_(false) // TODO: Not yet supported with ORB_SLAM3
+	previousPose_(Transform::getIdentity())
 #endif
 {
-#ifdef RTABMAP_ORB_SLAM
-	orbslam_ = new ORBSLAMSystem(parameters);
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
+	orbslam_ = new ORBSLAM2System(parameters);
 #endif
 }
 
-OdometryORBSLAM::~OdometryORBSLAM()
+OdometryORBSLAM2::~OdometryORBSLAM2()
 {
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 	if(orbslam_)
 	{
 		delete orbslam_;
@@ -923,10 +844,10 @@ OdometryORBSLAM::~OdometryORBSLAM()
 #endif
 }
 
-void OdometryORBSLAM::reset(const Transform & initialPose)
+void OdometryORBSLAM2::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 	if(orbslam_)
 	{
 		orbslam_->shutdown();
@@ -934,60 +855,20 @@ void OdometryORBSLAM::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 OdometryORBSLAM::computeTransform(
+Transform OdometryORBSLAM2::computeTransform(
 		SensorData & data,
 		const Transform & guess,
 		OdometryInfo * info)
 {
 	Transform t;
 
-#ifdef RTABMAP_ORB_SLAM
+#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
 	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)
@@ -1007,18 +888,12 @@ Transform OdometryORBSLAM::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(), imuLocalTransform_))
+		if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline()))
 		{
 			return t;
 		}

corelib/src/odometry/OdometryORBSLAM3.cpp

@@ -0,0 +1,571 @@
+/*
+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 & model, 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);
+
+	//# Camera calibration and distortion parameters (OpenCV)
+	ofs << "Camera1.fx: " << model.fx() << std::endl;
+	ofs << "Camera1.fy: " << model.fy() << std::endl;
+	ofs << "Camera1.cx: " << model.cx() << std::endl;
+	ofs << "Camera1.cy: " << model.cy() << std::endl;
+	ofs << std::endl;
+
+	if(model.D().cols < 4)
+	{
+		ofs << "Camera1.k1: " << 0.0 << std::endl;
+		ofs << "Camera1.k2: " << 0.0 << std::endl;
+		ofs << "Camera1.p1: " << 0.0 << std::endl;
+		ofs << "Camera1.p2: " << 0.0 << std::endl;
+		if(!stereo)
+		{
+			ofs << "Camera1.k3: " << 0.0 << std::endl;
+		}
+	}
+	if(model.D().cols >= 4)
+	{
+		ofs << "Camera1.k1: " << model.D().at<double>(0,0) << std::endl;
+		ofs << "Camera1.k2: " << model.D().at<double>(0,1) << std::endl;
+		ofs << "Camera1.p1: " << model.D().at<double>(0,2) << std::endl;
+		ofs << "Camera1.p2: " << model.D().at<double>(0,3) << std::endl;
+	}
+	if(model.D().cols >= 5)
+	{
+		ofs << "Camera1.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);
+	}
+	ofs << "Camera.bf: " << model.fx()*baseline << std::endl;
+
+	ofs << "Camera.width: " << model.imageWidth() << std::endl;
+	ofs << "Camera.height: " << model.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: 1" << 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 = model.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, 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

corelib/src/odometry/OdometryOpenVINS.cpp

@@ -28,22 +28,17 @@ 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 "rtabmap/utilite/UStl.h"
-#include "rtabmap/utilite/UThread.h"
-#include "rtabmap/utilite/UDirectory.h"
+#include <opencv2/core/eigen.hpp>
 #include <opencv2/imgproc/types_c.h>
 
 #ifdef RTABMAP_OPENVINS
 #include "core/VioManager.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/Propagator.h"
 #include "state/State.h"
-#include "types/Type.h"
+#include "state/StateHelper.h"
 #endif
 
 namespace rtabmap {
@@ -52,17 +47,111 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
 	Odometry(parameters)
 #ifdef RTABMAP_OPENVINS
     ,
-	vioManager_(0),
 	initGravity_(false),
-	previousPose_(Transform::getIdentity())
+	previousPoseInv_(Transform::getIdentity())
 #endif
 {
-}
-
-OdometryOpenVINS::~OdometryOpenVINS()
-{
 #ifdef RTABMAP_OPENVINS
-	delete vioManager_;
+	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;
 #endif
 }
 
@@ -72,11 +161,9 @@ void OdometryOpenVINS::reset(const Transform & initialPose)
 #ifdef RTABMAP_OPENVINS
 	if(!initGravity_)
 	{
-		delete vioManager_;
-		vioManager_ = 0;
-		previousPose_.setIdentity();
-		previousLocalTransform_.setNull();
-		imuBuffer_.clear();
+		vioManager_.reset();
+		previousPoseInv_.setIdentity();
+		imuLocalTransformInv_.setNull();
 	}
 	initGravity_ = false;
 #endif
@@ -90,395 +177,306 @@ Transform OdometryOpenVINS::computeTransform(
 {
 	Transform t;
 #ifdef RTABMAP_OPENVINS
-	UTimer timer;
 
-	// Buffer imus;
-	if(!data.imu().empty())
+	if(!vioManager_)
 	{
-		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())
+		if(!data.imu().empty())
 		{
-			UWARN("Waiting IMU for initialization...");
-			return t;
+			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);
 		}
-		if(vioManager_ == 0)
+
+		if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull())
 		{
-			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)
+			Transform T_imu_left;
+			Eigen::VectorXd left_calib(8), right_calib(8);
+			if(!data.rightRaw().empty())
 			{
-				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);
-			}
+				params_->state_options.num_cameras = params_->init_options.num_cameras = 2;
+				T_imu_left = imuLocalTransformInv_ * data.stereoCameraModels()[0].localTransform();
 
-			// Filter initialization
-			//params.init_window_time = 1;
-			//params.init_imu_thresh = 1;
+				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()));
+				}
 
-			// 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;
+				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);
+				}
 			}
 			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);
+				params_->state_options.num_cameras = params_->init_options.num_cameras = 1;
+				T_imu_left = imuLocalTransformInv_ * data.cameraModels()[0].localTransform();
 
-				//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)
+				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()));
+				}
 
-				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));
-
-			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())
-			{
-				cam0cam1 = Transform(
-						1, 0, 0, data.stereoCameraModels()[0].baseline(),
-						0, 1, 0, 0,
-						0, 0, 1, 0);
-			}
-			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);
-		}
-
-		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 = 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(vioManager_->initialized())
-		{
-			// Get the current state
-			std::shared_ptr<ov_msckf::State> state = vioManager_->get_state();
-
-			if(state->_timestamp != data.stamp())
-			{
-				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(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);
 				}
 			}
 
-			if(!p.isNull())
+			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())
 			{
-				p =  p * imuLocalTransform_.inverse();
+				Transform T_left_right;
+				if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
+				{
+					T_left_right = 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
+	{
+		if(!data.imu().empty())
+		{
+			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];
+			vioManager_->feed_measurement_imu(message);
+		}
+
+		if(!data.imageRaw().empty())
+		{
+			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);
+
+			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())
+			{
+				p = p * imuLocalTransformInv_;
 
 				if(this->getPose().rotation().isIdentity())
 				{
 					initGravity_ = true;
-					this->reset(this->getPose()*p.rotation());
+					this->reset(this->getPose() * p.rotation());
 				}
 
-				if(previousPose_.isIdentity())
-				{
-					previousPose_ = p;
-				}
+				if(previousPoseInv_.isIdentity())
+					previousPoseInv_ = p.inverse();
 
-				// make it incremental
-				Transform previousPoseInv = previousPose_.inverse();
-				t = previousPoseInv*p;
-				previousPose_ = p;
+				t = previousPoseInv_ * p;
 
 				if(info)
 				{
-					info->type = this->getType();
-					info->reg.covariance = covariance;
+					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();
 
-					// 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())
+					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)
 					{
-						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));
+						Eigen::Matrix<double, 6, 6> covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose();
+						cv::eigen2cv(covariance, info->reg.covariance);
+					}
+
+					if(this->isInfoDataFilled())
+					{
+						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);
+						}
 
 						if(this->imagesAlreadyRectified())
 						{
-							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);
+							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);
+							}
 						}
 					}
-					info->features = info->newCorners.size();
-					info->localMapSize = info->localMap.size();
 				}
-				UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
+
+				previousPoseInv_ = p.inverse();
 			}
 		}
 	}
-	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.");

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -527,7 +527,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				float x,y,z,roll,pitch,yaw;
 				std::map<int, Transform> tmpPoses;
+#if GTSAM_VERSION_NUMERIC >= 40200
+				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)
+#endif
 				{
 					if(iter->value.dim() > 1)
 					{
@@ -630,7 +634,11 @@ 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
+		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)
+#endif
 		{
 			if(iter->value.dim() > 1)
 			{

corelib/src/optimizer/OptimizerTORO.cpp

@@ -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.hh"
-#include "toro3d/treeoptimizer2.hh"
+#include "toro3d/treeoptimizer3.h"
+#include "toro3d/treeoptimizer2.h"
 #endif
 
 namespace rtabmap {

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -63,15 +63,21 @@ public:
 
   /** vector of errors */
   Vector attitudeError(const Rot3& p,
-      OptionalJacobian<2,3> H = boost::none) const;
+#if GTSAM_VERSION_NUMERIC >= 40300
+      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
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
     ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
   }
+#endif
 };
 
 /**
@@ -85,7 +91,11 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
 public:
 
   /// shorthand for a smart pointer to a factor
+#if GTSAM_VERSION_NUMERIC >= 40300
+  typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
+  #else
   typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
+#endif
 
   /// Typedef to this class
   typedef Rot3GravityFactor This;
@@ -111,7 +121,11 @@ public:
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+#if GTSAM_VERSION_NUMERIC >= 40300
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+#else
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+#endif
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
@@ -124,7 +138,11 @@ public:
 
   /** vector of errors */
   virtual Vector evaluateError(const Rot3& nRb, //
-      boost::optional<Matrix&> H = boost::none) const {
+#if GTSAM_VERSION_NUMERIC >= 40300
+	OptionalMatrixType H = OptionalNone) const {
+#else
+	boost::optional<Matrix&> H = boost::none) const {
+#endif
     return attitudeError(nRb, H);
   }
   Unit3 nZ() const {
@@ -135,7 +153,7 @@ public:
   }
 
 private:
-
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -145,6 +163,7 @@ private:
     ar & boost::serialization::make_nvp("GravityFactor",
         boost::serialization::base_object<GravityFactor>(*this));
   }
+#endif
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -163,8 +182,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
 public:
 
   /// shorthand for a smart pointer to a factor
+#if GTSAM_VERSION_NUMERIC >= 40300
+  typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
+#else
   typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
-
+#endif
   /// Typedef to this class
   typedef Pose3GravityFactor This;
 
@@ -189,7 +211,11 @@ public:
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+#if GTSAM_VERSION_NUMERIC >= 40300
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+#else
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+#endif
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
@@ -202,7 +228,11 @@ public:
 
   /** vector of errors */
   virtual Vector evaluateError(const Pose3& nTb, //
+#if GTSAM_VERSION_NUMERIC >= 40300
+      OptionalMatrixType H = OptionalNone) const {
+#else
       boost::optional<Matrix&> H = boost::none) const {
+#endif
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;
@@ -219,7 +249,7 @@ public:
   }
 
 private:
-
+#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -229,7 +259,7 @@ private:
     ar & boost::serialization::make_nvp("GravityFactor",
         boost::serialization::base_object<GravityFactor>(*this));
   }
-
+#endif
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

corelib/src/optimizer/gtsam/XYFactor.h

@@ -41,7 +41,12 @@ public:
   // error function
   // @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, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const VALUE& p,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
 
     // note that use boost optional like a pointer
     // only calculate jacobian matrix when non-null pointer exists

corelib/src/optimizer/gtsam/XYZFactor.h

@@ -41,14 +41,24 @@ public:
   // error function
   // @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, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const gtsam::Pose3& p,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
     if(H)
     {
 	  p.translation(H);
     }
     return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
   }
-  gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
+  gtsam::Vector evaluateError(const gtsam::Point3& p,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H = OptionalNone) const {
+#else
+		  boost::optional<gtsam::Matrix&> H = boost::none) const {
+#endif
     return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
   }
 };

corelib/src/optimizer/toro3d/posegraph2.cpp

@@ -40,7 +40,8 @@
  * such as loading, saving, merging constraints, and etc.
  **/
 
-#include "posegraph2.hh"
+#include "posegraph2.h"
+
 #include <fstream>
 #include <sstream>
 #include <string>

corelib/src/optimizer/toro3d/posegraph2.h

@@ -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 {
 

corelib/src/optimizer/toro3d/posegraph3.cpp

@@ -39,7 +39,8 @@
  * such as loading, saving, merging constraints, and etc.
  **/
 
-#include "posegraph3.hh"
+#include "posegraph3.h"
+
 #include <fstream>
 #include <sstream>
 #include <string>

corelib/src/optimizer/toro3d/posegraph3.h

@@ -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

corelib/src/optimizer/toro3d/transformation3.h

@@ -39,7 +39,8 @@
 
 #include <assert.h>
 #include <cmath>
-#include "dmatrix.hh"
+
+#include "dmatrix.h"
 
 namespace AISNavigation {
 

corelib/src/optimizer/toro3d/treeoptimizer2.cpp

@@ -41,7 +41,8 @@
  *
  **/
 
-#include "treeoptimizer2.hh"
+#include "treeoptimizer2.h"
+
 #include <fstream>
 #include <sstream>
 #include <string>

corelib/src/optimizer/toro3d/treeoptimizer2.h

@@ -44,7 +44,7 @@
 #ifndef _TREEOPTIMIZER2_HH_
 #define _TREEOPTIMIZER2_HH_
 
-#include "posegraph2.hh"
+#include "posegraph2.h"
 
 namespace AISNavigation {
 

corelib/src/optimizer/toro3d/treeoptimizer3.cpp

@@ -41,7 +41,8 @@
  *
  **/
 
-#include "treeoptimizer3.hh"
+#include "treeoptimizer3.h"
+
 #include <fstream>
 #include <sstream>
 #include <string>

corelib/src/optimizer/toro3d/treeoptimizer3.h

@@ -44,7 +44,7 @@
 #ifndef _TREEOPTIMIZER3_HH_
 #define _TREEOPTIMIZER3_HH_
 
-#include "posegraph3.hh"
+#include "posegraph3.h"
 
 namespace AISNavigation {
 

corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp

@@ -34,9 +34,9 @@
  * PURPOSE.  
  **********************************************************************/
 
-#include "treeoptimizer3.hh"
 #include <fstream>
 #include <string>
+#include "treeoptimizer3.h"
 
 using namespace std;
 

corelib/src/optimizer/vertigo/gtsam/DerivedValue.h

@@ -72,9 +72,15 @@ public:
   /**
    * Clone this value (normal clone on the heap, delete with 'delete' operator)
    */
+#if GTSAM_VERSION_NUMERIC >= 40300
+  virtual std::shared_ptr<gtsam::Value> clone() const {
+    return std::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
+  }
+  #else
   virtual boost::shared_ptr<gtsam::Value> clone() const {
     return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
   }
+#endif
 
   /// equals implementing generic Value interface
   virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const {

corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h

@@ -12,7 +12,7 @@
 #include <Eigen/Eigen>
 #include <gtsam/config.h>
 
-#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
+#if GTSAM_VERSION_NUMERIC >= 40100
 namespace gtsam {
 gtsam::Matrix inverse(const gtsam::Matrix & matrix)
 {
@@ -49,7 +49,7 @@ namespace vertigo {
           double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
           double l1 = nu1 * exp(-0.5*m1);
 
-#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
+#if GTSAM_VERSION_NUMERIC >= 40100
           double m2 = nullHypothesisModel->squaredMahalanobisDistance(error);
 #else
           double m2 = nullHypothesisModel->distance(error);

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -30,9 +30,15 @@ namespace vertigo {
         betweenFactor(key1, key2, measured, model) {};
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H1 = OptionalNone,
+		  OptionalMatrixType H2 = OptionalNone,
+		  OptionalMatrixType H3 = OptionalNone) const
+#else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
-          boost::optional<gtsam::Matrix&> H2 =  boost::none,
-          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+          boost::optional<gtsam::Matrix&> H2 = boost::none,
+          boost::optional<gtsam::Matrix&> H3 = boost::none) const
+#endif
         {
 
           // calculate error
@@ -64,9 +70,15 @@ namespace vertigo {
         betweenFactor(key1, key2, measured, model) {};
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		  OptionalMatrixType H1 = OptionalNone,
+		  OptionalMatrixType H2 = OptionalNone,
+		  OptionalMatrixType H3 = OptionalNone) const
+#else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
-          boost::optional<gtsam::Matrix&> H2 =  boost::none,
-          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+          boost::optional<gtsam::Matrix&> H2 = boost::none,
+          boost::optional<gtsam::Matrix&> H3 = boost::none) const
+#endif
       {
 
         // calculate error

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -76,8 +76,13 @@ namespace vertigo {
 
     /** between operation */
     inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
+#if GTSAM_VERSION_NUMERIC >= 40300
+		OptionalMatrixType H1=OptionalNone,
+		OptionalMatrixType H2=OptionalNone) const {
+#else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {
+#endif
       if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
       if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
       return SwitchVariableLinear(l2.value() - value());
@@ -116,11 +121,19 @@ template<> struct traits<vertigo::SwitchVariableLinear> {
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef gtsam::Vector TangentVector;
   static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
-  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#if GTSAM_VERSION_NUMERIC >= 40300
+      ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+      ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
     return origin.localCoordinates(other);
   }
   static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
-        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#if GTSAM_VERSION_NUMERIC >= 40300
+      ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
+      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
       return g.retract(v);
     }
 };

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -76,8 +76,13 @@ namespace vertigo {
 
     /** between operation */
     inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
+#if GTSAM_VERSION_NUMERIC >= 40300
+        OptionalMatrixType H1=OptionalNone,
+		OptionalMatrixType H2=OptionalNone) const {
+#else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {
+#endif
       if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
       if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
       return SwitchVariableSigmoid(l2.value() - value());
@@ -117,11 +122,19 @@ template<> struct traits<vertigo::SwitchVariableSigmoid> {
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef gtsam::Vector TangentVector;
   static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
-  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#if GTSAM_VERSION_NUMERIC >= 40300
+	  ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
+#else
+	  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+#endif
     return origin.localCoordinates(other);
   }
   static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
+#if GTSAM_VERSION_NUMERIC >= 40300
+        ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
+#else
         ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+#endif
       return g.retract(v);
     }
 };