bump patch

We are shortly going to be removing octomap the package
from ROS 2 Rolling; that's because it's ABI conflicts with
the system package.  Instead, switch rtabmap to use the system
package, which should work fine.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>

.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>=5.1.0 
   # 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/android/devcontainer.json

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

.devcontainer/bionic/devcontainer.json

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

.devcontainer/focal/devcontainer.json

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

.devcontainer/jammy/devcontainer.json

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

.devcontainer/noble/devcontainer.json

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

.github/workflows/cmake-ros.yml

@@ -21,28 +21,26 @@ 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
+        # Currently CI has some errors on setup-ros with this OS, disabling for now
+        #- ros_distribution: 'jazzy'
+        #  os: ubuntu-24.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,8 +16,9 @@ jobs:
     name: ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
+      fail-fast: false
       matrix:
-        os: [ubuntu-22.04, ubuntu-20.04]
+        os: [ubuntu-24.04, ubuntu-22.04, ubuntu-20.04]
 
     steps:    
     - name: Install dependencies
@@ -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,82 @@ 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, noble-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
+            linux/arm/v7
+          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'
+        - docker_tag: noble-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble/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, noble, android23, android24, android26, android30]
+        include:
         - docker_tag: bionic
           docker_tags: |
             introlab3it/rtabmap:bionic
@@ -42,17 +102,8 @@ jobs:
           docker_platforms: |
             linux/amd64
             linux/arm64
+            linux/arm/v7
           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 +114,24 @@ 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: noble
+          docker_tags: |
+            introlab3it/rtabmap:noble
+            introlab3it/rtabmap:24.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23

.github/workflows/scheduled-stats.yml

@@ -0,0 +1,16 @@
+name: RTAB-Map Scheduled Stats Extraction From GitHub
+
+on:
+    workflow_dispatch:
+    schedule:
+        - cron: '0 5 * * *'
+jobs:
+    get_stats:
+        runs-on: ubuntu-latest
+        steps:
+            - name: Update Stats
+              uses: introlab/github-stats-action@v1
+              with:
+                  github-stats-token: ${{ secrets.STATS_TOKEN }}
+                  google-application-credentials: ${{ secrets.GOOGLE_APPLICATION_CREDENTIALS }}
+                  spreadsheet-id: ${{ secrets.SPREADSHEET_ID }}

CMakeLists.txt

@@ -1,5 +1,5 @@
 # Top-Level CmakeLists.txt
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.14)
 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 6)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -181,6 +181,7 @@ option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
 option(WITH_K4A           "Include Kinect for Azure support"     ON)
+option(WITH_OPENNI        "Include OpenNI support"               ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
@@ -203,11 +204,13 @@ 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_XVSDK         "Include XVisio SDK support"           OFF)
+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 +221,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 +231,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)
@@ -251,10 +254,10 @@ endif()
 
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
-FIND_PACKAGE(Sqlite3 QUIET)
-IF(Sqlite3_FOUND)
-   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
-ENDIF(Sqlite3_FOUND)
+FIND_PACKAGE(SQLite3 QUIET)
+IF(SQLite3_FOUND)
+   MESSAGE(STATUS "Found SQLite3: ${SQLite3_INCLUDE_DIRS} ${SQLite3_LIBRARIES}")
+ENDIF(SQLite3_FOUND)
 
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
    # fix libproj.so not found on Xenial
@@ -293,14 +296,20 @@ SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF FALSE)
 IF(WITH_QT)
     FIND_PACKAGE(VTK)
     IF(NOT VTK_FOUND)
-        MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.")
+      MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.")
     ENDIF(NOT VTK_FOUND)
 
     # If Qt is here, the GUI will be built
     IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
-	  IF(NOT VTK_QT_VERSION)
-	    MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.")
-	  ENDIF()
+      IF(NOT VTK_QT_VERSION)
+        MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.")
+      ENDIF()
+
+      option(VTK_GLOBAL_WARNING_DISPLAY  "Show VTK warning display on runtime" OFF)
+      IF(NOT VTK_GLOBAL_WARNING_DISPLAY)
+        ADD_DEFINITIONS(-DVTK_GLOBAL_WARNING_DISPLAY_OFF)
+      ENDIF()
+
       MESSAGE(STATUS "VTK>=9 detected, will use VTK_QT_VERSION=${VTK_QT_VERSION} for Qt version.")
       IF(${VTK_QT_VERSION} EQUAL 6)
         FIND_PACKAGE(Qt6 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
@@ -325,6 +334,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 +403,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 +533,19 @@ 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)
+        string(FIND "${libpointmatcher_LIBRARIES}" "yaml-cpp::yaml-cpp" value)
+        IF(NOT value EQUAL -1)
+            # Find yaml-cpp (Issue #1268):
+            find_package(yaml-cpp QUIET)
+        ENDIF(NOT value EQUAL -1)
     ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)
 
@@ -631,12 +658,19 @@ IF(WITH_MYNTEYE)
 ENDIF(WITH_MYNTEYE)
 
 IF(WITH_DEPTHAI)
-    FIND_PACKAGE(depthai 2 QUIET)
+    FIND_PACKAGE(depthai 2.24 QUIET)
     IF(depthai_FOUND)
        MESSAGE(STATUS "Found depthai-core (targets)")
     ENDIF(depthai_FOUND)
 ENDIF(WITH_DEPTHAI)
 
+IF(WITH_XVSDK)
+    FIND_PACKAGE(xvsdk QUIET)
+    IF(xvsdk_FOUND)
+       MESSAGE(STATUS "Found xvsdk (targets)")
+    ENDIF(xvsdk_FOUND)
+ENDIF(WITH_XVSDK)
+
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(octomap QUIET)
     IF(octomap_FOUND)
@@ -647,6 +681,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 +807,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 +818,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 +830,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 +845,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))
@@ -879,9 +906,9 @@ ENDIF()
 IF(NOT MRPT_FOUND)
    SET(MRPT "//")
 ENDIF(NOT MRPT_FOUND)
-IF(NOT CERES_FOUND)
+IF(NOT WITH_CERES OR NOT CERES_FOUND)
    SET(CERES "//")
-ENDIF(NOT CERES_FOUND)
+ENDIF(NOT WITH_CERES OR NOT CERES_FOUND)
 IF(NOT WITH_TORO)
    SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -903,9 +930,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)
@@ -938,6 +965,9 @@ ELSE()
       )
    ENDIF(WIN32)
 ENDIF()
+IF(NOT (OpenNI_FOUND AND WITH_OPENNI))
+   SET(OPENNI "//")
+ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
 ENDIF()
@@ -977,10 +1007,16 @@ 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 xvsdk_FOUND)
+   SET(XVSDK "//")
+   SET(CONF_WITH_XVSDK 0)
+ELSE()
+   SET(CONF_WITH_XVSDK 1)
 ENDIF()
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
@@ -988,6 +1024,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 +1071,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)
@@ -1048,13 +1093,15 @@ IF(ANDROID)
    IF(BUILD_APP)
       ADD_SUBDIRECTORY( app )
    ENDIF(BUILD_APP)
-ELSEIF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
-   ADD_SUBDIRECTORY( guilib )
-   IF(BUILD_APP)
-      ADD_SUBDIRECTORY( app )
-   ENDIF(BUILD_APP)
 ELSEIF(WITH_QT)
-   MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...")
+   IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
+      ADD_SUBDIRECTORY( guilib )
+      IF(BUILD_APP)
+         ADD_SUBDIRECTORY( app )
+      ENDIF(BUILD_APP)
+   ELSE()
+      MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...")
+   ENDIF()
 ENDIF()
 
 IF(BUILD_TOOLS)
@@ -1067,6 +1114,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,13 +1122,14 @@ CONFIGURE_FILE(
 
 ADD_CUSTOM_TARGET(uninstall
   "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
+ENDIF()
 
 ####
 # Global Export Target
 ####
 add_library(rtabmap INTERFACE)
 add_library(rtabmap::rtabmap ALIAS rtabmap)
-IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+IF(WITH_QT AND (QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND))
    set(CONF_WITH_GUI ON)
    IF(QT4_FOUND)
       set(CONF_QT_VERSION 4)
@@ -1292,13 +1341,13 @@ IF(OpenCV_FOUND)
   ENDIF()
 ENDIF(OpenCV_FOUND)
 
-IF(QT4_FOUND)
+IF(WITH_QT AND QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
-ELSEIF(Qt5_FOUND)
+ELSEIF(WITH_QT AND Qt5_FOUND)
 MESSAGE(STATUS "  With Qt ${Qt5_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
-ELSEIF(Qt6_FOUND)
+ELSEIF(WITH_QT AND Qt6_FOUND)
 MESSAGE(STATUS "  With Qt ${Qt6_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
 
@@ -1308,10 +1357,10 @@ ELSE()
 MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
 ENDIF()
 
-IF(Sqlite3_FOUND)
+IF(SQLite3_FOUND)
 MESSAGE(STATUS "  With external SQLite3     = YES (License: Public Domain)")
 ELSE()
-MESSAGE(STATUS "  With external SQLite3     = NO (sqlite3 not found, internal version is used for convenience)")
+MESSAGE(STATUS "  With external SQLite3     = NO (SQLite3 not found, internal version is used for convenience)")
 ENDIF()
 
 IF(WITH_ORB_OCTREE)
@@ -1382,12 +1431,8 @@ ELSE()
 MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()
 
-IF(CERES_FOUND)
-IF(WITH_CERES)
+IF(WITH_CERES AND CERES_FOUND)
 MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = YES (License: BSD)")
-ELSE()
-MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)")
-ENDIF()
 ELSEIF(NOT WITH_CERES)
 MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
 ELSE()
@@ -1446,7 +1491,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 +1502,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)
@@ -1498,6 +1551,14 @@ ELSE()
 MESSAGE(STATUS "  With Freenect             = NO (libfreenect not found)")
 ENDIF()
 
+IF(WITH_OPENNI AND OpenNI_FOUND)
+MESSAGE(STATUS "  With OpenNI               = YES (License: Apache v2)")
+ELSEIF(NOT WITH_OPENNI)
+MESSAGE(STATUS "  With OpenNI               = NO (WITH_OPENNI=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenNI               = NO (OpenNI not found)")
+ENDIF()
+
 IF(OpenNI2_FOUND)
 MESSAGE(STATUS "  With OpenNI2              = YES (License: Apache v2)")
 ELSEIF(NOT WITH_OPENNI2)
@@ -1605,6 +1666,14 @@ ELSE()
 MESSAGE(STATUS "  With DepthAI              = NO (depthai-core not found)")
 ENDIF()
 
+IF(xvsdk_FOUND)
+MESSAGE(STATUS "  With XVisio SDK ${xvsdk_VERSION}     = YES")
+ELSEIF(NOT WITH_XVSDK)
+MESSAGE(STATUS "  With XVisio SDK           = NO (WITH_XVSDK=OFF)")
+ELSE()
+MESSAGE(STATUS "  With XVisio SDK           = NO (xvsdk not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Odometry Approaches:")
 IF(loam_velodyne_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.4-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,26 @@ IF(@CONF_WITH_K4A@)
     ENDIF()
 ENDIF()
 
+IF(@CONF_WITH_DEPTH_AI@)
+  find_dependency(depthai 2.24)
+ENDIF()
+
+IF(@CONF_WITH_XVSDK@)
+  find_dependency(xvsdk)
+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 "")
@@ -81,8 +97,7 @@ endforeach()
 include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")
 
 foreach(_comp ${RTABMap_FIND_COMPONENTS})
-  if (NOT ";${_RTABMap_supported_components};" MATCHES ";${_comp};")
-    set(RTABMap_${_comp}_FOUND False)
+  if (NOT RTABMap_${_comp}_FOUND)
     if(${RTABMap_FIND_REQUIRED_${_comp}})
       set(RTABMap_FOUND False)
       set(RTABMap_NOT_FOUND_MESSAGE "Unsupported or not found required component: ${_comp}")

Version.h.in

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CERES@#define RTABMAP_CERES
 @MRPT@#define RTABMAP_MRPT
 @VERTIGO@#define RTABMAP_VERTIGO
+@OPENNI@#define RTABMAP_OPENNI
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
@@ -68,7 +69,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @REALSENSE2@#define RTABMAP_REALSENSE2
 @MYNTEYE@#define RTABMAP_MYNTEYE
 @DEPTHAI@#define RTABMAP_DEPTHAI
+@XVSDK@#define RTABMAP_XVSDK
 @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/CameraARCore.cpp

@@ -334,11 +334,12 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR
 	}
 }
 
-SensorData CameraARCore::captureImage(CameraInfo * info)
+SensorData CameraARCore::updateDataOnRender(Transform & pose)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");
 
+	pose.setNull();
 	SensorData data;
 	if(!arSession_)
 	{
@@ -370,7 +371,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	if (geometry_changed != 0 || !uvs_initialized_) {
 		ArFrame_transformCoordinates2d(
 				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
-				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
 				transformed_uvs_);
 		UASSERT(transformed_uvs_);
 		uvs_initialized_ = true;
@@ -393,7 +394,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
-	Transform pose;
 	CameraModel model;
 	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
 	{
@@ -401,24 +401,13 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		float pose_raw[7];
 		ArCamera_getPose(arSession_, ar_camera, arPose_);
 		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+		Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		poseArCore = rtabmap::rtabmap_world_T_opengl_world * poseArCore * rtabmap::opengl_world_T_rtabmap_world;
 
-		Transform poseArCore = pose;
-		if(pose.isNull())
+		if(poseArCore.isNull())
 		{
 			LOGE("CameraARCore: Pose is null");
 		}
-		else
-		{
-			this->poseReceived(pose);
-			// adjust origin
-			if(!getOriginOffset().isNull())
-			{
-				pose = getOriginOffset() * pose;
-			}
-			info->odomPose = pose;
-		}
 
 		// Get calibration parameters
 		float fx,fy, cx, cy;
@@ -551,6 +540,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 
 						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
+
+						if(!poseArCore.isNull())
+						{
+							pose = poseArCore;
+							this->poseReceived(pose, stamp);
+							// adjust origin
+							if(!getOriginOffset().isNull())
+							{
+								pose = getOriginOffset() * pose;
+							}
+						}
 					}
 				}
 				else
@@ -571,134 +571,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	ArCamera_release(ar_camera);
 
 	return data;
-
-}
-
-void CameraARCore::capturePoseOnly()
-{
-	UScopeMutex lock(arSessionMutex_);
-	//LOGI("Capturing image...");
-
-	if(!arSession_)
-	{
-		return;
-	}
-
-	if(textureId_ != 0)
-	{
-		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-		ArSession_setCameraTextureName(arSession_, textureId_);
-	}
-
-	// Update session to get current frame and render camera background.
-	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
-		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
-		return;
-	}
-
-	// If display rotation changed (also includes view size change), we need to
-	// re-query the uv coordinates for the on-screen portion of the camera image.
-	int32_t geometry_changed = 0;
-	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
-	if (geometry_changed != 0 || !uvs_initialized_) {
-		ArFrame_transformCoordinates2d(
-				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
-				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
-				transformed_uvs_);
-		UASSERT(transformed_uvs_);
-		uvs_initialized_ = true;
-	}
-
-	ArCamera* ar_camera;
-	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
-
-	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
-	ArCamera_getProjectionMatrix(arSession_, ar_camera,
-							   /*near=*/0.1f, /*far=*/100.f,
-							   glm::value_ptr(projectionMatrix_));
-
-	// adjust origin
-	if(!getOriginOffset().isNull())
-	{
-		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-	}
-
-	ArTrackingState camera_tracking_state;
-	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
-
-	Transform pose;
-	CameraModel model;
-	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
-	{
-		// pose in OpenGL coordinates
-		float pose_raw[7];
-		ArCamera_getPose(arSession_, ar_camera, arPose_);
-		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-		if(!pose.isNull())
-		{
-			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-			this->poseReceived(pose);
-
-			if(!getOriginOffset().isNull())
-			{
-				pose = getOriginOffset() * pose;
-			}
-		}
-
-		int32_t is_depth_supported = 0;
-		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
-
-		if(is_depth_supported)
-		{
-			LOGD("Acquire depth image!");
-			ArImage * depthImage = nullptr;
-			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
-
-			ArImageFormat format;
-			ArImage_getFormat(arSession_, depthImage, &format);
-			if(format == AR_IMAGE_FORMAT_DEPTH16)
-			{
-				LOGD("Depth format detected!");
-				int planeCount;
-				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
-				LOGD("planeCount=%d", planeCount);
-				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
-				const uint8_t *data = nullptr;
-				int len = 0;
-				int stride;
-				int width;
-				int height;
-				ArImage_getWidth(arSession_, depthImage, &width);
-				ArImage_getHeight(arSession_, depthImage, &height);
-				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
-				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
-
-				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
-
-				cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
-
-				float fx,fy, cx, cy;
-				int32_t rgb_width, rgb_height;
-				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
-				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
-				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
-				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
-
-				float scaleX = (float)width / (float)rgb_width;
-				float scaleY = (float)height / (float)rgb_height;
-				CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
-				this->setOcclusionImage(occlusionImage, occlusionModel);
-			}
-			ArImage_release(depthImage);
-		}
-	}
-
-	ArCamera_release(ar_camera);
 }
 
 } /* namespace rtabmap */

app/android/jni/CameraARCore.h

@@ -63,23 +63,14 @@ public:
 	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
 	virtual ~CameraARCore();
 
-	bool uvsInitialized() const {return uvs_initialized_;}
-	const float* uvsTransformed() const {return transformed_uvs_;}
-	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
-
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	void setupGL();
-	virtual void close(); // close Tango connection
+	virtual void close(); // close ARCore connection
 	virtual std::string getSerial() const;
-	GLuint getTextureId() const {return textureId_;}
-
-	void imageCallback(AImageReader *reader);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
-	virtual void capturePoseOnly();
+	virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);

app/android/jni/CameraAREngine.cpp

@@ -117,9 +117,6 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri
 
 	deviceTColorCamera_ = opticalRotation;
 
-	// Required as ArSession_update does some off-screen OpenGL stuff...
-	HwArSession_setCameraTextureName(arSession_, textureId_);
-
 	if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
 	{
 		UERROR("Cannot resume camera!");
@@ -169,38 +166,87 @@ void CameraAREngine::close()
 	CameraMobile::close();
 }
 
-SensorData CameraAREngine::captureImage(CameraInfo * info)
+void CameraAREngine::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		HwArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
+SensorData CameraAREngine::updateDataOnRender(Transform & pose)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");
 
+	pose.setNull();
 	SensorData data;
 	if(!arSession_)
 	{
 		return data;
 	}
 
+	if(textureId_ == 0)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	if(textureId_!=0)
+		HwArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
 		LOGE("CameraAREngine::captureImage() ArSession_update error");
 		return data;
 	}
 
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		HwArFrame_transformDisplayUvCoords(
+				arSession_, arFrame_,
+				BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView,
+				transformed_uvs_);
+		UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)", 
+			transformed_uvs_[0], transformed_uvs_[1],
+			transformed_uvs_[2], transformed_uvs_[3],
+			transformed_uvs_[4], transformed_uvs_[5],
+			transformed_uvs_[6], transformed_uvs_[7]);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	HwArCamera* ar_camera;
 	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
 
+	HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	HwArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
 	HwArTrackingState camera_tracking_state;
 	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
-	Transform pose;
 	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
 	{
-		// pose in OpenGL coordinates
-		float pose_raw[7];
-		HwArCamera_getPose(arSession_, ar_camera, arPose_);
-		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-
 		// Get calibration parameters
 		// FIXME: Hard-coded as getting intrinsics with the api fails
 		float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
@@ -274,6 +320,26 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 					double stamp = double(timestamp_ns)/10e8;
 					CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
 					data = SensorData(outputRGB, outputDepth, model, 0, stamp);
+
+					// pose in OpenGL coordinates
+					float pose_raw[7];
+					HwArCamera_getPose(arSession_, ar_camera, arPose_);
+					HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+					pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+					if(pose.isNull())
+					{
+						LOGE("CameraAREngine: Pose is null");
+					}
+					else
+					{
+						pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+						this->poseReceived(pose, stamp);
+						// adjust origin
+						if(!getOriginOffset().isNull())
+						{
+							pose = getOriginOffset() * pose;
+						}
+					}
 				}
 			}
 			else
@@ -291,66 +357,8 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 	}
 
 	HwArCamera_release(ar_camera);
-
-	if(pose.isNull())
-	{
-		LOGE("CameraAREngine: Pose is null");
-	}
-	else
-	{
-		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-		this->poseReceived(pose);
-        // adjust origin
-        if(!getOriginOffset().isNull())
-        {
-            pose = getOriginOffset() * pose;
-        }
-		info->odomPose = pose;
-	}
 	return data;
 
 }
 
-void CameraAREngine::capturePoseOnly()
-{
-	UScopeMutex lock(arSessionMutex_);
-	//LOGI("Capturing image...");
-
-	SensorData data;
-	if(!arSession_)
-	{
-		return;
-	}
-
-	// Update session to get current frame and render camera background.
-	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
-		LOGE("CameraARCore::captureImage() ArSession_update error");
-		return;
-	}
-
-	HwArCamera* ar_camera;
-	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
-
-	HwArTrackingState camera_tracking_state;
-	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
-
-	Transform pose;
-	CameraModel model;
-	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
-	{
-		// pose in OpenGL coordinates
-		float pose_raw[7];
-		HwArCamera_getPose(arSession_, ar_camera, arPose_);
-		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-		if(!pose.isNull())
-		{
-			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-			this->poseReceived(pose);
-		}
-	}
-
-	HwArCamera_release(ar_camera);
-}
-
 } /* namespace rtabmap */

app/android/jni/CameraAREngine.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <background_renderer.h>
 
 #include <huawei_arengine_interface.h>
 
@@ -48,13 +49,14 @@ public:
 	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
 	virtual ~CameraAREngine();
 
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual void close(); // close Tango connection
+	virtual void close(); // close AREngine connection
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	virtual void capturePoseOnly();
+	virtual SensorData updateDataOnRender(Transform & pose);
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -69,7 +71,6 @@ private:
 	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
 	HwArPose * arPose_ = nullptr;
 	bool arInstallRequested_;
-	GLuint textureId_;
 	UMutex arSessionMutex_;
 
 };

app/android/jni/CameraMobile.cpp

@@ -55,10 +55,8 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
 CameraMobile::CameraMobile(bool smoothing) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
-		spinOncePreviousStamp_(0.0),
 		textureId_(0),
 		uvs_initialized_(false),
-		previousStamp_(0.0),
 		stampEpochOffset_(0.0),
 		smoothing_(smoothing),
 		colorCameraToDisplayRotation_(ROTATION_0),
@@ -79,13 +77,12 @@ bool CameraMobile::init(const std::string &, const std::string &)
 
 void CameraMobile::close()
 {
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
+	firstFrame_ = true;
 	lastKnownGPS_ = GPS();
 	lastEnvSensors_.clear();
 	originOffset_ = Transform();
 	originUpdate_ = false;
-	pose_ = Transform();
+	dataPose_ = Transform();
 	data_ = SensorData();
 
     if(textureId_ != 0)
@@ -97,35 +94,107 @@ void CameraMobile::close()
 
 void CameraMobile::resetOrigin()
 {
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
+	firstFrame_ = true;
 	lastKnownGPS_ = GPS();
 	lastEnvSensors_.clear();
-	pose_ = Transform();
+	dataPose_ = Transform();
 	data_ = SensorData();
 	originUpdate_ = true;
 }
 
-void CameraMobile::poseReceived(const Transform & pose)
+bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+	pose.setNull();
+
+	int maxWaitTimeMs = maxWaitTime * 1000;
+
+	// Interpolate pose
+	if(!poseBuffer_.empty())
+	{
+		poseMutex_.lock();
+		int waitTry = 0;
+		while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		{
+			poseMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			poseMutex_.lock();
+		}
+		if(poseBuffer_.rbegin()->first < stamp)
+		{
+			if(maxWaitTimeMs > 0)
+			{
+				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+			}
+			else
+			{
+				UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first);
+			}
+		}
+		else
+		{
+			std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
+			std::map<double, Transform>::const_iterator iterA = iterB;
+			if(iterA != poseBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == poseBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				pose = iterA->second;
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
+			}
+			else // stamp < iterA->first
+			{
+				UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+			}
+		}
+		poseMutex_.unlock();
+	}
+	return !pose.isNull();
+}
+
+void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
 {
 	if(!pose.isNull())
 	{
-		// send pose of the camera (without optical rotation)
-		Transform p = pose*deviceTColorCamera_;
+		Transform p = pose;
 		if(originUpdate_)
 		{
 			originOffset_ = p.translation().inverse();
 			originUpdate_ = false;
 		}
+		
+		if(stampEpochOffset_ == 0.0)
+		{
+			stampEpochOffset_ = UTimer::now() - deviceStamp;
+		}
+        
+		double epochStamp = stampEpochOffset_ + deviceStamp;
 
 		if(!originOffset_.isNull())
 		{
-			this->post(new PoseEvent(originOffset_*p));
+			p = originOffset_*p;
 		}
-		else
+
 		{
-			this->post(new PoseEvent(p));
+			UScopeMutex lock(poseMutex_);
+			poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p));
+			if(poseBuffer_.size() > 1000)
+			{
+				poseBuffer_.erase(poseBuffer_.begin());
+			}
 		}
+
+		// send pose of the camera (with optical rotation)
+		this->post(new PoseEvent(p * deviceTColorCamera_));
 	}
 }
 
@@ -139,11 +208,20 @@ void CameraMobile::setGPS(const GPS & gps)
 	lastKnownGPS_ = gps;
 }
 
-void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
+void CameraMobile::addEnvSensor(int type, float value)
 {
-	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
+void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
+{
+	UScopeMutex lock(dataMutex_);
+
+	bool notify = !data_.isValid();
+
+	LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
 	data_ = data;
-    pose_ = pose;
+    dataPose_ = pose;
 
     viewMatrix_ = viewMatrix;
     projectionMatrix_ = projectionMatrix;
@@ -151,7 +229,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
     // adjust origin
     if(!originOffset_.isNull())
     {
-        pose_ = originOffset_ * pose_;
+        dataPose_ = originOffset_ * dataPose_;
         viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
     }
     
@@ -166,7 +244,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
         uvs_initialized_ = true;
     }
     
-    LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
+    LOGD("CameraMobile::update textureId_=%d", (int)textureId_);
 
     if(textureId_ != 0 && texCoord != 0)
     {
@@ -193,78 +271,63 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
             return;
         }
     }
-}
 
-void CameraMobile::addEnvSensor(int type, float value)
-{
-	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
-}
-
-void CameraMobile::spinOnce()
-{
-	if(!this->isRunning())
+	postUpdate();
+	
+	if(notify)
 	{
-		bool ignoreFrame = false;
-		//float rate = 10.0f; // maximum 10 FPS for image data
-		double now = UTimer::now();
-		/*if(rate>0.0f)
-		{
-			if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
-				((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
-			{
-				ignoreFrame = true;
-			}
-		}*/
+		dataReady_.release();
+	}
+}
 
-		if(!ignoreFrame)
+void CameraMobile::updateOnRender()
+{
+	UScopeMutex lock(dataMutex_);
+	bool notify = !data_.isValid();
+
+	data_ = updateDataOnRender(dataPose_);
+
+	if(data_.isValid())
+	{
+		postUpdate();
+
+		if(notify)
 		{
-			spinOnceFrameRateTimer_.start();
-			spinOncePreviousStamp_ = now;
-			mainLoop();
-		}
-		else
-		{
-			// just send pose
-			capturePoseOnly();
+			dataReady_.release();
 		}
 	}
 }
 
-void CameraMobile::mainLoopBegin()
+SensorData CameraMobile::updateDataOnRender(Transform & pose)
 {
-	double t = cameraStartedTime_.elapsed();
-	if(t < 5.0)
-	{
-		uSleep((5.0-t)*1000); // just to make sure that the camera is started
-	}
+	LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() "
+	     "should be overridden by inherited classes. Returning empty data!\n");
+	return SensorData();
 }
 
-void CameraMobile::mainLoop()
+void CameraMobile::postUpdate()
 {
-	CameraInfo info;
-	SensorData data = this->captureImage(&info);
-
-	if(data.isValid() && !info.odomPose.isNull())
+	if(data_.isValid())
 	{
-		if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
+		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
 		{
-			data.setGPS(lastKnownGPS_);
+			data_.setGPS(lastKnownGPS_);
 		}
 		else if(lastKnownGPS_.stamp()>0.0)
 		{
-			LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
+			LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
 		}
 
 		if(lastEnvSensors_.size())
 		{
-			data.setEnvSensors(lastEnvSensors_);
+			data_.setEnvSensors(lastEnvSensors_);
 			lastEnvSensors_.clear();
 		}
 
-		if(smoothing_ && !data.depthRaw().empty())
+		if(smoothing_ && !data_.depthRaw().empty())
 		{
 			//UTimer t;
-			data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
+			data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
 			//LOGD("Bilateral filtering, time=%fs", t.ticks());
 		}
 
@@ -273,15 +336,15 @@ void CameraMobile::mainLoop()
 		{
 			UDEBUG("ROTATION_90");
 			cv::Mat rgb, depth;
-			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
-			cv::flip(data.imageRaw(),rgb,1);
+			cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
+			cv::flip(data_.imageRaw(),rgb,1);
 			cv::transpose(rgb,rgbt);
 			rgb = rgbt;
-			cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
-			cv::flip(data.depthRaw(),depth,1);
+			cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
+			cv::flip(data_.depthRaw(),depth,1);
 			cv::transpose(depth,deptht);
 			depth = deptht;
-			CameraModel model = data.cameraModels()[0];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageHeight(), model.imageWidth());
 			model = CameraModel(
 					model.fy(),
@@ -290,25 +353,25 @@ void CameraMobile::mainLoop()
 					model.cx()>0?model.imageWidth()-model.cx():0,
 					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
-			data.setRGBDImage(rgb, depth, model);
+			data_.setRGBDImage(rgb, depth, model);
 
-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
 			for(size_t i=0; i<keypoints.size(); ++i)
 			{
-				keypoints[i].pt.x = data.keypoints()[i].pt.y;
-				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
+				keypoints[i].pt.x = data_.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
 			}
-			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_180)
 		{
 			UDEBUG("ROTATION_180");
 			cv::Mat rgb, depth;
-			cv::flip(data.imageRaw(),rgb,1);
+			cv::flip(data_.imageRaw(),rgb,1);
 			cv::flip(rgb,rgb,0);
-			cv::flip(data.depthOrRightRaw(),depth,1);
+			cv::flip(data_.depthOrRightRaw(),depth,1);
 			cv::flip(depth,depth,0);
-			CameraModel model = data.cameraModels()[0];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageWidth(), model.imageHeight());
 			model = CameraModel(
 					model.fx(),
@@ -317,26 +380,26 @@ void CameraMobile::mainLoop()
 					model.cy()>0?model.imageHeight()-model.cy():0,
 					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
 			model.setImageSize(sizet);
-			data.setRGBDImage(rgb, depth, model);
+			data_.setRGBDImage(rgb, depth, model);
 
-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
 			for(size_t i=0; i<keypoints.size(); ++i)
 			{
-				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
-				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
+				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
 			}
-			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_270)
 		{
 			UDEBUG("ROTATION_270");
-			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
-			cv::transpose(data.imageRaw(),rgb);
+			cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
+			cv::transpose(data_.imageRaw(),rgb);
 			cv::flip(rgb,rgb,1);
-			cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
-			cv::transpose(data.depthOrRightRaw(),depth);
+			cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
+			cv::transpose(data_.depthOrRightRaw(),depth);
 			cv::flip(depth,depth,1);
-			CameraModel model = data.cameraModels()[0];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageHeight(), model.imageWidth());
 			model = CameraModel(
 					model.fy(),
@@ -345,61 +408,54 @@ void CameraMobile::mainLoop()
 					model.cx(),
 					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
-			data.setRGBDImage(rgb, depth, model);
+			data_.setRGBDImage(rgb, depth, model);
 
-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
 			for(size_t i=0; i<keypoints.size(); ++i)
 			{
-				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
-				keypoints[i].pt.y = data.keypoints()[i].pt.x;
+				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
+				keypoints[i].pt.y = data_.keypoints()[i].pt.x;
 			}
-			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
 		}
-
-		rtabmap::Transform pose = info.odomPose;
-		data.setGroundTruth(Transform());
-
-		// convert stamp to epoch
-		bool firstFrame = previousPose_.isNull();
-		if(firstFrame)
-		{
-			stampEpochOffset_ = UTimer::now()-data.stamp();
-		}
-		data.setStamp(stampEpochOffset_ + data.stamp());
-		OdometryInfo info;
-		if(!firstFrame)
-		{
-			info.interval = data.stamp()-previousStamp_;
-			info.transform = previousPose_.inverse() * pose;
-		}
-		// linear cov = 0.0001
-		info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
-		if(!firstFrame)
-		{
-			// angular cov = 0.000001
-			info.reg.covariance.at<double>(3,3) *= 0.01;
-			info.reg.covariance.at<double>(4,4) *= 0.01;
-			info.reg.covariance.at<double>(5,5) *= 0.01;
-		}
-		LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
-		this->post(new OdometryEvent(data, pose, info));
-		previousPose_ = pose;
-		previousStamp_ = data.stamp();
-	}
-	else if(!this->isKilled() && info.odomPose.isNull())
-	{
-		LOGW("Odometry lost");
-		this->post(new OdometryEvent());
 	}
 }
 
-SensorData CameraMobile::captureImage(CameraInfo * info)
+SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
 {
-	if(info)
+	SensorData data;
+	if(dataReady_.acquire(1, 5000))
 	{
-		info->odomPose = pose_;
+		UScopeMutex lock(dataMutex_);
+		data = data_;
+		data_ = SensorData();
 	}
-	return data_;
+	if(data.isValid())
+	{
+		data.setGroundTruth(Transform());
+		data.setStamp(stampEpochOffset_ + data.stamp());
+
+		if(info)
+		{
+			// linear cov = 0.0001
+			info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001);
+			if(!firstFrame_)
+			{
+				// angular cov = 0.000001
+				info->odomCovariance.at<double>(3,3) *= 0.01;
+				info->odomCovariance.at<double>(4,4) *= 0.01;
+				info->odomCovariance.at<double>(5,5) *= 0.01;
+			}
+			info->odomPose = dataPose_;
+		}
+
+		firstFrame_ = false;
+	}
+	else
+	{
+		UWARN("CameraMobile::captureImage() invalid data!");
+	}
+	return data;
 }
 
 LaserScan CameraMobile::scanFromPointCloudData(

app/android/jni/CameraMobile.h

@@ -68,7 +68,7 @@ private:
 	Transform pose_;
 };
 
-class CameraMobile : public Camera, public UThread, public UEventsSender {
+class CameraMobile : public Camera, public UEventsSender {
 public:
 	static const float bilateralFilteringSigmaS;
 	static const float bilateralFilteringSigmaR;
@@ -93,14 +93,20 @@ public:
 
 	// abstract functions
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual void close(); // inherited classes should call its parent in their close().
+	virtual void close(); // inherited classes should call its parent at the end of their close().
 	virtual std::string getSerial() const {return "CameraMobile";}
 
+	void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
+	void updateOnRender();
+
 	const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
 	void resetOrigin();
 	virtual bool isCalibrated() const;
 
-	void poseReceived(const Transform & pose); // in rtabmap frame
+	virtual bool odomProvided() const { return true; }
+	virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time
+	void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
+	double getStampEpochOffset() const {return stampEpochOffset_;}
 
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
@@ -108,10 +114,7 @@ public:
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
-	void setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
 
-	void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
-    
     GLuint getTextureId() {return textureId_;}
     bool uvsInitialized() const {return uvs_initialized_;}
     const float* uvsTransformed() const {return transformed_uvs_;}
@@ -122,17 +125,15 @@ public:
     const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	virtual void capturePoseOnly() {}
+	virtual SensorData updateDataOnRender(Transform & pose);
 
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
+private:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+	void postUpdate(); // Should be called while being protected by dataMutex_
 
 protected:
 	CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
 	Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
-	UTimer spinOnceFrameRateTimer_;
-	double spinOncePreviousStamp_;
     
     GLuint textureId_;
     glm::mat4 viewMatrix_;
@@ -141,9 +142,7 @@ protected:
     bool uvs_initialized_ = false;
 
 private:
-	Transform previousPose_;
-	double previousStamp_;
-	UTimer cameraStartedTime_;
+	bool firstFrame_;
 	double stampEpochOffset_;
 	bool smoothing_;
 	ScreenRotation colorCameraToDisplayRotation_;
@@ -152,8 +151,13 @@ private:
 	Transform originOffset_;
 	bool originUpdate_;
 
+	USemaphore dataReady_;
+	UMutex dataMutex_;
 	SensorData data_;
-	Transform pose_;
+	Transform dataPose_;
+
+	UMutex poseMutex_;
+	std::map<double, Transform> poseBuffer_; // <stamp, Pose>
     
     cv::Mat occlusionImage_;
     CameraModel occlusionModel_;

app/android/jni/CameraTango.cpp

@@ -101,7 +101,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 	if(pose->status_code == TANGO_POSE_VALID)
 	{
 		CameraTango* app = static_cast<CameraTango*>(context);
-		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
+		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp);
 	}
 }
 
@@ -444,7 +444,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
 
 		UASSERT(cloud.type() == CV_32FC4);
-		boost::mutex::scoped_lock  lock(dataMutex_);
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
 
 		// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
 		// "In the current version of Project Tango Tablet RGB IR camera
@@ -463,7 +463,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 
 			if(dt >= 0.0 && dt < 0.5)
 			{
-				bool notify = !data_.isValid();
+				bool notify = !tangoData_.isValid();
 
 				cv::Mat tangoImage = tangoColor_;
 				cv::Mat rgb;
@@ -495,7 +495,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 				else
 				{
 					LOGE("Not supported color format : %d.", tangoColorType);
-					data_ = SensorData();
+					tangoData_ = SensorData();
 					return;
 				}
 
@@ -678,24 +678,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 
 					if(rawScanPublished_)
 					{
-						data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
 					else
 					{
-						data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
-					data_.setGroundTruth(odom);
+					tangoData_.setGroundTruth(odom);
 				}
 				else
 				{
 					LOGE("Could not get depth and rgb images!?!");
-					data_ = SensorData();
+					tangoData_ = SensorData();
 					return;
 				}
 
 				if(notify)
 				{
-					dataReady_.release();
+					tangoDataReady_.release();
 				}
 				LOGD("process cloud received %fs", timer.ticks());
 			}
@@ -709,7 +709,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	{
 		//LOGD("RGB received! %fs", timestamp);
 
-		boost::mutex::scoped_lock  lock(dataMutex_);
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
 
 		tangoColor_ = tangoImage.clone();
 		tangoColorStamp_ = timestamp;
@@ -775,10 +775,11 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	return pose;
 }
 
-SensorData CameraTango::captureImage(CameraInfo * info)
+SensorData CameraTango::updateDataOnRender(Transform & pose)
 {
 	//LOGI("Capturing image...");
 
+	pose.setNull();
 	if(textureId_ == 0)
 	{
 		glGenTextures(1, &textureId_);
@@ -797,10 +798,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 		if (status == TANGO_SUCCESS)
 		{
-			if(info)
-			{
-				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
-			}
+			pose = getPoseAtTimestamp(video_overlay_timestamp);
 
 			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
 			  if (rotation > 3) {
@@ -876,16 +874,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 	}
 
 	SensorData data;
-	if(dataReady_.acquireTry(1))
+	if(tangoDataReady_.acquireTry(1))
 	{
-		boost::mutex::scoped_lock  lock(dataMutex_);
-		data = data_;
-		data_ = SensorData();
-		if(info)
-		{
-			info->odomPose = data.groundTruth();
-			data.setGroundTruth(Transform());
-		}
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
+		data = tangoData_;
+		tangoData_ = SensorData();
+		pose = data.groundTruth();
+		data.setGroundTruth(Transform());
 	}
 	return data;
 

app/android/jni/CameraTango.h

@@ -52,7 +52,6 @@ public:
 	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
-	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 
@@ -61,7 +60,7 @@ public:
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData updateDataOnRender(Transform & pose);
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -71,12 +70,12 @@ private:
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	SensorData data_;
+	SensorData tangoData_;
 	cv::Mat tangoColor_;
 	int tangoColorType_;
 	double tangoColorStamp_;
-	boost::mutex dataMutex_;
-	USemaphore dataReady_;
+	boost::mutex tangoDataMutex_;
+	USemaphore tangoDataReady_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
 };

app/android/jni/RTABMapApp.cpp

@@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/GainCompensator.h>
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/Recovery.h>
+#include <rtabmap/core/lidar/LidarVLP16.h>
 #include <pcl/common/common.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
@@ -134,6 +135,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAggressiveLoopThr(), "0.0"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
@@ -202,6 +204,7 @@ RTABMapApp::RTABMapApp() :
 #endif
 		cameraDriver_(0),
 		camera_(0),
+		sensorCaptureThread_(0),
 		rtabmapThread_(0),
 		rtabmap_(0),
 		logHandler_(0),
@@ -216,6 +219,7 @@ RTABMapApp::RTABMapApp() :
 		cameraColor_(true),
 		fullResolution_(false),
 		appendMode_(true),
+		useExternalLidar_(false),
 		maxCloudDepth_(2.5),
 		minCloudDepth_(0.0),
 		cloudDensityLevel_(1),
@@ -537,7 +541,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							// Voxelize and filter depending on the previous cloud?
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
-							if(!data.imageRaw().empty() && !data.depthRaw().empty())
+							if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 							{
                                 int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
                                 
@@ -649,9 +653,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;
 						}
 					}
@@ -885,7 +889,7 @@ bool RTABMapApp::startCamera()
 #endif
 	LOGW("startCamera() camera driver=%d", cameraDriver_);
 	boost::mutex::scoped_lock  lock(cameraMutex_);
-    
+
 	if(cameraDriver_ == 0) // Tango
 	{
 #ifdef RTABMAP_TANGO
@@ -937,6 +941,19 @@ bool RTABMapApp::startCamera()
 
 		LOGI("Start camera thread");
 		cameraJustInitialized_ = true;
+		if(useExternalLidar_)
+		{
+			rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
+			lidar->init();
+			camera_->setImageRate(0); // if lidar, to get close camera synchronization
+			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
+            sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true);
+		}
+		else
+		{
+			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_);
+		}
+		sensorCaptureThread_->start();
 		return true;
 	}
 	UERROR("Failed camera initialization!");
@@ -948,13 +965,12 @@ void RTABMapApp::stopCamera()
 	LOGI("stopCamera()");
 	{
 		boost::mutex::scoped_lock  lock(cameraMutex_);
-		if(camera_!=0)
+		if(sensorCaptureThread_!=0)
 		{
-			camera_->join(true);
-			camera_->close();
-			delete camera_;
+			sensorCaptureThread_->join(true);
+			delete sensorCaptureThread_; // camera_ is closed and deleted inside
+			sensorCaptureThread_ = 0;
 			camera_ = 0;
-			poseBuffer_.clear();
 		}
 	}
     {
@@ -1241,7 +1257,7 @@ int RTABMapApp::Render()
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
 	try
 	{
-        if(camera_ == 0)
+        if(sensorCaptureThread_ == 0)
         {
             // We are not doing continous drawing, just measure single draw
             fpsTime_.restart();
@@ -1272,49 +1288,45 @@ int RTABMapApp::Render()
 			{
 				if(cameraDriver_ <= 2)
 				{
-					camera_->spinOnce();
+					camera_->updateOnRender();
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
-				LOGW("Camera spinOnce %fs", time.ticks());
+				LOGW("Camera updateOnRender %fs", time.ticks());
 #endif
-
-				if(cameraDriver_ != 2)
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
 				{
-					if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
+					main_scene_.background_renderer_ = new BackgroundRenderer();
+					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
+				}
+				if(camera_->uvsInitialized())
+				{
+					uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
+					((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+					if(graphOptimization_ && !mapToOdom_.isIdentity())
 					{
-						main_scene_.background_renderer_ = new BackgroundRenderer();
-						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
+						rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
+						arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
 					}
-					if(camera_->uvsInitialized())
-					{
-						uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
-						((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
-						if(graphOptimization_ && !mapToOdom_.isIdentity())
-						{
-							rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
-							arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
-						}
-					}
-					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
-					{
-						rtabmap::CameraModel occlusionModel;
-						cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
+				}
+				if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+				{
+					rtabmap::CameraModel occlusionModel;
+					cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
 
-						if(occlusionModel.isValidForProjection())
-						{
-							pcl::IndicesPtr indices(new std::vector<int>);
-							int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
-							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
-							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
-							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
-							occlusionMesh.indices = indices;
-							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
-						}
-						else if(!occlusionImage.empty())
-						{
-							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
-						}
+					if(occlusionModel.isValidForProjection())
+					{
+						pcl::IndicesPtr indices(new std::vector<int>);
+						int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
+						cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
+						occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
+						pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
+						occlusionMesh.indices = indices;
+						occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
+					}
+					else if(!occlusionImage.empty())
+					{
+						UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
 					}
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
@@ -1334,14 +1346,14 @@ int RTABMapApp::Render()
 			}
 		}
 
-		rtabmap::OdometryEvent odomEvent;
+		rtabmap::SensorEvent sensorEvent;
 		{
-			boost::mutex::scoped_lock  lock(odomMutex_);
-			if(odomEvents_.size())
+			boost::mutex::scoped_lock  lock(sensorMutex_);
+			if(sensorEvents_.size())
 			{
-				LOGI("Process odom events");
-				odomEvent = odomEvents_.back();
-				odomEvents_.clear();
+				LOGI("Process sensor events");
+				sensorEvent = sensorEvents_.back();
+				sensorEvents_.clear();
 				if(cameraJustInitialized_)
 				{
 					notifyCameraStarted = true;
@@ -1361,7 +1373,7 @@ int RTABMapApp::Render()
 			{
 				main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl);
 			}
-			if(camera_!=0 && cameraJustInitialized_)
+			if(sensorCaptureThread_!=0 && cameraJustInitialized_)
 			{
 				notifyCameraStarted = true;
 				cameraJustInitialized_ = false;
@@ -1562,9 +1574,9 @@ int RTABMapApp::Render()
 			if(clearSceneOnNextRender_)
 			{
 				LOGI("Clearing all rendering data...");
-				odomMutex_.lock();
-				odomEvents_.clear();
-				odomMutex_.unlock();
+				sensorMutex_.lock();
+				sensorEvents_.clear();
+				sensorMutex_.unlock();
 
 				poseMutex_.lock();
 				poseEvents_.clear();
@@ -1800,7 +1812,7 @@ int RTABMapApp::Render()
 										// Voxelize and filter depending on the previous cloud?
 										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 										pcl::IndicesPtr indices(new std::vector<int>);
-										if(!data.imageRaw().empty() && !data.depthRaw().empty())
+										if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 										{
                                             int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
 											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -2004,26 +2016,26 @@ int RTABMapApp::Render()
 			}
 			else
 			{
-				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && camera_!=0);
+				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0);
 
 				//just process the last one
-				if(!odomEvent.pose().isNull())
+				if(!sensorEvent.info().odomPose.isNull())
 				{
 					if(odomCloudShown_ && !trajectoryMode_)
 					{
-						if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) || !odomEvent.data().laserScanRaw().isEmpty())
+						if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty())
 						{
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
-							if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()))
+							if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
 							{
-                                int meshDecimation = updateMeshDecimation(odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows);
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+                                int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 							}
 							else
 							{
 								//scan
-								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
+								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
 								indices->resize(cloud->size());
 								for(unsigned int i=0; i<cloud->size(); ++i)
 								{
@@ -2034,10 +2046,10 @@ int RTABMapApp::Render()
 							if(cloud->size() && indices->size())
 							{
 								LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
-										odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
-										odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
+										sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows,
+										sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows,
 									   (int)cloud->width, (int)cloud->height);
-								main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
+								main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose);
 								main_scene_.setCloudVisible(-1, true);
 							}
 							else
@@ -2127,7 +2139,7 @@ int RTABMapApp::Render()
 
 				lastPostRenderEventTime_ = UTimer::now();
 
-				if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
+				if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
 				{
 					UERROR("TangoPoseEventNotReceived");
 					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
@@ -2319,7 +2331,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
-	if((camera_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
+	if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
@@ -3709,19 +3721,12 @@ void RTABMapApp::postCameraPoseEvent(
 		if(qx==0 && qy==0 && qz==0 && qw==0)
 		{
 			// Lost! clear buffer
-			poseBuffer_.clear();
 			camera_->resetOrigin(); // we are lost, create new session on next valid frame
 			return;
 		}
 		rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
 		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-		camera_->poseReceived(pose);
-
-		poseBuffer_.insert(std::make_pair(stamp, pose));
-		if(poseBuffer_.size() > 1000)
-		{
-			poseBuffer_.erase(poseBuffer_.begin());
-		}
+		camera_->poseReceived(pose, stamp);
 	}
 }
 
@@ -3833,66 +3838,41 @@ void RTABMapApp::postOdometryEvent(
 				{
 					pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 
+					rtabmap::Transform poseWithOriginOffset = pose;
+					if(!camera_->getOriginOffset().isNull())
+					{
+						poseWithOriginOffset = camera_->getOriginOffset() * pose;
+					}
+
 					// Registration depth to rgb
 					if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
 					{
 						UTimer time;
 						rtabmap::Transform motion = rtabmap::Transform::getIdentity();
-						if(depthStamp != stamp && !poseBuffer_.empty())
+						if(depthStamp != stamp)
 						{
 							// Interpolate pose
-							if(!poseBuffer_.empty())
+							rtabmap::Transform poseDepth;
+							cv::Mat cov;
+							if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
+							{
+								UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
+							}
+							else
 							{
-								if(poseBuffer_.rbegin()->first < depthStamp)
-								{
-									UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first);
-								}
-								else
-								{
-									std::map<double, rtabmap::Transform >::const_iterator iterB = poseBuffer_.lower_bound(depthStamp);
-									std::map<double, rtabmap::Transform >::const_iterator iterA = iterB;
-									rtabmap::Transform poseDepth;
-									if(iterA != poseBuffer_.begin())
-									{
-										iterA = --iterA;
-									}
-									if(iterB == poseBuffer_.end())
-									{
-										iterB = --iterB;
-									}
-									if(iterA == iterB && depthStamp == iterA->first)
-									{
-										poseDepth = iterA->second;
-									}
-									else if(depthStamp >= iterA->first && depthStamp <= iterB->first)
-									{
-										poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
-									}
-									else if(depthStamp < iterA->first)
-									{
-										UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first);
-									}
-									else
-									{
-										UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first);
-									}
-									if(!poseDepth.isNull())
-									{
 #ifndef DISABLE_LOG
-										UDEBUG("poseRGB  =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp);
-										UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
+								UDEBUG("poseRGB  =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp);
+								UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
 #endif
-										motion = pose.inverse()*poseDepth;
-										// transform in camera frame
+								motion = poseWithOriginOffset.inverse()*poseDepth;
+								// transform in camera frame
 #ifndef DISABLE_LOG
-										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+								UDEBUG("motion=%s", motion.prettyPrint().c_str());
 #endif
-										motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
+								motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
 #ifndef DISABLE_LOG
-										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+								UDEBUG("motion=%s", motion.prettyPrint().c_str());
 #endif
-									}
-								}
 							}
 						}
 						rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
@@ -3941,11 +3921,6 @@ void RTABMapApp::postOdometryEvent(
                     
                     if(!outputDepth.empty())
                     {
-                    	rtabmap::Transform poseWithOriginOffset = pose;
-                    	if(!camera_->getOriginOffset().isNull())
-                    	{
-                    		poseWithOriginOffset = camera_->getOriginOffset() * pose;
-                    	}
                         rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
                         depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
                         camera_->setOcclusionImage(outputDepth, depthModel);
@@ -3971,8 +3946,7 @@ void RTABMapApp::postOdometryEvent(
                     texCoords[5] = t5;
                     texCoords[6] = t6;
                     texCoords[7] = t7;
-					camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
-					camera_->spinOnce();
+					camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
 				}
 			}
 		}
@@ -3989,17 +3963,17 @@ void RTABMapApp::postOdometryEvent(
 
 bool RTABMapApp::handleEvent(UEvent * event)
 {
-	if(camera_!=0)
+	if(sensorCaptureThread_!=0)
 	{
 		// called from events manager thread, so protect the data
-		if(event->getClassName().compare("OdometryEvent") == 0)
+		if(event->getClassName().compare("SensorEvent") == 0)
 		{
-			LOGI("Received OdometryEvent!");
-			if(odomMutex_.try_lock())
+			LOGI("Received SensorEvent!");
+			if(sensorMutex_.try_lock())
 			{
-				odomEvents_.clear();
-				odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
-				odomMutex_.unlock();
+				sensorEvents_.clear();
+				sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event)));
+				sensorMutex_.unlock();
 			}
 		}
 		if(event->getClassName().compare("RtabmapEvent") == 0)

app/android/jni/RTABMapApp.h

@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "util.h"
 #include "ProgressionStatus.h"
 
+#include <rtabmap/core/SensorCaptureThread.h>
 #include <rtabmap/core/RtabmapThread.h>
+#include <rtabmap/core/SensorEvent.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <boost/thread/mutex.hpp>
 #include <pcl/pcl_base.h>
@@ -209,6 +211,7 @@ class RTABMapApp : public UEventsHandler {
  private:
   int cameraDriver_;
   rtabmap::CameraMobile * camera_;
+  rtabmap::SensorCaptureThread * sensorCaptureThread_;
   rtabmap::RtabmapThread * rtabmapThread_;
   rtabmap::Rtabmap * rtabmap_;
   rtabmap::LogHandler * logHandler_;
@@ -224,6 +227,7 @@ class RTABMapApp : public UEventsHandler {
   bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;
+  bool useExternalLidar_;
   float maxCloudDepth_;
   float minCloudDepth_;
   int cloudDensityLevel_;
@@ -270,16 +274,15 @@ class RTABMapApp : public UEventsHandler {
     UTimer fpsTime_;
 
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
-	std::list<rtabmap::OdometryEvent> odomEvents_;
+	std::list<rtabmap::SensorEvent> sensorEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
-	std::map<double, rtabmap::Transform> poseBuffer_;
 
 	rtabmap::Transform mapToOdom_;
 
 	boost::mutex cameraMutex_;
 	boost::mutex rtabmapMutex_;
 	boost::mutex meshesMutex_;
-	boost::mutex odomMutex_;
+	boost::mutex sensorMutex_;
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;
 

app/android/jni/background_renderer.cc

@@ -155,7 +155,7 @@ void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
 }
 
 void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) {
-  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+  static_assert(std::extent<decltype(BackgroundRenderer_kVerticesDevice)>::value == kNumVertices * 2, "Incorrect kVertices length");
 
   GLuint program = shaderPrograms_[depthTexture>0?1:0];
     
@@ -170,7 +170,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
   else
 #endif
     glBindTexture(GL_TEXTURE_2D, texture_id_);
-    
+
   if(depthTexture>0)
   {
     // Texture activate unit 1
@@ -191,7 +191,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
   GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
   GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
 
-  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice);
   glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
 
   glEnableVertexAttribArray(attributeVertices);

app/android/jni/background_renderer.h

@@ -28,9 +28,15 @@
 
 #include "util.h"
 
-static const GLfloat BackgroundRenderer_kVertices[] = {
+static const GLfloat BackgroundRenderer_kVerticesDevice[] = {
 	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
 };
+//static const GLfloat BackgroundRenderer_kVerticesView[] = {
+//	0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f,
+//};
+static const GLfloat BackgroundRenderer_kVerticesView[] = {
+	0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+};
 static const GLfloat BackgroundRenderer_kTexCoord[] = {
    1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
 };

app/android/libs/.gitignore

@@ -1 +1,4 @@
-*.jar
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore

app/android/res/values/strings.xml

@@ -84,7 +84,7 @@
     <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
     <string name="pref_default_depth_from_motion">false</string>
     <string name="pref_key_arcore_localization_filtering_speed">pref_key_arcore_localization_filtering_speed</string>
-    <string name="pref_default_arcore_localization_filtering_speed">1</string>
+    <string name="pref_default_arcore_localization_filtering_speed">0</string>
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
     <string name="pref_key_max_speed">pref_key_max_speed</string>

app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java

@@ -115,6 +115,9 @@ public class ARCoreSharedCamera {
 	// Image reader that continuously processes CPU images.
 	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
 	private boolean mTOFAvailable = false;
+
+	ByteBuffer mPreviousDepth = null;
+	double mPreviousDepthStamp = 0.0;
 	
 	public boolean isDepthSupported() {return mTOFAvailable;}
 
@@ -698,6 +701,7 @@ public class ARCoreSharedCamera {
 							mToast.setText(msg);
 						}
 						previousAnchorPose = null;
+						arCoreCorrection = Pose.IDENTITY;
 					}
 				}
 			});
@@ -718,7 +722,8 @@ public class ARCoreSharedCamera {
 				final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8);
 				if(speed>=mARCoreLocalizationFilteringSpeed)
 				{
-					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose).compose(pose.inverse());
+					// Only correct the translation to not lose rotation aligned with gravity
+					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose.compose(pose.inverse()).extractTranslation());
 					t = arCoreCorrection.getTranslation();
 					Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2]));
 					
@@ -744,7 +749,6 @@ public class ARCoreSharedCamera {
 								{
 									mToast.setText(msg);
 								}
-								previousAnchorPose = null;
 							}
 						}
 					});
@@ -755,9 +759,8 @@ public class ARCoreSharedCamera {
 			previousAnchorTimeStamp = frame.getTimestamp();
 			
 			double stamp = (double)frame.getTimestamp()/10e8;
-			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp));
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f arcore %f %f %f cor= %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), pose.tx(), pose.ty(), pose.tz(), arCoreCorrection.tx(), arCoreCorrection.ty(), arCoreCorrection.tz(), stamp));
 			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
-			
 			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
 			try{
 				Image image = frame.acquireCameraImage();
@@ -813,6 +816,12 @@ public class ARCoreSharedCamera {
 						depth = mTOFImageReader.depth16_raw;
 						depthStamp = (double)mTOFImageReader.timestamp/10e8;
 					}
+
+					if(mPreviousDepth == null)
+					{
+						mPreviousDepth = depth;
+						mPreviousDepthStamp = depthStamp;
+					}
 					
 					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
 							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp));
@@ -825,13 +834,17 @@ public class ARCoreSharedCamera {
 							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
 							depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(),
 							stamp,
-							depthStamp,
+							depthStamp>stamp?mPreviousDepthStamp:depthStamp,
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
 							points, points.limit()/4,
 							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
 							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
                             texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
+					
+					
+					mPreviousDepthStamp = depthStamp;
+					mPreviousDepth = depth;
 				}
 				else
 				{

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -284,11 +284,11 @@ void setCameraNative(const void *object, int type) {
 }
 
 void postCameraPoseEventNative(const void *object,
-        float x, float y, float z, float qx, float qy, float qz, float qw)
+        float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
 {
     if(object)
     {
-        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,0.0);
+        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp);
     }
     else
     {

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -67,7 +67,7 @@ bool startCameraNative(const void *object);
 void stopCameraNative(const void *object);
 void setCameraNative(const void *object, int type);
 void postCameraPoseEventNative(const void *object,
-                         float x, float y, float z, float qx, float qy, float qz, float qw);
+                         float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
 void postOdometryEventNative(const void *object,
                        float x, float y, float z, float qx, float qy, float qz, float qw,
                        float fx, float fy, float cx, float cy,

app/ios/RTABMapApp/RTABMap.swift

@@ -216,18 +216,18 @@ class RTABMap {
         setCameraNative(native_rtabmap, Int32(type))
     }
     
-    func postCameraPoseEvent(pose: simd_float4x4) {
+    func postCameraPoseEvent(pose: simd_float4x4, stamp: TimeInterval) {
         let rotation = GLKMatrix3(
             m: (pose[0,0], pose[0,1], pose[0,2],
             pose[1,0], pose[1,1], pose[1,2],
             pose[2,0], pose[2,1], pose[2,2]))
         let quat = GLKQuaternionMakeWithMatrix3(rotation)
-        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, stamp)
     }
     
     func notifyLost() {
         // a null transform will make rtabmap creating a new session
-        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0)
+        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0)
     }
     
     func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) {
@@ -239,7 +239,7 @@ class RTABMap {
         
         let quat = GLKQuaternionMakeWithMatrix3(rotation)
                 
-        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, frame.timestamp)
                 
         let confMap = frame.sceneDepth?.confidenceMap
         let depthMap = frame.sceneDepth?.depthMap

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

app/src/CMakeLists.txt

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

app/src/main.cpp

@@ -55,9 +55,6 @@ int main(int argc, char* argv[])
 	CoInitialize(nullptr);
 #endif
 
-#if VTK_MAJOR_VERSION >= 8 && defined(BUILD_AS_BUNDLE)
-	vtkObject::GlobalWarningDisplayOff();
-#endif
 #if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
     // needed to ensure appropriate OpenGL context is created for VTK rendering.
     QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());

archive/2010-LoopClosure/README.md

@@ -0,0 +1,25 @@
+
+ To reproduce results (based on parameters of this [paper](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/b/bc/TRO2013.pdf)):
+ 
+ ```
+ rtabmap-console \
+       --Rtabmap/StatisticLogged true\
+       --Rtabmap/StatisticLoggedHeaders false\
+       --Kp/DetectorStrategy 0\
+       --Rtabmap/MemoryThr 300\
+       --Rtabmap/LoopRatio 0.9\
+       --SURF/HessianThreshold 150\
+       --Mem/STMSize 30\
+       --Vis/MaxFeatures 400\
+       --Kp/TfIdfLikelihoodUsed false\
+       --Kp/MaxFeatures 400\
+       --Kp/BadSignRatio 0.25\
+       --Mem/BadSignaturesIgnored true\
+       --Mem/RehearsalSimilarity 0.20\
+       --Mem/RecentWmRatio 0.2\
+       -gt "~/Downloads/UdeS_1Hz.png"\
+       ~/Downloads/UdeS_1Hz
+ ```
+Adding the ground truth file here is optional to show recall at 100% precision at the end of the process directly without using the octave/MATLAB script below. For NewCollege and CityCentre datasets, `rtabmap-imagesJoiner` can be used to assemble the left and right images together.
+ 
+ To analyze with Octave/MATLAB, drop `LogF.txt` and `LogI.txt` generated files from command above in ShowLogs directly, then execute `showLogs.m`.

archive/2010-LoopClosure/README.txt

@@ -1,4 +0,0 @@
-This directory contains some basic concepts on Bayes filtering.
-Main scripts :
- RecursivesBayes.m
- RecursivesBayesAvpd.m

archive/2010-LoopClosure/ShowLogs/getPrecisionRecall.m

@@ -32,8 +32,7 @@ if ~isempty(GroundTruth)
     if size(GroundTruth, 1) ~=  length(LogF(:,1)) ||  size(GroundTruth, 1) ~=  length(LogI(:,1))
         error(['The ground truth size doesn''t match the log files (LogI=' num2str(length(LogI(:,1))) ', LogF=' num2str(length(LogF(:,1))) ', GT=' num2str(size(GroundTruth, 1)) ')'])
     end
-    
-    
+
     %[highestHypot, CorrespondingID, GT, Accepted, Good, Index, UnderLoopRatio] descending order
     if(sum(LogI(:,8) == 10) > 0)
         %OLD
@@ -111,14 +110,14 @@ if ~isempty(GroundTruth)
     index = find(PR(:,1) == 1);
     if ~isempty(index)
         maxRecall = PR(index(end),2) * 100;
-        display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2)))])
+        display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2))) '/' num2str(GT_total_positives)])
     else
         display('Recall max (Precision=100%) = 0')
     end
     indexAccepted = find(PR(:,3) == 1);
     if ~isempty(indexAccepted)
         maxRecall = PR(indexAccepted(end),2) * 100;
-        display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2)))])
+        display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2))) '/' num2str(GT_total_positives)])
     else
         display('Recall max accepted (Precision=100%) = 0')
     end

archive/2010-LoopClosure/ShowLogs/showlogs.m

@@ -22,6 +22,8 @@ function [LogF LogI] = showlogs(PathPrefix, GT_file)
 set(0,'defaultAxesFontName', 'Times')
 set(0,'defaultTextFontName', 'Times')
 
+close all
+
 if nargin < 2, GT_file = ''; end
 if nargin < 1, PathPrefix = '.'; end
 
@@ -325,7 +327,7 @@ y(LogI(:, 1) == 0) = [];
 x(LogI(:, 1) == 0) = [];
 plot(x,y, 'g.')
 
-set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))})
+%set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))})
 % %matched sign words
 % y = LogI(:,2);
 % x = 1:length(y);

archive/2010-LoopClosure/run_all.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/NewCollege ~/loop_closure_detection_datasets/NewCollege.png
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/CityCentre ~/loop_closure_detection_datasets/CityCentre.png
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/UdeS_1Hz ~/loop_closure_detection_datasets/UdeS_1Hz.png
+ 

archive/2010-LoopClosure/run_bow.sh

@@ -0,0 +1,32 @@
+#!/bin/bash
+
+DATASET_FOLDER=""
+GT_FILE=""
+if [ $# -eq 2 ] 
+then
+    DATASET_FOLDER=$1
+    GT_FILE=$2
+else
+echo "Usage: run_bow.sh \"dataset folder\" \"ground truth file\""
+exit
+fi
+
+rtabmap-console \
+       -quiet \
+       --Rtabmap/StatisticLogged true\
+       --Rtabmap/StatisticLoggedHeaders false\
+       --Kp/DetectorStrategy 0\
+       --SURF/HessianThreshold 150\
+       --Rtabmap/MemoryThr 300\
+       --Rtabmap/LoopRatio 0.9\
+       --Mem/STMSize 30\
+       --Vis/MaxFeatures 400\
+       --Kp/TfIdfLikelihoodUsed false\
+       --Kp/MaxFeatures 400\
+       --Kp/BadSignRatio 0.25\
+       --Mem/BadSignaturesIgnored true\
+       --Mem/RehearsalSimilarity 0.20\
+       --Mem/RecentWmRatio 0.20\
+       -gt "$GT_FILE"\
+       "$DATASET_FOLDER"
+

archive/2022-IlluminationInvariant/README.md

@@ -25,14 +25,14 @@ The following image shows when we do the same localization experiment at differe
 
 We provide two formats: the first one is more general and the second one is used to produce the results in this paper with RTAB-Map. Please open issue if the links are outdated.
 
-* [Images](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EV8F4PZUxOxLhwAyEehlzKwBjF-9xNuxR32Q4mUjx5u-rA?e=eCJ3TW):
+* [Images](https://drive.google.com/file/d/1fUm1m8oW6q8qlThx7BjrBH2vrVbNQ9bQ/view?usp=drive_link):
   * `rgb`: folder containing *.jpg color camera images
   * `depth`: folder containing *.png 16bits mm depth images
   * `calib`: folder containing calibration for each color image. Each calibration contains also the transform between `device` and `camera` frames as `local_transform`.
   * `device_poses.txt`: VIO poses of each image in `device` frame
   * `camera_poses.txt`: VIO poses of each image in `camera` frame
-* [RTAB-Map Databases](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EU5fb0jEKzlGhPK3OWjMGLUBnDo1BRAoZwtB2czyeVLE_A?e=Y0JyXY)
-
+* [RTAB-Map Databases](https://drive.google.com/file/d/1TklUcTKFSrcg8b0t0U80G_IpFRMVRlY5/view?usp=drive_link)
+* Dataset now also available on [Federated Research Data Repository (FRDR)](https://doi.org/10.20383/103.0931) (if links above don't work)
 
 
 ## How reproduce results shown in the paper

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)

cmake_modules/FindSqlite3.cmake

@@ -1,30 +0,0 @@
-# - Find Sqlite3
-# This module finds an installed Sqlite3 package.
-#
-# It sets the following variables:
-#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
-#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.
-
-FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})
-
-FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
-
-IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
-   SET(Sqlite3_FOUND TRUE)
-   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
-   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
-ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
-
-IF (Sqlite3_FOUND)
-   # show which Sqlite3 was found only if not quiet
-   IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
-   ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (Sqlite3_FOUND)
-   # fatal error if Sqlite3 is required but not found
-   IF (Sqlite3_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find Sqlite3")
-   ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (Sqlite3_FOUND)
-

corelib/include/rtabmap/core/Camera.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -28,47 +28,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once
 
 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/CameraInfo.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
-
-class UDirectory;
-class UTimer;
+#include <rtabmap/core/SensorCapture.h>
+#include <rtabmap/core/IMU.h>
 
 namespace rtabmap
 {
 
+class IMUFilter;
+
 /**
  * Class Camera
  *
  */
-class RTABMAP_CORE_EXPORT Camera
+class RTABMAP_CORE_EXPORT Camera : public SensorCapture
 {
 public:
 	virtual ~Camera();
-	SensorData takeImage(CameraInfo * info = 0);
+
+	SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
+	float getImageRate() const {return getFrameRate();}
+	void setImageRate(float imageRate) {setFrameRate(imageRate);}
+	void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter
+	bool isInterIMUPublishing() const {return publishInterIMU_;}
 
 	bool initFromFile(const std::string & calibrationPath);
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
-	virtual std::string getSerial() const = 0;
-	virtual bool odomProvided() const { return false; }
-	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }
 
-	//getters
-	float getImageRate() const {return _imageRate;}
-	const Transform & getLocalTransform() const {return _localTransform;}
-
-	//setters
-	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-
-	void resetTimer();
 protected:
 	/**
 	 * Constructor
@@ -78,19 +63,16 @@ protected:
 	 */
 	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
 
-	/**
-	 * returned rgb and depth images should be already rectified if calibration was loaded
-	 */
-	virtual SensorData captureImage(CameraInfo * info = 0) = 0;
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0;
 
-	int getNextSeqID() {return ++_seq;}
+	void postInterIMU(const IMU & imu, double stamp);
 
 private:
-	float _imageRate;
-	Transform _localTransform;
-	cv::Size _targetImageSize;
-	UTimer * _frameRateTimer;
-	int _seq;
+	virtual SensorData captureData(SensorCaptureInfo * info = 0) {return captureImage(info);}
+
+private:
+	IMUFilter * imuFilter_;
+	bool publishInterIMU_;
 };
 
 

corelib/include/rtabmap/core/CameraEvent.h

@@ -27,65 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include <rtabmap/utilite/UEvent.h>
-#include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/CameraInfo.h"
-
-namespace rtabmap
-{
-
-class CameraEvent :
-	public UEvent
-{
-public:
-	enum Code {
-		kCodeData,
-		kCodeNoMoreImages
-	};
-
-public:
-	CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
-		UEvent(kCodeData),
-		data_(image, seq, stamp)
-	{
-		cameraInfo_.cameraName = cameraName;
-	}
-
-	CameraEvent() :
-		UEvent(kCodeNoMoreImages)
-	{
-	}
-
-	CameraEvent(const SensorData & data) :
-		UEvent(kCodeData),
-		data_(data)
-	{
-	}
-
-	CameraEvent(const SensorData & data, const std::string & cameraName) :
-		UEvent(kCodeData),
-		data_(data)
-	{
-		cameraInfo_.cameraName = cameraName;
-	}
-	CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) :
-		UEvent(kCodeData),
-		data_(data),
-		cameraInfo_(cameraInfo)
-	{
-	}
-
-	// Image or descriptors
-	const SensorData & data() const {return data_;}
-	const std::string & cameraName() const {return cameraInfo_.cameraName;}
-	const CameraInfo & info() const {return cameraInfo_;}
-
-	virtual ~CameraEvent() {}
-	virtual std::string getClassName() const {return std::string("CameraEvent");}
-
-private:
-	SensorData data_;
-	CameraInfo cameraInfo_;
-};
-
-} // namespace rtabmap
+#include "rtabmap/core/SensorEvent.h"

corelib/include/rtabmap/core/CameraInfo.h

@@ -27,48 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include <string>
-
-namespace rtabmap
-{
-
-class CameraInfo
-{
-
-public:
-	CameraInfo() :
-		cameraName(""),
-		id(0),
-		stamp(0.0),
-		timeCapture(0.0f),
-		timeDisparity(0.0f),
-		timeMirroring(0.0f),
-		timeStereoExposureCompensation(0.0f),
-		timeImageDecimation(0.0f),
-		timeScanFromDepth(0.0f),
-		timeUndistortDepth(0.0f),
-		timeBilateralFiltering(0.0f),
-		timeTotal(0.0f),
-		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
-	{
-	}
-	virtual ~CameraInfo() {}
-
-	std::string cameraName;
-	int id;
-	double stamp;
-	float timeCapture;
-	float timeDisparity;
-	float timeMirroring;
-	float timeStereoExposureCompensation;
-	float timeImageDecimation;
-	float timeScanFromDepth;
-	float timeUndistortDepth;
-	float timeBilateralFiltering;
-	float timeTotal;
-	Transform odomPose;
-	cv::Mat odomCovariance;
-	std::vector<float> odomVelocity;
-};
-
-} // namespace rtabmap
+#include "rtabmap/core/SensorCaptureInfo.h"

corelib/include/rtabmap/core/CameraRGBD.h

@@ -37,4 +37,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraRealSense2.h>
 #include <rtabmap/core/camera/CameraRGBDImages.h>
 #include <rtabmap/core/camera/CameraK4A.h>
-
+#include <rtabmap/core/camera/CameraSeerSense.h>

corelib/include/rtabmap/core/CameraThread.h

@@ -27,129 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/UThread.h>
-#include <rtabmap/utilite/UEventsSender.h>
-
-namespace clams
-{
-class DiscreteDepthDistortionModel;
-}
-
-namespace rtabmap
-{
-
-class Camera;
-class CameraInfo;
-class SensorData;
-class StereoDense;
-class IMUFilter;
-
-/**
- * Class CameraThread
- *
- */
-class RTABMAP_CORE_EXPORT CameraThread :
-	public UThread,
-	public UEventsSender
-{
-public:
-	// ownership transferred
-	CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
-	/**
-	 * @param camera the camera to take images from
-	 * @param odomSensor an odometry sensor to get a pose
-	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame
-	 */
-	CameraThread(Camera * camera,
-			Camera * odomSensor,
-			const Transform & extrinsics,
-			double poseTimeOffset = 0.0,
-			float poseScaleFactor = 1.0f,
-			bool odomAsGt = false,
-			const ParametersMap & parameters = ParametersMap());
-	CameraThread(Camera * camera,
-				bool odomAsGt,
-				const ParametersMap & parameters = ParametersMap());
-	virtual ~CameraThread();
-
-	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
-	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
-	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
-	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
-	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
-	void setImageRate(float imageRate);
-	void setDistortionModel(const std::string & path);
-	void enableBilateralFiltering(float sigmaS, float sigmaR);
-	void disableBilateralFiltering() {_bilateralFiltering = false;}
-	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
-	void disableIMUFiltering();
-
-	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
-	RTABMAP_DEPRECATED void setScanParameters(
-			bool fromDepth,
-			int downsampleStep, // decimation of the depth image in case the scan is from depth image
-			float rangeMin,
-			float rangeMax,
-			float voxelSize,
-			int normalsK,
-			int normalsRadius,
-			bool forceGroundNormalsUp);
-	void setScanParameters(
-			bool fromDepth,
-			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
-			float rangeMin=0.0f,
-			float rangeMax=0.0f,
-			float voxelSize = 0.0f,
-			int normalsK = 0,
-			int normalsRadius = 0.0f,
-			float groundNormalsUp = 0.0f);
-
-	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
-
-	//getters
-	bool isPaused() const {return !this->isRunning();}
-	bool isCapturing() const {return this->isRunning();}
-	bool odomProvided() const;
-
-	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
-	Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
-
-private:
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-	virtual void mainLoopKill();
-
-private:
-	Camera * _camera;
-	Camera * _odomSensor;
-	Transform _extrinsicsOdomToCamera;
-	bool _odomAsGt;
-	double _poseTimeOffset;
-	float _poseScaleFactor;
-	bool _mirroring;
-	bool _stereoExposureCompensation;
-	bool _colorOnly;
-	int _imageDecimation;
-	bool _stereoToDepth;
-	bool _scanFromDepth;
-	int _scanDownsampleStep;
-	float _scanRangeMin;
-	float _scanRangeMax;
-	float _scanVoxelSize;
-	int _scanNormalsK;
-	float _scanNormalsRadius;
-	float _scanForceGroundNormalsUp;
-	StereoDense * _stereoDense;
-	clams::DiscreteDepthDistortionModel * _distortionModel;
-	bool _bilateralFiltering;
-	float _bilateralSigmaS;
-	float _bilateralSigmaR;
-	IMUFilter * _imuFilter;
-	bool _imuBaseFrameConversion;
-};
-
-} // namespace rtabmap
+#include "rtabmap/core/SensorCaptureThread.h"

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/DBReader.h

@@ -82,14 +82,15 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const {return !_odometryIgnored;}
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
 
 	const DBDriver * driver() const {return _dbDriver;}
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
-	SensorData getNextData(CameraInfo * info = 0);
+	SensorData getNextData(SensorCaptureInfo * info = 0);
 
 private:
 	std::list<std::string> _paths;

corelib/include/rtabmap/core/Features2d.h

@@ -192,16 +192,17 @@ public:
 			const cv::Mat & disparity,
 			float minDisparity);
 
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
-	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
-	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc = false);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
 	int getMaxFeatures() const {return maxFeatures_;}
+	bool getSSC() const {return SSC_;}
 	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
 	int getGridRows() const {return gridRows_;}
@@ -234,6 +235,7 @@ private:
 private:
 	ParametersMap parameters_;
 	int maxFeatures_;
+	bool SSC_;
 	float _maxDepth; // 0=inf
 	float _minDepth;
 	std::vector<float> _roiRatios; // size 4

corelib/include/rtabmap/core/GlobalDescriptor.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/GlobalDescriptorExtractor.h

@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2010-2024, 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 GLOBAL_DESCRIPTOR_EXTRACTOR_H_
+#define GLOBAL_DESCRIPTOR_EXTRACTOR_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
+
+
+namespace rtabmap {
+
+// Feature2D
+class RTABMAP_CORE_EXPORT GlobalDescriptorExtractor {
+public:
+	enum Type {
+		kUndef=0,
+		kPyDescriptor=1};
+
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kPyDescriptor:
+			return "PyDescriptor";
+		default:
+			return "Unknown";
+		}
+	}
+
+	static GlobalDescriptorExtractor * create(const ParametersMap & parameters = ParametersMap());
+	static GlobalDescriptorExtractor * create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
+
+public:
+	virtual ~GlobalDescriptorExtractor();
+
+	virtual GlobalDescriptor extract(const SensorData & data) const = 0;
+
+	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual GlobalDescriptorExtractor::Type getType() const = 0;
+
+protected:
+	GlobalDescriptorExtractor(const ParametersMap & parameters = ParametersMap());
+};
+
+}
+
+#endif /* GLOBAL_DESCRIPTOR_EXTRACTOR_H_ */

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/IMUFilter.h

@@ -28,12 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
 
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include <rtabmap/core/Parameters.h>
 #include <Eigen/Geometry>
 
 namespace rtabmap {
 
-class IMUFilter
+class RTABMAP_CORE_EXPORT IMUFilter
 {
 public:
 	enum Type {

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/LaserScan.h

@@ -47,7 +47,8 @@ public:
 		kXYZRGB=7,
 		kXYZNormal=8,
 		kXYZINormal=9,
-		kXYZRGBNormal=10};
+		kXYZRGBNormal=10,
+		kXYZIT=11};
 
 	static std::string formatName(const Format & format);
 	static int channels(const Format & format);
@@ -55,6 +56,7 @@ public:
 	static bool isScanHasNormals(const Format & format);
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
+	static bool isScanHasTime(const Format & format);
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			int maxPoints = 0,
@@ -121,22 +123,27 @@ public:
 	float angleMin() const {return angleMin_;}
 	float angleMax() const {return angleMax_;}
 	float angleIncrement() const {return angleIncrement_;}
+	void setLocalTransform(const Transform & t) {localTransform_ = t;}
 	Transform localTransform() const {return localTransform_;}
 
 	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
-	int size() const {return data_.cols;}
+	int size() const {return data_.total();}
 	int dataType() const {return data_.type();}
 	bool is2d() const {return isScan2d(format_);}
 	bool hasNormals() const {return isScanHasNormals(format_);}
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
+	bool hasTime() const {return isScanHasTime(format_);}
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
+	bool isOrganized() const {return data_.rows > 1;}
 	LaserScan clone() const;
+	LaserScan densify() const;
 
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
+	int getTimeOffset() const {return hasTime()?4:-1;}
 
 	float & field(unsigned int pointIndex, unsigned int channelOffset);
 

corelib/include/rtabmap/core/Lidar.h

@@ -0,0 +1,57 @@
+/*
+Copyright (c) 2010-2022, 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include <rtabmap/core/SensorCapture.h>
+
+namespace rtabmap
+{
+
+/**
+ * Class Lidar
+ *
+ */
+class RTABMAP_CORE_EXPORT Lidar : public SensorCapture
+{
+public:
+	virtual ~Lidar() {}
+
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param lidarRate the frame rate (Hz), 0 for fast as the lidar can
+	 * @param localTransform the transform from base frame to lidar frame
+	 */
+	Lidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) :
+		SensorCapture(lidarRate, localTransform) {}
+};
+
+
+} // 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,8 +57,9 @@ class RegistrationInfo;
 class RegistrationIcp;
 class RegistrationVis;
 class Stereo;
-class OccupancyGrid;
+class LocalGridMaker;
 class MarkerDetector;
+class GlobalDescriptorExtractor;
 
 class RTABMAP_CORE_EXPORT Memory
 {
@@ -330,8 +331,10 @@ private:
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
 	bool _useOdometryGravity;
+	bool _rotateImagesUpsideUp;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
+	bool _visSSC;
 	bool _imagesAlreadyRectified;
 	bool _rectifyOnlyFeatures;
 	bool _covOffDiagonalIgnored;
@@ -371,9 +374,11 @@ private:
 	RegistrationIcp * _registrationIcpMulti;
 	RegistrationVis * _registrationVis;
 
-	OccupancyGrid * _occupancy;
+	LocalGridMaker * _localMapMaker;
 
 	MarkerDetector * _markerDetector;
+
+	GlobalDescriptorExtractor * _globalDescriptorExtractor;
 };
 
 } // namespace rtabmap

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/Odometry.h

@@ -111,6 +111,7 @@ private:
 	bool _alignWithGround;
 	bool _publishRAMUsage;
 	bool _imagesAlreadyRectified;
+	bool _deskewing;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -50,6 +50,7 @@ public:
 		localBundleConstraints(0),
 		localBundleTime(0),
 		keyFrameAdded(false),
+		timeDeskewing(0.0f),
 		timeEstimation(0.0f),
 		timeParticleFiltering(0.0f),
 		stamp(0),
@@ -76,6 +77,7 @@ public:
 		output.localBundlePoses = localBundlePoses;
 		output.localBundleModels = localBundleModels;
 		output.keyFrameAdded = keyFrameAdded;
+		output.timeDeskewing = timeDeskewing;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
@@ -105,6 +107,7 @@ public:
 	std::map<int, Transform> localBundlePoses;
 	std::map<int, std::vector<CameraModel> > localBundleModels;
 	bool keyFrameAdded;
+	float timeDeskewing;
 	float timeEstimation;
 	float timeParticleFiltering;
 	double stamp;

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

@@ -197,6 +197,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
     RTABMAP_PARAM(Rtabmap, LoopRatio,         float, 0,         "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
     RTABMAP_PARAM(Rtabmap, LoopGPS,           bool,  true,      uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));
+    RTABMAP_PARAM(Rtabmap, VirtualPlaceLikelihoodRatio, int, 0,  "Likelihood ratio for virtual place (for no loop closure hypothesis): 0=Mean / StdDev, 1=StdDev / (Max-Mean)");
 
     // Memory
     RTABMAP_PARAM(Mem, RehearsalSimilarity,         float, 0.6,     "Rehearsal similarity.");
@@ -231,6 +232,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features instead of regenerating them.");
     RTABMAP_PARAM(Mem, UseOdomGravity,              bool, false,    uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
     RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
+    RTABMAP_PARAM(Mem, GlobalDescriptorStrategy,    int, 0,        "Extract global descriptor from sensor data. 0=disabled, 1=PyDescriptor");
+    RTABMAP_PARAM(Mem, RotateImagesUpsideUp,        bool, false,    "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");
 
     // KeypointMemory (Keypoint-based)
     RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -241,6 +244,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
+    RTABMAP_PARAM(Kp, SSC,                      bool, false,  "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
     RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
@@ -353,10 +357,12 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
     RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
+    RTABMAP_PARAM(RGBD, AggressiveLoopThr,        float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
     RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
+    RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
     RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
     RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
     RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
@@ -376,6 +382,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.");
@@ -433,6 +441,9 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(g2o, Baseline,          double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
 
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
+    RTABMAP_PARAM(GTSAM, Incremental,     bool, false,     uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));
+    RTABMAP_PARAM(GTSAM, IncRelinearizeThreshold,  double, 0.01, "Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info.");
+    RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
 
     // Odometry
     RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
@@ -455,6 +466,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, ImageDecimation,     unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
+    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).");
 
     // Odometry Frame-to-Map
     RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -525,12 +537,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 +594,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 +666,58 @@ 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, PnPSplitLinearCovComponents, bool,  false, uFormat("[%s = 1] Compute variance for each linear component instead of using the combined XYZ variance for all linear components.", kVisEstimationType().c_str()).c_str());
 
-    RTABMAP_PARAM(Vis, 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, SSC,                       bool,  false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
+    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
@@ -646,6 +731,10 @@ class RTABMAP_CORE_EXPORT Parameters
 	RTABMAP_PARAM(GMS, WithScale,            bool, false,   "Take scale transformation into account.");
 	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");
 
+	// Global descriptor approaches
+	RTABMAP_PARAM_STR(PyDescriptor, Path,    "",          "Path to python script file (see available ones in rtabmap/corelib/src/pydescriptor/*). See the header to see where the script should be used.");
+	RTABMAP_PARAM(PyDescriptor, Dim,         int, 4096,   "Descriptor dimension.");
+
     // ICP registration parameters
 #ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, Strategy,                  int, 1,       "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
@@ -668,6 +757,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
     RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
     RTABMAP_PARAM(Icp, Force4DoF,                 bool, false,   uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
+    RTABMAP_PARAM(Icp, FiltersEnabled,            int, 3,       "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both.");
 #ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else
@@ -763,8 +853,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).");
@@ -849,6 +937,7 @@ public:
     static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);
 
     static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
+    static void readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly = false);
     static void writeINI(const std::string & configFile, const ParametersMap & parameters);
 
     /**

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/RegistrationIcp.h

@@ -69,6 +69,7 @@ private:
 	float _epsilon;
 	float _correspondenceRatio;
 	bool _force4DoF;
+	int _filtersEnabled;
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;

corelib/include/rtabmap/core/RegistrationVis.h

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

corelib/include/rtabmap/core/Rtabmap.h

@@ -151,6 +151,8 @@ public:
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
+	int getMemoryThreshold() const {return _maxMemoryAllowed;} // in nodes
+	void setMemoryThreshold(int maxMemoryAllowed); // in nodes
 
 	void setInitialPose(const Transform & initialPose);
 	int triggerNewMap();
@@ -282,6 +284,8 @@ private:
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
 	float _loopRatio;
+	float _aggressiveLoopThr;
+	int _virtualPlaceLikelihoodRatio;
 	float _maxLoopClosureDistance;
 	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
@@ -322,10 +326,13 @@ private:
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
+	bool _forceOdom3doF;
 	bool _restartAtOrigin;
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;
+	bool _localizationSmoothing;
+	double _localizationPriorInf;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;

corelib/include/rtabmap/core/SensorCapture.h

@@ -0,0 +1,93 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+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 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include "rtabmap/core/SensorData.h"
+#include <set>
+#include <stack>
+#include <list>
+#include <vector>
+
+class UDirectory;
+class UTimer;
+
+namespace rtabmap
+{
+
+/**
+ * Class Camera
+ *
+ */
+class RTABMAP_CORE_EXPORT SensorCapture
+{
+public:
+	virtual ~SensorCapture();
+	SensorData takeData(SensorCaptureInfo * info = 0);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; }
+
+	//getters
+	float getFrameRate() const {return _frameRate;}
+	const Transform & getLocalTransform() const {return _localTransform;}
+
+	//setters
+	void setFrameRate(float frameRate) {_frameRate = frameRate;}
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
+
+	void resetTimer();
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param frameRate the frame rate (Hz), 0 for fast as the sensor can
+	 * @param localTransform the transform from base frame to sensor frame
+	 */
+	SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity());
+
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0;
+
+	int getNextSeqID() {return ++_seq;}
+
+private:
+	float _frameRate;
+	Transform _localTransform;
+	UTimer * _frameRateTimer;
+	int _seq;
+};
+
+
+} // namespace rtabmap

corelib/include/rtabmap/core/SensorCaptureInfo.h

@@ -0,0 +1,82 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/Transform.h"
+#include <string>
+
+namespace rtabmap
+{
+
+class SensorCaptureInfo
+{
+
+public:
+	SensorCaptureInfo() :
+		cameraName(""),
+		id(0),
+		stamp(0.0),
+		timeCapture(0.0f),
+		timeDeskewing(0.0f),
+		timeDisparity(0.0f),
+		timeMirroring(0.0f),
+		timeStereoExposureCompensation(0.0f),
+		timeImageDecimation(0.0f),
+		timeHistogramEqualization(0.0f),
+		timeScanFromDepth(0.0f),
+		timeUndistortDepth(0.0f),
+		timeBilateralFiltering(0.0f),
+		timeTotal(0.0f),
+		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
+	{
+	}
+	virtual ~SensorCaptureInfo() {}
+
+	std::string cameraName;
+	int id;
+	double stamp;
+	float timeCapture;
+	float timeDeskewing;
+	float timeDisparity;
+	float timeMirroring;
+	float timeStereoExposureCompensation;
+	float timeImageDecimation;
+	float timeHistogramEqualization;
+	float timeScanFromDepth;
+	float timeUndistortDepth;
+	float timeBilateralFiltering;
+	float timeTotal;
+	Transform odomPose;
+	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo;
+
+} // namespace rtabmap

corelib/include/rtabmap/core/SensorCaptureThread.h

@@ -0,0 +1,216 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
+
+namespace clams
+{
+class DiscreteDepthDistortionModel;
+}
+
+namespace rtabmap
+{
+
+class Camera;
+class Lidar;
+class SensorCapture;
+class SensorCaptureInfo;
+class SensorData;
+class StereoDense;
+class IMUFilter;
+class Feature2D;
+
+/**
+ * Class CameraThread
+ *
+ */
+class RTABMAP_CORE_EXPORT SensorCaptureThread :
+	public UThread,
+	public UEventsSender
+{
+public:
+	// ownership transferred
+	SensorCaptureThread(
+			Camera * camera,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose (can be again the camera)
+	 * @param odomAsGt set odometry sensor pose as ground truth instead of odometry
+	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame  (without optical rotation)
+	 */
+	SensorCaptureThread(
+			Camera * camera,
+			SensorCapture * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			Camera * camera,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar)
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			SensorCapture * odomSensor,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar)
+	 * @param extrinsics the static transform between odometry frame to camera frame (without optical rotation)
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			Camera * camera,
+			SensorCapture * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	virtual ~SensorCaptureThread();
+
+	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	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 setFrameRate(float frameRate);
+	RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);}
+	void setDistortionModel(const std::string & path);
+	void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;}
+	void enableBilateralFiltering(float sigmaS, float sigmaR);
+	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(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			float normalsRadius,
+			bool forceGroundNormalsUp,
+			bool deskewing);
+	void setScanParameters(
+			bool fromDepth,
+			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
+			float rangeMin=0.0f,
+			float rangeMax=0.0f,
+			float voxelSize = 0.0f,
+			int normalsK = 0,
+			float normalsRadius = 0.0f,
+			float groundNormalsUp = 0.0f,
+			bool deskewing = false);
+
+	void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const;
+
+	//getters
+	bool isPaused() const {return !this->isRunning();}
+	bool isCapturing() const {return this->isRunning();}
+	bool odomProvided() const;
+
+	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
+	SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
+	Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+	virtual void mainLoopKill();
+
+private:
+	Camera * _camera;
+	SensorCapture * _odomSensor;
+	Lidar * _lidar;
+	Transform _extrinsicsOdomToCamera;
+	bool _odomAsGt;
+	double _poseTimeOffset;
+	float _poseScaleFactor;
+	double _poseWaitTime;
+	bool _mirroring;
+	bool _stereoExposureCompensation;
+	bool _colorOnly;
+	int _imageDecimation;
+	int _histogramMethod;
+	bool _stereoToDepth;
+	bool _scanDeskewing;
+	bool _scanFromDepth;
+	int _scanDownsampleStep;
+	float _scanRangeMin;
+	float _scanRangeMax;
+	float _scanVoxelSize;
+	int _scanNormalsK;
+	float _scanNormalsRadius;
+	float _scanForceGroundNormalsUp;
+	StereoDense * _stereoDense;
+	clams::DiscreteDepthDistortionModel * _distortionModel;
+	bool _bilateralFiltering;
+	float _bilateralSigmaS;
+	float _bilateralSigmaR;
+	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
+	Feature2D * _featureDetector;
+	bool _depthAsMask;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread;
+
+} // namespace rtabmap

corelib/include/rtabmap/core/SensorEvent.h

@@ -0,0 +1,94 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include <rtabmap/utilite/UEvent.h>
+#include "rtabmap/core/SensorData.h"
+
+namespace rtabmap
+{
+
+class SensorEvent :
+	public UEvent
+{
+public:
+	enum Code {
+		kCodeData,
+		kCodeNoMoreImages
+	};
+
+public:
+	SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
+		UEvent(kCodeData),
+		data_(image, seq, stamp)
+	{
+		sensorCaptureInfo_.cameraName = cameraName;
+	}
+
+	SensorEvent() :
+		UEvent(kCodeNoMoreImages)
+	{
+	}
+
+	SensorEvent(const SensorData & data) :
+		UEvent(kCodeData),
+		data_(data)
+	{
+	}
+
+	SensorEvent(const SensorData & data, const std::string & cameraName) :
+		UEvent(kCodeData),
+		data_(data)
+	{
+		sensorCaptureInfo_.cameraName = cameraName;
+	}
+	SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) :
+		UEvent(kCodeData),
+		data_(data),
+		sensorCaptureInfo_(sensorCaptureInfo)
+	{
+	}
+
+	// Image or descriptors
+	const SensorData & data() const {return data_;}
+	const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;}
+	const SensorCaptureInfo & info() const {return sensorCaptureInfo_;}
+
+	virtual ~SensorEvent() {}
+	virtual std::string getClassName() const {return std::string("SensorEvent");}
+
+private:
+	SensorData data_;
+	SensorCaptureInfo sensorCaptureInfo_;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorEvent CameraEvent;
+
+} // namespace rtabmap

corelib/include/rtabmap/core/Signature.h

@@ -90,9 +90,10 @@ public:
 	void removeLink(int idTo);
 	void removeVirtualLinks();
 
-	void addLandmark(const Link & landmark) {_landmarks.insert(std::make_pair(landmark.to(), landmark));}
+	void addLandmark(const Link & landmark);
 	const std::map<int, Link> & getLandmarks() const {return _landmarks;}
-	void removeLandmarks() {_landmarks.clear();}
+	void removeLandmarks();
+	void removeLandmark(int landmarkId);
 
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}

corelib/include/rtabmap/core/Statistics.h

@@ -157,6 +157,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
 	RTABMAP_STATS(Timing, Proximity_by_time, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space_search, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);

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

@@ -49,39 +49,65 @@ 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 setExtendedDisparity(bool extendedDisparity);
+	void setSubpixelMode(bool enabled, int fractionalBits = 3);
+	void setCompanding(bool enabled, int width=96);
+	void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f, bool enabled=true);
+	void setIMU(bool imuPublished, bool publishInterIMU);
+	void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.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;
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 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 extendedDisparity_;
+	int subpixelFractionalBits_;
+	int compandingWidth_;
+	bool useSpecTranslation_;
+	float alphaScaling_;
+	bool imagesRectified_;
 	bool imuPublished_;
+	bool publishInterIMU_;
+	float dotIntensity_;
+	float floodIntensity_;
+	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> rightOrDepthQueue_;
-	std::shared_ptr<dai::DataOutputQueue> imuQueue_;
+	std::shared_ptr<dai::DataOutputQueue> cameraQueue_;
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
+	UMutex imuMutex_;
 #endif
 };
 

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

@@ -61,7 +61,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_FREENECT

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

@@ -77,7 +77,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_FREENECT2

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

@@ -118,7 +118,7 @@ public:
 	}
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	bool readPoses(

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

@@ -63,7 +63,7 @@ public:
 	void setPreferences(int rgb_resolution, int framerate, int depth_resolution);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	void close();

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

@@ -72,7 +72,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	void close();

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

@@ -67,7 +67,7 @@ protected:
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded
 	 */
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_MYNTEYE

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

@@ -69,7 +69,7 @@ public:
 	void setDepthDecimation(int decimation);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_OPENNI2

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

@@ -51,7 +51,7 @@ public:
 	virtual std::string getSerial() const {return "";} // unknown with OpenCV
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	bool _asus;

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

@@ -34,15 +34,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/pcl_config.h>
 
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 #if __linux__ && __i386__ && __cplusplus >= 201103L
 #warning "Openni driver is not available on i386 when building with c++11 support"
-#else
-#define RTABMAP_OPENNI
+#endif
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
 #endif
-#endif
 
 #include <boost/signals2/connection.hpp>
 
@@ -74,7 +72,7 @@ public:
 			float constant);
 #else
     void image_cb (
-    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+    	const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 			float constant);
 #endif
@@ -85,7 +83,7 @@ public:
     virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
     pcl::Grabber* interface_;

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

@@ -53,7 +53,7 @@ public:
 	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	CameraImages cameraDepth_;

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

@@ -72,7 +72,7 @@ public:
 	virtual bool odomProvided() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_REALSENSE

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

@@ -68,7 +68,7 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const;
-	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
 
 	// parameters are set during initialization
 	// D400 series
@@ -77,7 +77,7 @@ public:
 	void setResolution(int width, int height, int fps = 30);
 	void setDepthResolution(int width, int height, int fps = 30);
 	void setGlobalTimeSync(bool enabled);
-	void publishInterIMU(bool enabled);
+
 	/**
 	 * Dual mode (D400+T265 or L500+T265)
 	 * @param enabled enable dual mode
@@ -105,7 +105,7 @@ private:
 #endif
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_REALSENSE2
@@ -142,7 +142,6 @@ private:
 	int cameraDepthHeight_;
 	int cameraDepthFps_;
 	bool globalTimeSync_;
-	bool publishInterIMU_;
 	bool dualMode_;
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;

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

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+#include "rtabmap/utilite/USemaphore.h"
+
+#ifdef RTABMAP_XVSDK
+#include <xv-sdk.h>
+#endif
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraSeerSense :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraSeerSense(
+		bool computeOdometry = false,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity()
+	);
+	virtual ~CameraSeerSense();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_XVSDK
+	CameraModel cameraModel_;
+	bool computeOdometry_;
+	int imuId_;
+	int tofId_;
+	std::shared_ptr<xv::Device> device_;
+	std::map<double, std::pair<cv::Vec3d, cv::Vec3d>> imuBuffer_;
+	std::pair<double, std::pair<cv::Mat, cv::Mat>> lastData_;
+	UMutex imuMutex_;
+	UMutex dataMutex_;
+	USemaphore dataReady_;
+#endif
+};
+
+} // namespace rtabmap

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

@@ -51,7 +51,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_DC1394

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

@@ -53,7 +53,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_FLYCAPTURE2

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

@@ -64,7 +64,7 @@ public:
 	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);}
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	CameraImages * camera2_;

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

@@ -60,7 +60,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	cv::VideoCapture capture_;

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

@@ -71,7 +71,7 @@ public:
 	void setResolution(int width, int height) {_width=width, _height=height;}
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	cv::VideoCapture capture_;

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,
@@ -76,12 +76,12 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const;
-	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
 
-	void publishInterIMU(bool enabled);
+	void postInterIMUPublic(const IMU & imu, double stamp);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 #ifdef RTABMAP_ZED
@@ -100,7 +100,6 @@ private:
 	bool computeOdometry_;
 	bool lost_;
 	bool force3DoF_;
-	bool publishInterIMU_;
 	ZedIMUThread * imuPublishingThread_;
 #endif
 };