latest update

Co-authored-by: FIRST_NAME LAST_NAME <MY_NAME@example.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
   # 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\""

.github/workflows/cmake-ros.yml

@@ -2,10 +2,10 @@ name: CMake-ROS
 
 on:
   push:
-    branches:
+    branches: 
-      - '**'
+      - master
   pull_request:
-    branches:
+    branches: 
       - '**'
 
 env:
@@ -18,50 +18,49 @@ jobs:
     # well on Windows or Mac.  You can convert this to a matrix build if you need
     # cross-platform coverage.
     # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-20.04, ubuntu-18.04]
+        ros_distribution: [ noetic, foxy, humble, rolling]
         include:
-        - os: ubuntu-20.04
+        - ros_distribution: 'noetic'
-          ros_distro: 'noetic'
+          os: ubuntu-20.04
-        - os: ubuntu-18.04
+        - ros_distribution: 'foxy'
-          ros_distro: 'melodic'
+          os: ubuntu-20.04
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-22.04
 
     steps:
-    - uses: ros-tooling/setup-ros@v0.2
+    - name: Workaround dpkg grub-efi-amd64-signed error
-      with:
-        required-ros-distributions: ${{ matrix.ros_distro }}
-    
-    - name: Install dependencies
       run: |
-        sudo apt-get update
+        sudo apt-mark hold grub-efi-amd64-signed
-        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
+    
-        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
+    - uses: ros-tooling/setup-ros@v0.5
-
+      with:
+        required-ros-distributions: ${{ matrix.ros_distribution }}
+    
     - uses: actions/checkout@v2
 
-    - name: Configure CMake
+    - name: Install dependencies
-      # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
-      # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
       run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        rosdep update
+        rosdep install --from-paths ${{github.workspace}} -y
+
+    - name: Configure CMake
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
         cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
 
     - name: Build
-      # Build your program with the given configuration
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
 
     - name: Info
       working-directory: ${{github.workspace}}/build/bin
       run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
         ./rtabmap-console --version
-
-#    - name: Test
-#      working-directory: ${{github.workspace}}/build
-#      # Execute tests defined by the CMake configuration.  
-#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
-#      run: ctest -C ${{env.BUILD_TYPE}}
       

.github/workflows/cmake.yml

@@ -3,7 +3,7 @@ name: CMake
 on:
   push:
     branches: 
-      - '**'
+      - master
   pull_request:
     branches: 
       - '**'
@@ -17,14 +17,14 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-22.04, ubuntu-20.04, ubuntu-18.04]
+        os: [ubuntu-22.04, ubuntu-20.04]
 
     steps:    
     - name: Install dependencies
       run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
 
     - uses: actions/checkout@v2
 

.github/workflows/docker.yml

@@ -11,12 +11,14 @@ jobs:
     
     strategy:
       matrix:
-        docker_tag: [xenial, bionic, focal, android23, android24, android26, android30]
+        docker_tag: [xenial, bionic, focal, focal-foxy, jammy, android23, android24, android26, android30]
         include:
         - docker_tag: xenial
           docker_tags: |
             introlab3it/rtabmap:xenial
             introlab3it/rtabmap:16.04
+          docker_args: |
+            NOT_USED=0
           docker_platforms: |
             linux/amd64
           docker_path: 'xenial'
@@ -24,6 +26,8 @@ jobs:
           docker_tags: |
             introlab3it/rtabmap:bionic
             introlab3it/rtabmap:18.04
+          docker_args: |
+            NOT_USED=0
           docker_platforms: |
             linux/amd64
             linux/arm64
@@ -33,10 +37,32 @@ jobs:
             introlab3it/rtabmap:focal
             introlab3it/rtabmap:20.04
             introlab3it/rtabmap:latest
+          docker_args: |
+            NOT_USED=0
           docker_platforms: |
             linux/amd64
             linux/arm64
           docker_path: 'focal'
+        - docker_tag: focal-foxy
+          docker_tags: |
+            introlab3it/rtabmap:focal-foxy
+            introlab3it/rtabmap:20.04-foxy
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal-foxy'
+        - docker_tag: jammy
+          docker_tags: |
+            introlab3it/rtabmap:jammy
+            introlab3it/rtabmap:22.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy'
         - docker_tag: android23
           docker_tags: |
             introlab3it/rtabmap:android23

.gitignore

@@ -10,3 +10,5 @@ app/android/AndroidManifest.xml
 app/android/res/raw/
 compile_flags.txt
 tags
+build_*
+*.bak

CMakeLists.txt

@@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 20)
+SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 21)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -69,7 +69,11 @@ if(POLICY CMP0020)
     cmake_policy(SET CMP0020 NEW)
 endif()
 if(POLICY CMP0043)
-    cmake_policy(SET CMP0043 OLD)
+    cmake_policy(SET CMP0043 NEW)
+endif()
+# To suppress g2o related opengl warning
+if(POLICY CMP0072)
+    cmake_policy(SET CMP0072 NEW)
 endif()
 
 IF(MINGW)
@@ -106,6 +110,7 @@ if(MSVC)
       SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N}")
     endif()
   endif()
+  add_compile_options("/bigobj")
 endif()
 
 # [Eclipse] Automatic Discovery of Include directories (Optional, but handy) 
@@ -145,7 +150,7 @@ endif()
 set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR})
   
 ####### BUILD OPTIONS #######
-
+include (GenerateExportHeader)
 # ANDROID_PREBUILD (early exit if true)
 OPTION( ANDROID_PREBUILD "Set to ON to build rtabmap resource build tool (required for android build)" OFF )
 IF(ANDROID_PREBUILD)
@@ -182,6 +187,7 @@ option(WITH_G2O           "Include g2o support"                  ON)
 option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_CERES         "Include Ceres support"                OFF)
+option(WITH_MRPT          "Include MRPT support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                OFF)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
@@ -212,7 +218,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()
@@ -220,9 +226,9 @@ option(PCL_OMP            "With PCL OMP implementations"         ON)
 ENDIF()
 
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
-set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5)
+set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)
 
-FIND_PACKAGE(OpenCV REQUIRED QUIET 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)
@@ -291,35 +297,50 @@ IF(WITH_QT)
     ENDIF(NOT VTK_FOUND)
 
     # If Qt is here, the GUI will be built
-    # look for Qt5 (if vtk>5 is installed) before Qt4
+    IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
-    IF("${VTK_MAJOR_VERSION}" GREATER 5)
+	  IF(NOT VTK_QT_VERSION)
-        if(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
+	    MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.")
-            FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
+	  ENDIF()
-            IF(Qt5_FOUND)
+      MESSAGE(STATUS "VTK>=9 detected, will use VTK_QT_VERSION=${VTK_QT_VERSION} for Qt version.")
-                FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
+      IF(${VTK_QT_VERSION} EQUAL 6)
-            ENDIF(Qt5_FOUND)
+        FIND_PACKAGE(Qt6 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
+      ELSEIF(${VTK_QT_VERSION} EQUAL 5)
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
+      ELSE()
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
+      ENDIF()
+    ELSE()
+      # look for Qt5 (if vtk>5 is installed) before Qt4
+      IF("${VTK_MAJOR_VERSION}" GREATER 5)
+        IF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
+          FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
         ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
-    ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
+      ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
 
-    IF(NOT Qt5_FOUND)
+      IF(NOT Qt5_FOUND)
         IF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4")
-            FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
+          FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
         ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4")
-    ENDIF(NOT Qt5_FOUND)
+      ENDIF(NOT Qt5_FOUND)
+    ENDIF()
         
-    IF(QT4_FOUND OR Qt5_FOUND)
+    IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
         IF("${VTK_MAJOR_VERSION}" EQUAL 5)
             FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
         ELSE()
-		    list(FIND PCL_LIBRARIES VTK::GUISupportQt value)
+            list(FIND PCL_LIBRARIES VTK::GUISupportQt value)
-			IF(value EQUAL -1)
+            IF(value EQUAL -1)
                 list(FIND PCL_LIBRARIES vtkGUISupportQt value)
                 IF(value EQUAL -1)
-                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::GUISupportQt")
+                    ELSE()
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    ENDIF()
                     SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
                 ENDIF(value EQUAL -1)
-		    ENDIF(value EQUAL -1)
+            ENDIF(value EQUAL -1)
-		    
+
             MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}")
 
             IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
@@ -337,17 +358,17 @@ IF(WITH_QT)
                 ENDIF(value EQUAL -1)
             ELSEIF("${VTK_MAJOR_VERSION}" EQUAL 9)
                 list(FIND PCL_LIBRARIES VTK::RenderingOpenGL2 value)
-			    IF(NOT value EQUAL -1)
+                IF(NOT value EQUAL -1)
-			        list(FIND PCL_LIBRARIES VTK::RenderingVolumeOpenGL2 value)
+                    list(FIND PCL_LIBRARIES VTK::RenderingVolumeOpenGL2 value)
                     IF(value EQUAL -1)
                         SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::RenderingVolumeOpenGL2")
                     ENDIF(value EQUAL -1)
-			    ENDIF(NOT value EQUAL -1)
+                ENDIF(NOT value EQUAL -1)
             ENDIF()
 
         ENDIF()
         ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals/foreach and Qt macros
-    ENDIF(QT4_FOUND OR Qt5_FOUND)
+    ENDIF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
 ENDIF(WITH_QT)
 
 IF(NOT VTK_FOUND)
@@ -405,7 +426,7 @@ ENDIF(WITH_FREENECT2)
 
 IF(WITH_K4W2 AND WIN32)
     FIND_PACKAGE(KinectSDK2 QUIET)
-	IF(KinectSDK2_FOUND)
+    IF(KinectSDK2_FOUND)
        MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}")
     ENDIF(KinectSDK2_FOUND)
 ENDIF(WITH_K4W2 AND WIN32)
@@ -420,7 +441,7 @@ IF(WITH_K4A)
             SET(k4a_FOUND FALSE)
         ENDIF(NOT (k4a_FOUND AND k4arecord_FOUND))
     ENDIF()
-	IF(k4a_FOUND)
+    IF(k4a_FOUND)
        MESSAGE(STATUS "Found Kinect for Azure: ${k4a_INCLUDE_DIRS}")
     ENDIF(k4a_FOUND)
 ENDIF(WITH_K4A)
@@ -441,13 +462,13 @@ IF(WITH_DC1394)
 ENDIF(WITH_DC1394)
 
 IF(WITH_G2O)
-	  FIND_PACKAGE(g2o QUIET NO_MODULE)
+    FIND_PACKAGE(g2o NO_MODULE)
     IF(g2o_FOUND)
        MESSAGE(STATUS "Found g2o (targets)")
-	     SET(G2O_FOUND ${g2o_FOUND})
+         SET(G2O_FOUND ${g2o_FOUND})
-	     get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
+         get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
-	     MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
+         MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
-	     FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+         FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
          PATHS ${G2O_INCLUDES}
          NO_DEFAULT_PATH)
        FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
@@ -472,6 +493,14 @@ IF(WITH_GTSAM)
     FIND_PACKAGE(GTSAM CONFIG QUIET)
 ENDIF(WITH_GTSAM)
 
+IF(WITH_MRPT)
+    FIND_PACKAGE(MRPT COMPONENTS poses QUIET)
+    IF(MRPT_FOUND)
+        message(STATUS "MRPT_VERSION: ${MRPT_VERSION}")
+        message(STATUS "MRPT_LIBRARIES: ${MRPT_LIBRARIES}")
+    ENDIF(MRPT_FOUND)
+ENDIF(WITH_MRPT)
+
 IF(WITH_FLYCAPTURE2)
     FIND_PACKAGE(FlyCapture2 QUIET)
     IF(FlyCapture2_FOUND)
@@ -499,8 +528,8 @@ IF(libpointmatcher_FOUND OR GTSAM_FOUND)
         find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED)
     ENDIF(Boost_MINOR_VERSION GREATER 47)
     IF(WIN32)
-	    MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
+        MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
-	    link_directories(${Boost_LIBRARY_DIRS})
+        link_directories(${Boost_LIBRARY_DIRS})
     ENDIF(WIN32)
 ENDIF(libpointmatcher_FOUND OR GTSAM_FOUND)
 
@@ -737,51 +766,62 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
 IF(NOT MSVC)
-	IF((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND))
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1))
-	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
+      # Qt6 requires c++17
-	 include(CheckCXXCompilerFlag)
+      include(CheckCXXCompilerFlag)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+      CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
-	  IF(COMPILER_SUPPORTS_CXX14)
+      IF(COMPILER_SUPPORTS_CXX17)
-	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
-	    set(CMAKE_CXX_STANDARD 14)
+        set(CMAKE_CXX_STANDARD 17)
-	  ELSE()
+      ELSE()
-	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
+        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()
-	ENDIF()
+    ENDIF()
+    IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND ((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)))
+     #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
+     include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+      IF(COMPILER_SUPPORTS_CXX14)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+        set(CMAKE_CXX_STANDARD 14)
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
+      ENDIF()
+    ENDIF()
 
-	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
+    IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "17") AND NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
-	       G2O_FOUND OR
+           G2O_FOUND OR
-	       GTSAM_FOUND OR
+           GTSAM_FOUND OR
-	       CERES_FOUND OR
+           CERES_FOUND OR
-	       ZED_FOUND OR
+           ZED_FOUND OR
-	       ZEDOC_FOUND OR
+           ZEDOC_FOUND OR
-	       ANDROID OR
+           ANDROID OR
-	       RealSense_FOUND OR
+           RealSense_FOUND OR
-	       realsense2_FOUND OR
+           realsense2_FOUND OR
-	       ORB_SLAM_FOUND OR
+           ORB_SLAM_FOUND OR
-	       okvis_FOUND OR
+           okvis_FOUND OR
-	       open_chisel_FOUND OR
+           open_chisel_FOUND OR
-	       msckf_vio_FOUND OR
+           msckf_vio_FOUND OR
-	       vins_FOUND OR
+           vins_FOUND OR
-	       ov_msckf_FOUND OR
+           ov_msckf_FOUND OR
-	       libpointmatcher_FOUND))
+           libpointmatcher_FOUND))
-	 #Newest versions require std11
+     #Newest versions require std11
-	 include(CheckCXXCompilerFlag)
+     include(CheckCXXCompilerFlag)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-	  IF(COMPILER_SUPPORTS_CXX11)
+      IF(COMPILER_SUPPORTS_CXX11)
-	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-	  ELSEIF(COMPILER_SUPPORTS_CXX0X)
+      ELSEIF(COMPILER_SUPPORTS_CXX0X)
-	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-	  ELSE()
+      ELSE()
-	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-	  ENDIF()
+      ENDIF()
-	ENDIF()
+    ENDIF()
 ENDIF()
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
-  IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
+  IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
        
     # Required when packaging, and set CMAKE_INSTALL_PREFIX to "/".
     SET(CMAKE_INSTALL_PREFIX "/")
@@ -809,11 +849,6 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 
 ####### SOURCES (Projects) #######
 
-# CONF_DEPENDENCIES contains only dependencies not required by the headers
-SET(CONF_DEPENDENCIES 
-    ${ZLIB_LIBRARIES}
-)
-
 # OpenCV2 has nonfree if OPENCV_NONFREE_FOUND
 # OpenCV<=3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND
 # OpenCV>3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND and OPENCV_ENABLE_NONFREE is defined
@@ -834,16 +869,16 @@ IF(NOT G2O_FOUND)
    SET(G2O "//")
    SET(G2O_CPP_CONF "//")
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
    IF(NOT G2O_CPP11)
-   SET(G2O_CPP_CONF "//")
+     SET(G2O_CPP_CONF "//")
    ENDIF(NOT G2O_CPP11)
 ENDIF()
 IF(NOT GTSAM_FOUND)
    SET(GTSAM "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
 ENDIF()
+IF(NOT MRPT_FOUND)
+   SET(MRPT "//")
+ENDIF(NOT MRPT_FOUND)
 IF(NOT CERES_FOUND)
    SET(CERES "//")
 ENDIF(NOT CERES_FOUND)
@@ -855,8 +890,6 @@ IF(NOT WITH_VERTIGO)
 ENDIF(NOT WITH_VERTIGO)
 IF(NOT cvsba_FOUND)
    SET(CVSBA "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES})
 ENDIF()
 IF(NOT libpointmatcher_FOUND)
    SET(POINTMATCHER "//")
@@ -884,129 +917,109 @@ IF(NOT floam_FOUND)
 ENDIF(NOT floam_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${Freenect_LIBRARIES})
 ENDIF()
 IF(NOT freenect2_FOUND)
    SET(FREENECT2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
 ENDIF()
 IF(NOT KinectSDK2_FOUND)
    SET(K4W2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES})
 ENDIF()
 IF(NOT k4a_FOUND)
    SET(K4A "//")
+   SET(CONF_WITH_K4A 0)
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${k4a_LIBRARIES})
+   SET(CONF_WITH_K4A 1)
+   IF(WIN32)
+      install(
+         FILES
+         "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/FindK4A.cmake"
+         DESTINATION ${INSTALL_CMAKE_DIR}/Modules/.
+         COMPONENT devel
+      )
+   ENDIF(WIN32)
 ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OpenNI2_LIBRARIES})
 ENDIF()
 IF(NOT DC1394_FOUND)
    SET(DC1394 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${DC1394_LIBRARIES})
 ENDIF()
 IF(NOT FlyCapture2_FOUND)
    SET(FLYCAPTURE2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${FlyCapture2_LIBRARIES})
 ENDIF()
 IF(NOT ZED_FOUND)
    SET(ZED "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
 ENDIF()
 IF(NOT ZEDOC_FOUND)
    SET(ZEDOC "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZEDOC_LIBRARIES}) 
 ENDIF()
 IF(NOT RealSense_FOUND)
    SET(REALSENSE "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) 
 ENDIF()
 IF(NOT RealSenseSlam_FOUND)
    SET(REALSENSESLAM "//")
 ENDIF(NOT RealSenseSlam_FOUND)
 IF(NOT realsense2_FOUND)
    SET(REALSENSE2 "//")
+   SET(CONF_WITH_REALSENSE2 0)
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) 
+   SET(CONF_WITH_REALSENSE2 1)
+   IF(WIN32)
+      install(
+         FILES
+         "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/FindRealSense2.cmake"
+         DESTINATION ${INSTALL_CMAKE_DIR}/Modules/.
+         COMPONENT devel
+      )
+   ENDIF(WIN32)
 ENDIF()
 IF(NOT mynteye_FOUND)
    SET(MYNTEYE "//")
 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)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} depthai::core depthai::opencv) 
 ENDIF()
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
+   SET(CONF_WITH_OCTOMAP 0)
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
+   SET(CONF_WITH_OCTOMAP 1)
 ENDIF()
 IF(NOT CPUTSDF_FOUND)
    SET(CPUTSDF "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) 
 ENDIF()
 IF(NOT open_chisel_FOUND)
    SET(OPENCHISEL "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) 
 ENDIF()
 IF(NOT AliceVision_FOUND)
    SET(ALICE_VISION "//")
 ENDIF(NOT AliceVision_FOUND)
 IF(NOT libfovis_FOUND)
    SET(FOVIS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_LIBRARIES}) 
 ENDIF()
 IF(NOT libviso2_FOUND)
    SET(VISO2 "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES}) 
 ENDIF()
 IF(NOT dvo_core_FOUND)
    SET(DVO "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) 
 ENDIF()
 IF(NOT okvis_FOUND)
    SET(OKVIS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) 
 ENDIF()
 IF(NOT msckf_vio_FOUND)
    SET(MSCKF_VIO "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${msckf_vio_LIBRARIES}) 
 ENDIF()
 IF(NOT vins_FOUND)
    SET(VINS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
 ENDIF()
 IF(NOT ov_msckf_FOUND)
    SET(OPENVINS "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ov_msckf_LIBRARIES}) 
 ENDIF()
 IF(NOT ORB_SLAM_FOUND)
    SET(ORB_SLAM "//")
-ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM_LIBRARIES})
 ENDIF()
 IF(NOT WITH_ORB_OCTREE)
    SET(ORB_OCTREE "//")
@@ -1019,18 +1032,14 @@ IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
 ELSE()
    SET(CONF_VTK_QT false)
 ENDIF()
-IF(VTK_USE_QVTK)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) 
-ENDIF(VTK_USE_QVTK)
 IF(NOT WITH_MADGWICK)
    SET(MADGWICK "//")
 ENDIF()
 
-CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
+CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/src/include/${PROJECT_PREFIX}/core/Version.h)
 
 ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
@@ -1039,7 +1048,7 @@ IF(ANDROID)
    IF(BUILD_APP)
       ADD_SUBDIRECTORY( app )
    ENDIF(BUILD_APP)
-ELSEIF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
+ELSEIF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
    ADD_SUBDIRECTORY( guilib )
    IF(BUILD_APP)
       ADD_SUBDIRECTORY( app )
@@ -1067,55 +1076,84 @@ ADD_CUSTOM_TARGET(uninstall
   "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
 
 ####
-# Setup RTABMapConfig.cmake
+# Global Export Target
 ####
-# Create the RTABMapConfig.cmake and RTABMapConfigVersion files
+add_library(rtabmap INTERFACE)
-file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR}")
+add_library(rtabmap::rtabmap ALIAS rtabmap)
-file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
-   
-# ... for the build tree
-set(CONF_INCLUDE_DIRS "${PROJECT_BINARY_DIR}/corelib/include"
-                      "${PROJECT_SOURCE_DIR}/corelib/include" 
-                      "${PROJECT_SOURCE_DIR}/guilib/include" 
-                      "${PROJECT_SOURCE_DIR}/utilite/include")
-set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
-IF(QT4_FOUND OR Qt5_FOUND)
    set(CONF_WITH_GUI ON)
    IF(QT4_FOUND)
       set(CONF_QT_VERSION 4)
-   ELSE()
+   ELSEIF(Qt5_FOUND)
       set(CONF_QT_VERSION 5)
+   ELSE()
+      set(CONF_QT_VERSION 6)
    ENDIF()
+   target_link_libraries(rtabmap INTERFACE rtabmap_utilite rtabmap_core rtabmap_gui)
 ELSE()
    set(CONF_WITH_GUI OFF)
+   target_link_libraries(rtabmap INTERFACE rtabmap_utilite rtabmap_core)
 ENDIF()
-configure_file(RTABMapConfig.cmake.in
+install(TARGETS rtabmap EXPORT rtabmapTargets)
-  "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY)
+export(EXPORT rtabmapTargets
-  
+  FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake"
-# ... for the install tree
+  NAMESPACE rtabmap::
-set(CONF_INCLUDE_DIRS "\${RTABMap_CMAKE_DIR}/${REL_INCLUDE_DIR}")
+)
-set(CONF_LIB_DIR "\${RTABMap_CMAKE_DIR}/${REL_LIB_DIR}")
+install(EXPORT rtabmapTargets
-configure_file(RTABMapConfig.cmake.in
+  FILE
-  "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake" @ONLY)
+    ${PROJECT_NAME}Targets.cmake
-  
+  DESTINATION
-# ... for both
+    ${INSTALL_CMAKE_DIR}
-configure_file(RTABMapConfigVersion.cmake.in
+  NAMESPACE rtabmap::
-  "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake" @ONLY)
+  COMPONENT
+    devel
+)
 
-# Install the RTABMapConfig.cmake and RTABMapConfigVersion.cmake
+####
-install(FILES
+# Setup RTABMapConfig.cmake
-  "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake"
+####
-  "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake"
+include(CMakePackageConfigHelpers)
-  DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT devel)
+write_basic_package_version_file(
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+  VERSION ${PROJECT_VERSION}
+  COMPATIBILITY AnyNewerVersion
+)
+
+# Build tree:
+SET(CONF_MODULES_DIR "../cmake_modules")
+configure_file(
+  ${PROJECT_NAME}Config.cmake.in
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake"
+  @ONLY
+)
+
+# Install tree:
+SET(CONF_MODULES_DIR "Modules")
+configure_file(
+  ${PROJECT_NAME}Config.cmake.in
+  "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake"
+  @ONLY
+)
+install(
+  FILES
+    "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake"
+    "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+  DESTINATION
+    ${INSTALL_CMAKE_DIR}
+  COMPONENT
+    devel
+)
+  
 ####
 
 ### Install package.xml for catkin
-install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PREFIX}")
+install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PREFIX}" COMPONENT devel)
 
 #######################
 # CPACK (Packaging)
 #######################
 IF(BUILD_AS_BUNDLE)
+  SET(CMAKE_INSTALL_SYSTEM_RUNTIME_COMPONENT runtime)
   INCLUDE(InstallRequiredSystemLibraries)
 ENDIF(BUILD_AS_BUNDLE)
 
@@ -1132,6 +1170,7 @@ SET(CPACK_PACKAGE_VERSION_PATCH "${PROJECT_VERSION_PATCH}")
 #SET(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}")
 SET(CPACK_PACKAGE_CONTACT "matlabbe@gmail.com")
 
+
 set(CPACK_SOURCE_IGNORE_FILES 
   "\\\\.svn/" 
   "\\\\.settings/" 
@@ -1259,6 +1298,9 @@ MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}
 ELSEIF(Qt5_FOUND)
 MESSAGE(STATUS "  With Qt ${Qt5_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
+ELSEIF(Qt6_FOUND)
+MESSAGE(STATUS "  With Qt ${Qt6_VERSION}            = YES (License: Open Source or Commercial)")
+MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
 
 ELSEIF(NOT WITH_QT)
 MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
@@ -1309,7 +1351,7 @@ MESSAGE(STATUS "  With FastCV               = NO (FastCV not found)")
 ENDIF()
 
 IF(PDAL_FOUND)
-MESSAGE(STATUS "  With PDAL                 = YES (License: BSD)")
+MESSAGE(STATUS "  With PDAL ${PDAL_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_PDAL)
 MESSAGE(STATUS "  With PDAL                 = NO (WITH_PDAL=OFF)")
 ELSE()
@@ -1325,7 +1367,7 @@ MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
 ENDIF()
 
 IF(G2O_FOUND)
-MESSAGE(STATUS " *With g2o                  = YES (License: BSD)")
+MESSAGE(STATUS " *With g2o ${g2o_VERSION}            = YES (License: BSD)")
 ELSEIF(NOT WITH_G2O)
 MESSAGE(STATUS " *With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
@@ -1333,7 +1375,7 @@ MESSAGE(STATUS " *With g2o                  = NO (g2o not found)")
 ENDIF()
 
 IF(GTSAM_FOUND)
-MESSAGE(STATUS " *With GTSAM                = YES (License: BSD)")
+MESSAGE(STATUS " *With GTSAM ${GTSAM_VERSION}          = YES (License: BSD)")
 ELSEIF(NOT WITH_GTSAM)
 MESSAGE(STATUS " *With GTSAM                = NO (WITH_GTSAM=OFF)")
 ELSE()
@@ -1342,9 +1384,9 @@ ENDIF()
 
 IF(CERES_FOUND)
 IF(WITH_CERES)
-MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}          = YES (License: BSD)")
+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)")
+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)")
@@ -1352,6 +1394,14 @@ ELSE()
 MESSAGE(STATUS " *With Ceres                = NO (Ceres not found)")
 ENDIF()
 
+IF(MRPT_FOUND)
+MESSAGE(STATUS "  With MRPT ${MRPT_VERSION}           = YES (License: BSD)")
+ELSEIF(NOT WITH_MRPT)
+MESSAGE(STATUS "  With MRPT                 = NO (WITH_MRPT=OFF)")
+ELSE()
+MESSAGE(STATUS "  With MRPT                 = NO (MRPT not found)")
+ENDIF()
+
 IF(G2O_FOUND OR GTSAM_FOUND)
 IF(WITH_VERTIGO)
 MESSAGE(STATUS "  With VERTIGO              = YES (License: GPLv3)")
@@ -1371,7 +1421,7 @@ MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()
 
 IF(libpointmatcher_FOUND)
-MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
+MESSAGE(STATUS " *With libpointmatcher ${libpointmatcher_VERSION} = YES (License: BSD)")
 ELSEIF(NOT WITH_POINTMATCHER)
 MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
 ELSE()
@@ -1397,7 +1447,7 @@ MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()
 
 IF(opengv_FOUND)
-MESSAGE(STATUS "  With OpenGV               = YES (License: BSD)")
+MESSAGE(STATUS "  With OpenGV ${opengv_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_OPENGV)
 MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")
 ELSE()
@@ -1407,7 +1457,7 @@ ENDIF()
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)
-MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+MESSAGE(STATUS "  With OCTOMAP ${octomap_VERSION}        = YES (License: BSD)")
 ELSEIF(NOT WITH_OCTOMAP)
 MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
 ELSE()
@@ -1528,7 +1578,11 @@ MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
 ENDIF()
 
 IF(realsense2_FOUND)
+IF(WIN32)
 MESSAGE(STATUS "  With RealSense2           = YES (License: Apache-2)")
+ELSE()
+MESSAGE(STATUS "  With RealSense2 ${realsense2_VERSION}    = YES (License: Apache-2)")
+ENDIF()
 ELSEIF(NOT WITH_REALSENSE2)
 MESSAGE(STATUS "  With RealSense2           = NO (WITH_REALSENSE2=OFF)")
 ELSE()
@@ -1544,7 +1598,7 @@ MESSAGE(STATUS "  With MyntEyeS             = NO (mynteye s sdk not found)")
 ENDIF()
 
 IF(depthai_FOUND)
-MESSAGE(STATUS "  With DepthAI              = YES (License: MIT)")
+MESSAGE(STATUS "  With DepthAI ${depthai_VERSION}       = YES (License: MIT)")
 ELSEIF(NOT WITH_DEPTHAI)
 MESSAGE(STATUS "  With DepthAI              = NO (WITH_DEPTHAI=OFF)")
 ELSE()

README.md

@@ -4,14 +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]
 
- * Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) 
+[release-image]: https://img.shields.io/badge/release-0.21.0-green.svg?style=flat
- * Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
-
-[release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
+[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
 
@@ -28,3 +29,58 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
 <a href="https://introlab.3it.usherbrooke.ca/">
 <img src="https://github.com/introlab/16SoundsUSB/blob/master/images/IntRoLab.png" alt="IntRoLab" height="100">
 </a>
+
+#### CI Latest
+
+  <table>
+    <tbody>
+        <tr>
+           <td>Linux</td>
+           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
+           </td>
+        </tr>
+        <tr>
+           <td>Windows</td>
+           <td><a href="https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true" alt="Build Status"/>
+           </td>
+        </tr>
+     </tbody>
+  </table>
+ 
+ #### ROS Binaries
+ 
+ `ros-$ROS_DISTRO-rtabmap`
+ 
+ <table>
+    <tbody>
+        <tr>
+           <td rowspan="2">ROS 1</td>
+           <td>Melodic</td>
+            <td><a href="http://build.ros.org/job/Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Mbin_ubv8_uBv8__rtabmap__ubuntu_bionic_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Noetic</td>
+            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td rowspan="3">ROS 2</td>
+            <td>Foxy</td>
+            <td><a href="http://build.ros2.org/job/Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Humble</td>
+            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Rolling</td>
+            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <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

@@ -1,99 +1,95 @@
-# - Config file for the RTABMap package
+include(CMakeFindDependencyMacro)
-# Components:
-#  core    (required)
-#  gui     (optional)
-#  utilite (required)
-# It defines the following variables
-#  RTABMap_INCLUDE_DIRS - include directories for RTABMap
-#  RTABMap_LIBRARIES    - libraries to link against
-#  RTABMap_CORE         - core library
-#  RTABMap_UTILITE      - utilite library
-#  RTABMap_GUI          - gui library (set if RTABMap is built with Qt)
 
-# Compute paths
+# Mandatory dependencies
-get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
-set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
 
-#core lib
+if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+    
-
+    if(@CONF_QT_VERSION@ EQUAL 6)
-IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
+        find_dependency(Qt6 COMPONENTS Widgets Core Gui OpenGL)
-   SET(RTABMap_CORE
+    elseif(@CONF_QT_VERSION@ EQUAL 5)
-      debug ${RTABMap_CORE_DEBUG}
+        find_dependency(Qt5 COMPONENTS Widgets Core Gui OpenGL)
-      optimized ${RTABMap_CORE_RELEASE}
+    else() # Qt4
-   )
+        find_dependency(Qt4 COMPONENTS QtCore QtGui)
-ELSEIF(RTABMap_CORE_DEBUG)
+    endif()
-   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+    set(RTABMap_QT_VERSION @CONF_QT_VERSION@)
 ELSE()
-   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation)
+ENDIF()
+set(RTABMap_DEFINITIONS ${PCL_DEFINITIONS})
+add_definitions(${RTABMap_DEFINITIONS}) # To include -march=native if set
+
+# Optional dependencies
+IF(EXISTS "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
+    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
 ENDIF()
 
-#utilite lib
+IF(@CONF_WITH_REALSENSE2@)
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+    IF(WIN32)
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+        find_dependency(RealSense2)
-
+    ELSE()
-IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
+        find_dependency(realsense2)
-   SET(RTABMap_UTILITE
+    ENDIF()
-      debug ${RTABMap_UTILITE_DEBUG}
-      optimized ${RTABMap_UTILITE_RELEASE}
-   )
-ELSEIF(RTABMap_UTILITE_DEBUG)
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
-ELSE()
-   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
 ENDIF()
 
-set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
+IF(@CONF_WITH_K4A@)
+    IF(WIN32)
+        find_dependency(K4A)
+    ELSE()
+        find_dependency(k4a)
+        find_dependency(k4arecord)
+    ENDIF()
+ENDIF()
 
-list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
+IF(@CONF_WITH_DEPTH_AI@)
-set(WITH_GUI ON)
+ find_dependency(depthai 2)
-if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+ENDIF()
-   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
-   if (${_index} EQUAL -1)
-      set(WITH_GUI OFF)
-   endif()
-endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
 
+IF(@CONF_WITH_OCTOMAP@)
+  find_dependency(octomap)
+ENDIF()
 
-#gui lib (OFF if RTAB-Map is not built with Qt)
+# Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies)
-if(@CONF_WITH_GUI@ AND ${WITH_GUI})
+set(RTABMap_INCLUDE_DIRS "")
-   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+set(RTABMap_LIBRARIES "")
-   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+set(RTABMap_TARGETS "")
-   
-   IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
-      SET(RTABMap_GUI
-         debug ${RTABMap_GUI_DEBUG}
-         optimized ${RTABMap_GUI_RELEASE}
-      )
-   ELSEIF(RTABMap_GUI_RELEASE)
-      SET(RTABMap_GUI ${RTABMap_GUI_RELEASE})
-   ELSEIF(RTABMap_GUI_DEBUG)
-      SET(RTABMap_GUI ${RTABMap_GUI_DEBUG})
-   ENDIF()
-   
-   set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
-elseif(${WITH_GUI})
-   MESSAGE(WARNING "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
-endif()
 
-# Dependencies
+set(_RTABMap_supported_components utilite core gui)
-if(@CONF_VTK_QT@ AND ${WITH_GUI})
-   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
-endif(@CONF_VTK_QT@ AND ${WITH_GUI})
-if(@CONF_DEPTH_AI@)
-   FIND_PACKAGE(depthai 2 QUIET REQUIRED)
-endif(@CONF_DEPTH_AI@)
-SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
 
-#backward compatibilities
+foreach(_comp ${_RTABMap_supported_components})
-set(RTABMAP_CORE ${RTABMap_CORE})
+  if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
-set(RTABMAP_UTILITE ${RTABMap_UTILITE})
+    include("${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
-if(RTABMap_GUI)
+    set(RTABMap_${_comp}_FOUND True)
-   set(RTABMAP_GUI ${RTABMap_GUI})
+    set(RTABMap_TARGETS
-   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
+      ${RTABMap_TARGETS}
-endif(RTABMap_GUI)
+      rtabmap::${_comp})
+    get_target_property(RTABMap_${_comp}_INCLUDE_DIRS rtabmap::${_comp} INTERFACE_INCLUDE_DIRECTORIES)
+    get_target_property(RTABMap_${_comp}_LIBRARIES rtabmap::${_comp} INTERFACE_LINK_LIBRARIES)
+    set(RTABMap_INCLUDE_DIRS 
+      ${RTABMap_INCLUDE_DIRS}
+      ${RTABMap_${_comp}_INCLUDE_DIRS})
+    set(RTABMap_LIBRARIES 
+      ${RTABMap_LIBRARIES}
+      rtabmap::${_comp})
+    if(RTABMap_${_comp}_LIBRARIES)
+      set(RTABMap_LIBRARIES 
+        ${RTABMap_LIBRARIES}
+        ${RTABMap_${_comp}_LIBRARIES})
+    endif()
+  else()
+    set(RTABMap_${_comp}_FOUND False)
+  endif()
+endforeach()
 
-include(FindPackageHandleStandardArgs)
+include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")
-find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
+
-mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)
+foreach(_comp ${RTABMap_FIND_COMPONENTS})
+  if (NOT ";${_RTABMap_supported_components};" MATCHES ";${_comp};")
+    set(RTABMap_${_comp}_FOUND False)
+    if(${RTABMap_FIND_REQUIRED_${_comp}})
+      set(RTABMap_FOUND False)
+      set(RTABMap_NOT_FOUND_MESSAGE "Unsupported or not found required component: ${_comp}")
+    endif()
+  endif()
+endforeach()

RTABMapConfigVersion.cmake.in

@@ -1,11 +0,0 @@
-set(PACKAGE_VERSION "@RTABMAP_VERSION@")
-
-# Check whether the requested PACKAGE_FIND_VERSION is compatible
-if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
-  set(PACKAGE_VERSION_COMPATIBLE FALSE)
-else()
-  set(PACKAGE_VERSION_COMPATIBLE TRUE)
-  if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
-    set(PACKAGE_VERSION_EXACT TRUE)
-  endif()
-endif()

Version.h.in

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
 @GTSAM@#define RTABMAP_GTSAM
 @CERES@#define RTABMAP_CERES
+@MRPT@#define RTABMAP_MRPT
 @VERTIGO@#define RTABMAP_VERTIGO
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT

app/android/jni/CameraMobile.cpp

@@ -432,23 +432,25 @@ LaserScan CameraMobile::scanFromPointCloudData(
             //get color from rgb image
             cv::Point3f org= pt;
             pt = util3d::transformPoint(pt, opticalRotationInv);
-            int u,v;
+            if(pt.z > 0)
-            model.reproject(pt.x, pt.y, pt.z, u, v);
-            unsigned char r=255,g=255,b=255;
-            if(model.inFrame(u, v))
             {
-                b=rgb.at<cv::Vec3b>(v,u).val[0];
+                int u,v;
-                g=rgb.at<cv::Vec3b>(v,u).val[1];
+                model.reproject(pt.x, pt.y, pt.z, u, v);
-                r=rgb.at<cv::Vec3b>(v,u).val[2];
+                unsigned char r=255,g=255,b=255;
-                if(kpts)
+                if(model.inFrame(u, v))
-                    kpts->push_back(cv::KeyPoint(u,v,kptsSize));
+                {
-                if(kpts3D)
+                    b=rgb.at<cv::Vec3b>(v,u).val[0];
-                    kpts3D->push_back(org);
+                    g=rgb.at<cv::Vec3b>(v,u).val[1];
-                
+                    r=rgb.at<cv::Vec3b>(v,u).val[2];
-                *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+                    if(kpts)
-                ++oi;
+                        kpts->push_back(cv::KeyPoint(u,v,kptsSize));
+                    if(kpts3D)
+                        kpts3D->push_back(org);
+                    
+                    *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+                    ++oi;
+                }
             }
-
             //confidence
             //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
 

app/android/jni/RTABMapApp.cpp

@@ -2130,7 +2130,7 @@ int RTABMapApp::Render()
 				if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
 				{
 					UERROR("TangoPoseEventNotReceived");
-					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_)));
+					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
 				}
 			}
 		}

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -1007,7 +1007,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.19;
+				MARKETING_VERSION = 0.21.0;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1020,7 +1020,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
@@ -1064,7 +1064,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.19;
+				MARKETING_VERSION = 0.21.0;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -1078,7 +1078,7 @@
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"",
-					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"",
+					"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"",
 					"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
 					"\"$(SRCROOT)/../android/jni/third-party/include\"",
 					"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",

app/ios/RTABMapApp/.gitignore

@@ -0,0 +1 @@
+Libraries

app/ios/RTABMapApp/Libraries/.gitignore

@@ -1,6 +0,0 @@
-# Ignore everything
-*
-
-# But not these files...
-!.gitignore
-!install_deps.sh

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -489,13 +489,6 @@ void setGridVisibleNative(const void *object, bool visible)
     else
         UERROR("object is null!");
 }
-void setRawScanSavedNative(const void *object, bool enabled)
-{
-    if(object)
-        native(object)->setRawScanSaved(enabled);
-    else
-        UERROR("object is null!");
-}
 void setFullResolutionNative(const void *object, bool enabled)
 {
     if(object)

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -97,7 +97,6 @@ void setGraphOptimizationNative(const void *object, bool enabled);
 void setNodesFilteringNative(const void *object, bool enabled);
 void setGraphVisibleNative(const void *object, bool visible);
 void setGridVisibleNative(const void *object, bool visible);
-void setRawScanSavedNative(const void *object, bool enabled);
 void setFullResolutionNative(const void *object, bool enabled);
 void setSmoothingNative(const void *object, bool enabled);
 void setAppendModeNative(const void *object, bool enabled);

app/ios/RTABMapApp/RTABMap.swift

@@ -405,9 +405,6 @@ class RTABMap {
     func setGridVisible(visible: Bool) {
         setGridVisibleNative(native_rtabmap, visible)
     }
-    func setRawScanSaved(enabled: Bool) {
-        setRawScanSavedNative(native_rtabmap, enabled)
-    }
     func setFullResolution(enabled: Bool) {
         setFullResolutionNative(native_rtabmap, enabled)
     }

app/ios/RTABMapApp/ViewController.swift

@@ -1319,7 +1319,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         // update preference
         rtabmap!.setOnlineBlending(enabled: defaults.bool(forKey: "Blending"));
         rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering"));
-        rtabmap!.setRawScanSaved(enabled: defaults.bool(forKey: "SaveRawScan"));
         rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode"));
         rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing"));
         rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode"));

app/ios/RTABMapApp/install_deps.sh

@@ -1,7 +1,11 @@
 #!/bin/bash
 
+set -euxo pipefail
+
 # Tested on Apple Silicon Mac, with cmake 3.19.2.
 
+mkdir Libraries
+cd Libraries
 pwd=$(pwd)
 prefix=$pwd
 sysroot=iphoneos
@@ -107,7 +111,7 @@ git checkout tags/v8.2.0
 git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
 mkdir build
 cd build
-cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DIOS_SIMULATOR_ARCHITECTURES=arm64 -DModule_vtkFiltersModeling=ON ..
+cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
 # For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES=""
 cmake --build . --config Release
 cd $pwd

app/ios/Settings.bundle/License.plist

@@ -13,7 +13,7 @@
 			<string>
 ======= RTAB-Map =======
 RTAB-Map - https://github.com/introlab/rtabmap
-Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
 Copyright (c) XXX, contributors, all rights reserved.
 
 Redistribution and use in source and binary forms, with or without

app/ios/Settings.bundle/Mapping.plist

@@ -552,50 +552,50 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
-        <dict>
+		<dict>
-            <key>Type</key>
+			<key>Type</key>
-            <string>PSGroupSpecifier</string>
+			<string>PSGroupSpecifier</string>
-            <key>FooterText</key>
+			<key>FooterText</key>
-            <string>Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don't teleport to a location very similar to one we previously localized on.</string>
+			<string>Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don&apos;t teleport to a location very similar to one we previously localized on.</string>
-        </dict>
+		</dict>
-        <dict>
+		<dict>
-            <key>Type</key>
+			<key>Type</key>
-            <string>PSMultiValueSpecifier</string>
+			<string>PSMultiValueSpecifier</string>
-            <key>Title</key>
+			<key>Title</key>
-            <string>Maximum odometry cache size</string>
+			<string>Maximum odometry cache size</string>
-            <key>Key</key>
+			<key>Key</key>
-            <string>MaximumOdometryCacheSize</string>
+			<string>MaximumOdometryCacheSize</string>
-            <key>DefaultValue</key>
+			<key>DefaultValue</key>
-            <string>10</string>
+			<string>10</string>
-            <key>Titles</key>
+			<key>Titles</key>
-            <array>
+			<array>
-                <string>500</string>
+				<string>500</string>
-                <string>200</string>
+				<string>200</string>
-                <string>100</string>
+				<string>100</string>
-                <string>75</string>
+				<string>75</string>
-                <string>50</string>
+				<string>50</string>
-                <string>40</string>
+				<string>40</string>
-                <string>30</string>
+				<string>30</string>
-                <string>20</string>
+				<string>20</string>
-                <string>10</string>
+				<string>10</string>
-                <string>5</string>
+				<string>5</string>
-                <string>Disabled</string>
+				<string>Disabled</string>
-            </array>
+			</array>
-            <key>Values</key>
+			<key>Values</key>
-            <array>
+			<array>
-                <string>500</string>
+				<string>500</string>
-                <string>200</string>
+				<string>200</string>
-                <string>100</string>
+				<string>100</string>
-                <string>75</string>
+				<string>75</string>
-                <string>50</string>
+				<string>50</string>
-                <string>40</string>
+				<string>40</string>
-                <string>30</string>
+				<string>30</string>
-                <string>20</string>
+				<string>20</string>
-                <string>10</string>
+				<string>10</string>
-                <string>5</string>
+				<string>5</string>
-                <string>0</string>
+				<string>0</string>
-            </array>
+			</array>
-        </dict>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -752,22 +752,6 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
-		<dict>
-			<key>Type</key>
-			<string>PSGroupSpecifier</string>
-			<key>FooterText</key>
-			<string>Save raw point clouds in database.</string>
-		</dict>
-		<dict>
-			<key>Type</key>
-			<string>PSToggleSwitchSpecifier</string>
-			<key>Title</key>
-			<string>Save Raw Scan</string>
-			<key>Key</key>
-			<string>SaveRawScan</string>
-			<key>DefaultValue</key>
-			<true/>
-		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>

app/ios/Settings.bundle/Root.plist

@@ -454,7 +454,7 @@
 		</dict>
 		<dict>
 			<key>FooterText</key>
-			<string>Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
+			<string>Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
 			<key>Title</key>
 			<string>About</string>
 			<key>Type</key>
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.19</string>
+			<string>0.21.0</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

app/src/CMakeLists.txt

@@ -3,32 +3,6 @@ SET(SRC_FILES
     main.cpp
 )
 
-SET(INCLUDE_DIRS
-    ${PROJECT_BINARY_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/utilite/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-	${PROJECT_SOURCE_DIR}/guilib/include
-    ${CMAKE_CURRENT_SOURCE_DIR}
-    ${OpenCV_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-)
-
-IF(QT4_FOUND)
-    INCLUDE(${QT_USE_FILE})
-ENDIF(QT4_FOUND)
-
-SET(LIBRARIES
-    ${QT_LIBRARIES} 
-    ${OpenCV_LIBS}
-    ${PCL_LIBRARIES}
-)
-
-# rc.exe has problems with these defintions... commented!
-#add_definitions(${PCL_DEFINITIONS})
-
-# Make sure the compiler can find include files from our library.
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
 # For Apple set the icns file containing icons
 IF(APPLE AND BUILD_AS_BUNDLE)
   # set how it shows up in the Info.plist file
@@ -50,40 +24,37 @@ IF(WIN32)
   ELSE( MINGW )
     SET(SRC_FILES ${SRC_FILES} ${PROJECT_NAME}.rc)
   ENDIF( MINGW )
-ENDIF(WIN32)
+ENDIF(WIN32)
+
+IF(BUILD_AS_BUNDLE)
+  ADD_DEFINITIONS("-DBUILD_AS_BUNDLE")
+ENDIF()
 
 # Add binary
 IF(APPLE AND BUILD_AS_BUNDLE)
-  ADD_EXECUTABLE(rtabmap MACOSX_BUNDLE ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES})
-ELSEIF(MINGW)
+ELSEIF(WIN32 AND BUILD_AS_BUNDLE)
-  ADD_EXECUTABLE(rtabmap WIN32 ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app WIN32 ${SRC_FILES})
 ELSE()
-  ADD_EXECUTABLE(rtabmap ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app ${SRC_FILES})
 ENDIF()
-TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+TARGET_LINK_LIBRARIES(rtabmap_app rtabmap_gui)
-IF(Qt5_FOUND)
-    IF(Qt5Svg_FOUND)
-        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
-    ELSE()
-        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
-    ENDIF()
-ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
     OUTPUT_NAME ${CMAKE_BUNDLE_NAME})
 ELSEIF(WIN32)
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
     OUTPUT_NAME ${PROJECT_NAME})
 ELSE()
-  SET_TARGET_PROPERTIES(rtabmap PROPERTIES
+  SET_TARGET_PROPERTIES(rtabmap_app PROPERTIES
     OUTPUT_NAME ${PROJECT_PREFIX})
 ENDIF()
 
 #---------------------------
 # Installation stuff
 #--------------------------- 
-INSTALL(TARGETS rtabmap
+INSTALL(TARGETS rtabmap_app
         RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
         BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
       
@@ -154,16 +125,32 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 	ENDIF(WIN32 AND CUDA_FOUND)
   ENDIF(Torch_FOUND)
 
-  # Install needed Qt plugins by copying directories from the qt installation
+  IF(Qt6_FOUND)
-  # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
+	# Reference: https://doc-snapshots.qt.io/qt6-6.4/qt-deploy-runtime-dependencies.html
-  # Exclude debug libraries
+	# The following script must only be executed at install time
-  IF(QT_PLUGINS_DIR)
+	set(deploy_script "${CMAKE_CURRENT_BINARY_DIR}/deploy_app$<CONFIG>.cmake")
-    INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
+	file(GENERATE OUTPUT ${deploy_script} CONTENT "
-          DESTINATION ${plugin_dest_dir}/plugins 
+	# Including the file pointed to by QT_DEPLOY_SUPPORT ensures the generated
-          COMPONENT runtime 
+	# deployment script has access to qt_deploy_runtime_dependencies()
-          REGEX ".*d4.dll" EXCLUDE
+	include(\"${QT_DEPLOY_SUPPORT}\")
-          REGEX ".*d4.a" EXCLUDE)
+	qt_deploy_runtime_dependencies(
-  ELSE()
+		EXECUTABLE \"${APPS}\"
+		VERBOSE
+		PLUGINS_DIR ${plugin_dest_dir}/plugins
+	)")
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appDebug.cmake"
+		CONFIGURATIONS Debug
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake"
+		CONFIGURATIONS Release
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake"
+		CONFIGURATIONS RelWithDebInfo
+		COMPONENT runtime)
+	install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake"
+		CONFIGURATIONS MinSizeRel
+		COMPONENT runtime)
+  ELSEIF(Qt5_FOUND)
 	#Qt5
 	foreach(plugin ${Qt5Gui_PLUGINS})
 		get_target_property(plugin_loc ${plugin} LOCATION)
@@ -175,8 +162,8 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 		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}
+		  DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
-          COMPONENT runtime)
+		  COMPONENT runtime)
 	endforeach()
 	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
 		IF(WIN32)
@@ -194,22 +181,31 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 			#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		ENDIF(EXISTS ${plugin_loc})
 	ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+  ELSEIF(QT_PLUGINS_DIR) # Qt4
+    # Install needed Qt plugins by copying directories from the qt installation
+	# One can cull what gets copied by using 'REGEX "..." EXCLUDE'
+	# Exclude debug libraries
+	INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
+		DESTINATION ${plugin_dest_dir}/plugins 
+		COMPONENT runtime 
+		REGEX ".*d4.dll" EXCLUDE
+		REGEX ".*d4.a" EXCLUDE)
   ENDIF()
-  
+ 
   # install a qt.conf file
   # this inserts some cmake code into the install script to write the file
   SET(QT_CONF_FILE [Paths]\nPlugins=plugins)
   IF(APPLE)
-    SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
+	SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins)
   ENDIF(APPLE)
   INSTALL(CODE "
-    file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
+	file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\")
-    " COMPONENT runtime)
+	" COMPONENT runtime)
-
+ 
   # directories to look for dependencies
-  SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_SOURCE_DIR}/bin)
+  SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_BINARY_DIR}/bin)
   IF(APPLE)
-    SET(DIRS ${DIRS} /usr/local /usr/local/lib)
+    SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current)
   ENDIF(APPLE)
 
   # Now the work of copying dependencies into the bundle/package

app/src/main.cpp

@@ -36,8 +36,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UConversion.h"
 
+#include <vtkVersionMacros.h>
 #include <vtkObject.h>
 
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+#include <QVTKRenderWidget.h>
+#endif
+
 using namespace rtabmap;
 
 int main(int argc, char* argv[])
@@ -50,9 +55,13 @@ int main(int argc, char* argv[])
 	CoInitialize(nullptr);
 #endif
 
-#if VTK_MAJOR_VERSION >= 8
+#if VTK_MAJOR_VERSION >= 8 && defined(BUILD_AS_BUNDLE)
 	vtkObject::GlobalWarningDisplayOff();
 #endif
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+    // needed to ensure appropriate OpenGL context is created for VTK rendering.
+    QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());
+#endif
 
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);

build/.gitignore

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

cmake_modules/FindEigen3.cmake

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

cmake_modules/FindZEDOC.cmake

@@ -17,7 +17,7 @@ ENDIF (ZEDOC_INCLUDE_DIR AND ZEDOC_LIBRARY)
 
 IF (ZEDOC_FOUND)
    # show which ZEDOC was found only if not quiet
-   IF (NOT _FIND_QUIETLY)
+   IF (NOT ZEDOC_FIND_QUIETLY)
       MESSAGE(STATUS "Found ZEDOC: ${ZEDOC_LIBRARIES}")
    ENDIF (NOT ZEDOC_FIND_QUIETLY)
 ELSE (ZEDOC_FOUND)

corelib/include/rtabmap/core/BayesFilter.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef BAYESFILTER_H_
 #define BAYESFILTER_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <opencv2/core/core.hpp>
 #include <list>
@@ -41,7 +41,7 @@ namespace rtabmap {
 class Memory;
 class Signature;
 
-class RTABMAP_EXP BayesFilter
+class RTABMAP_CORE_EXPORT BayesFilter
 {
 public:
 	BayesFilter(const ParametersMap & parameters = ParametersMap());

corelib/include/rtabmap/core/Camera.h

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <opencv2/highgui/highgui.hpp>
 #include "rtabmap/core/SensorData.h"
@@ -47,7 +47,7 @@ namespace rtabmap
  * Class Camera
  *
  */
-class RTABMAP_EXP Camera
+class RTABMAP_CORE_EXPORT Camera
 {
 public:
 	virtual ~Camera();

corelib/include/rtabmap/core/CameraModel.h

@@ -30,12 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/opencv.hpp>
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/Transform.h"
 
 namespace rtabmap {
 
-class RTABMAP_EXP CameraModel
+class RTABMAP_CORE_EXPORT CameraModel
 {
 public:
 	/**
@@ -159,7 +159,7 @@ private:
 	Transform localTransform_;
 };
 
-RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const CameraModel& model);
+RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const CameraModel& model);
 
 } /* namespace rtabmap */
 #endif /* CAMERAMODEL_H_ */

corelib/include/rtabmap/core/CameraThread.h

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Transform.h>
@@ -52,7 +52,7 @@ class IMUFilter;
  * Class CameraThread
  *
  */
-class RTABMAP_EXP CameraThread :
+class RTABMAP_CORE_EXPORT CameraThread :
 	public UThread,
 	public UEventsSender
 {
@@ -88,7 +88,8 @@ public:
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
 	void disableIMUFiltering();
 
-	RTABMAP_DEPRECATED(void setScanParameters(
+	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
+	RTABMAP_DEPRECATED void setScanParameters(
 			bool fromDepth,
 			int downsampleStep, // decimation of the depth image in case the scan is from depth image
 			float rangeMin,
@@ -96,7 +97,7 @@ public:
 			float voxelSize,
 			int normalsK,
 			int normalsRadius,
-			bool forceGroundNormalsUp) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.");
+			bool forceGroundNormalsUp);
 	void setScanParameters(
 			bool fromDepth,
 			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image

corelib/include/rtabmap/core/Compression.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef COMPRESSION_H_
 #define COMPRESSION_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/utilite/UThread.h>
 #include <opencv2/opencv.hpp>
@@ -52,7 +52,7 @@ namespace rtabmap {
  *   ct.join();
  *   cv::Mat image = ct.getUncompressedData();
  */
-class RTABMAP_EXP CompressionThread : public UThread
+class RTABMAP_CORE_EXPORT CompressionThread : public UThread
 {
 public:
 	// format : ".png" ".jpg" "" (empty is general)
@@ -70,21 +70,21 @@ private:
 	bool compressMode_;
 };
 
-std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
+std::vector<unsigned char> RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png");
-cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
+cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");
 
-cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);
 
-std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
+std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(const cv::Mat & data);
-cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
+cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat & data);
 
-cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const std::vector<unsigned char> & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned long size);
 
-cv::Mat RTABMAP_EXP compressString(const std::string & str);
+cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str);
-std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
 
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

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

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef DBDRIVERSQLITE3_H_
 #define DBDRIVERSQLITE3_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
 
@@ -37,7 +37,7 @@ typedef struct sqlite3 sqlite3;
 
 namespace rtabmap {
 
-class RTABMAP_EXP DBDriverSqlite3: public DBDriver {
+class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 public:
 	DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
 	virtual ~DBDriverSqlite3();

corelib/include/rtabmap/core/DBReader.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef DBREADER_H_
 #define DBREADER_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/Transform.h>
@@ -43,7 +43,7 @@ namespace rtabmap {
 
 class DBDriver;
 
-class RTABMAP_EXP DBReader : public Camera {
+class RTABMAP_CORE_EXPORT DBReader : public Camera {
 public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -55,7 +55,10 @@ public:
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
-			 bool featuresIgnored = false);
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1,
+			 bool priorsIgnored = false);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
@@ -66,7 +69,10 @@ public:
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
 			 bool landmarksIgnored = false,
-			 bool featuresIgnored = false);
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1,
+			 bool priorsIgnored = false);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -77,6 +83,8 @@ public:
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const {return !_odometryIgnored;}
 
+	const DBDriver * driver() const {return _dbDriver;}
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
@@ -94,6 +102,9 @@ private:
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
+	bool _priorsIgnored;
+	int _startMapId;
+	int _stopMapId;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
@@ -42,7 +42,7 @@ namespace rtabmap
 
 class Signature;
 
-class RTABMAP_EXP EpipolarGeometry
+class RTABMAP_CORE_EXPORT EpipolarGeometry
 {
 public:
 	EpipolarGeometry(const ParametersMap & parameters = ParametersMap());

corelib/include/rtabmap/core/Features2d.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef FEATURES2D_H_
 #define FEATURES2D_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/core/core.hpp>
@@ -103,7 +103,7 @@ class CV_ORB;
 #endif
 
 // Feature2D
-class RTABMAP_EXP Feature2D {
+class RTABMAP_CORE_EXPORT Feature2D {
 public:
 	enum Type {kFeatureUndef=-1,
 		kFeatureSurf=0,
@@ -247,7 +247,7 @@ private:
 };
 
 //SURF
-class RTABMAP_EXP SURF : public Feature2D
+class RTABMAP_CORE_EXPORT SURF : public Feature2D
 {
 public:
 	SURF(const ParametersMap & parameters = ParametersMap());
@@ -274,7 +274,7 @@ private:
 };
 
 //SIFT
-class RTABMAP_EXP SIFT : public Feature2D
+class RTABMAP_CORE_EXPORT SIFT : public Feature2D
 {
 public:
 	SIFT(const ParametersMap & parameters = ParametersMap());
@@ -298,7 +298,7 @@ private:
 };
 
 //ORB
-class RTABMAP_EXP ORB : public Feature2D
+class RTABMAP_CORE_EXPORT ORB : public Feature2D
 {
 public:
 	ORB(const ParametersMap & parameters = ParametersMap());
@@ -329,7 +329,7 @@ private:
 };
 
 //FAST
-class RTABMAP_EXP FAST : public Feature2D
+class RTABMAP_CORE_EXPORT FAST : public Feature2D
 {
 public:
 	FAST(const ParametersMap & parameters = ParametersMap());
@@ -365,7 +365,7 @@ private:
 };
 
 //FAST_BRIEF
-class RTABMAP_EXP FAST_BRIEF : public FAST
+class RTABMAP_CORE_EXPORT FAST_BRIEF : public FAST
 {
 public:
 	FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
@@ -384,7 +384,7 @@ private:
 };
 
 //FAST_FREAK
-class RTABMAP_EXP FAST_FREAK : public FAST
+class RTABMAP_CORE_EXPORT FAST_FREAK : public FAST
 {
 public:
 	FAST_FREAK(const ParametersMap & parameters = ParametersMap());
@@ -406,7 +406,7 @@ private:
 };
 
 //GFTT
-class RTABMAP_EXP GFTT : public Feature2D
+class RTABMAP_CORE_EXPORT GFTT : public Feature2D
 {
 public:
 	GFTT(const ParametersMap & parameters = ParametersMap());
@@ -428,7 +428,7 @@ private:
 };
 
 //GFTT_BRIEF
-class RTABMAP_EXP GFTT_BRIEF : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_BRIEF : public GFTT
 {
 public:
 	GFTT_BRIEF(const ParametersMap & parameters = ParametersMap());
@@ -447,7 +447,7 @@ private:
 };
 
 //GFTT_FREAK
-class RTABMAP_EXP GFTT_FREAK : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_FREAK : public GFTT
 {
 public:
 	GFTT_FREAK(const ParametersMap & parameters = ParametersMap());
@@ -469,7 +469,7 @@ private:
 };
 
 //SURF_FREAK
-class RTABMAP_EXP SURF_FREAK : public SURF
+class RTABMAP_CORE_EXPORT SURF_FREAK : public SURF
 {
 public:
 	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
@@ -491,7 +491,7 @@ private:
 };
 
 //GFTT_ORB
-class RTABMAP_EXP GFTT_ORB : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_ORB : public GFTT
 {
 public:
 	GFTT_ORB(const ParametersMap & parameters = ParametersMap());
@@ -508,7 +508,7 @@ private:
 };
 
 //BRISK
-class RTABMAP_EXP BRISK : public Feature2D
+class RTABMAP_CORE_EXPORT BRISK : public Feature2D
 {
 public:
 	BRISK(const ParametersMap & parameters = ParametersMap());
@@ -530,7 +530,7 @@ private:
 };
 
 //KAZE
-class RTABMAP_EXP KAZE : public Feature2D
+class RTABMAP_CORE_EXPORT KAZE : public Feature2D
 {
 public:
 	KAZE(const ParametersMap & parameters = ParametersMap());
@@ -557,7 +557,7 @@ private:
 };
 
 //ORB OCTREE
-class RTABMAP_EXP ORBOctree : public Feature2D
+class RTABMAP_CORE_EXPORT ORBOctree : public Feature2D
 {
 public:
 	ORBOctree(const ParametersMap & parameters = ParametersMap());
@@ -583,7 +583,7 @@ private:
 };
 
 //SuperPointTorch
-class RTABMAP_EXP SuperPointTorch : public Feature2D
+class RTABMAP_CORE_EXPORT SuperPointTorch : public Feature2D
 {
 public:
 	SuperPointTorch(const ParametersMap & parameters = ParametersMap());
@@ -606,7 +606,7 @@ private:
 };
 
 //GFTT_DAISY
-class RTABMAP_EXP GFTT_DAISY : public GFTT
+class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT
 {
 public:
 	GFTT_DAISY(const ParametersMap & parameters = ParametersMap());
@@ -630,7 +630,7 @@ private:
 };
 
 //SURF_DAISY
-class RTABMAP_EXP SURF_DAISY : public SURF
+class RTABMAP_CORE_EXPORT SURF_DAISY : public SURF
 {
 public:
 	SURF_DAISY(const ParametersMap & parameters = ParametersMap());

corelib/include/rtabmap/core/FlannIndex.h

@@ -28,23 +28,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_SRC_FLANNINDEX_H_
 #define CORELIB_SRC_FLANNINDEX_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include <list>
 #include <opencv2/opencv.hpp>
 
 namespace rtabmap {
 
-class RTABMAP_EXP FlannIndex
+class RTABMAP_CORE_EXPORT FlannIndex
 {
 public:
 	FlannIndex();
 	virtual ~FlannIndex();
 
 	void release();
-	unsigned int indexedFeatures() const;
+	size_t indexedFeatures() const;
 
 	// return Bytes
-	unsigned long memoryUsed() const;
+	size_t memoryUsed() const;
 
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(

corelib/include/rtabmap/core/GainCompensator.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_SRC_GAINCOMPENSATOR_H_
 #define CORELIB_SRC_GAINCOMPENSATOR_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -41,7 +41,7 @@ namespace rtabmap {
 /**
  * Works like cv::GainCompensator but with point clouds
  */
-class RTABMAP_EXP GainCompensator {
+class RTABMAP_CORE_EXPORT GainCompensator {
 public:
 	GainCompensator(double maxCorrespondenceDistance = 0.02, double minOverlap = 0.0, double alpha = 0.01, double beta = 10);
 	virtual ~GainCompensator();

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -43,13 +43,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef GEODETICCOORDS_H_
 #define GEODETICCOORDS_H_
 
-#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/rtabmap_core_export.h>
 
 #include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
-class RTABMAP_EXP GeodeticCoords
+class RTABMAP_CORE_EXPORT GeodeticCoords
 {
 public:
 	GeodeticCoords();

corelib/include/rtabmap/core/Graph.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef GRAPH_H_
 #define GRAPH_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <map>
 #include <list>
@@ -46,7 +46,7 @@ namespace graph {
 // Graph utilities
 ////////////////////////////////////////////
 
-bool RTABMAP_EXP exportPoses(
+bool RTABMAP_CORE_EXPORT exportPoses(
 		const std::string & filePath,
 		int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame, 11=10+ID), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
 		const std::map<int, Transform> & poses,
@@ -54,14 +54,14 @@ bool RTABMAP_EXP exportPoses(
 		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
 		const ParametersMap & parameters = ParametersMap()); // optional for formats 3 and 4
 
-bool RTABMAP_EXP importPoses(
+bool RTABMAP_CORE_EXPORT importPoses(
 		const std::string & filePath,
 		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1 and 9
 
-bool RTABMAP_EXP exportGPS(
+bool RTABMAP_CORE_EXPORT exportGPS(
 		const std::string & filePath,
 		const std::map<int, GPS> & gpsValues,
 		unsigned int rgba = 0xFFFFFFFF);
@@ -74,7 +74,7 @@ bool RTABMAP_EXP exportGPS(
  * @param t_err, Output translation error (%)
  * @param r_err, Output rotation error (deg/m)
  */
-void RTABMAP_EXP calcKittiSequenceErrors(
+void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
 		const std::vector<Transform> &poses_gt,
 		const std::vector<Transform> &poses_result,
 		float & t_err,
@@ -87,7 +87,7 @@ void RTABMAP_EXP calcKittiSequenceErrors(
  * @param t_err, Output translation error (m)
  * @param r_err, Output rotation error (deg)
  */
-void RTABMAP_EXP calcRelativeErrors (
+void RTABMAP_CORE_EXPORT calcRelativeErrors (
 		const std::vector<Transform> &poses_gt,
 		const std::vector<Transform> &poses_result,
 		float & t_err,
@@ -101,7 +101,7 @@ void RTABMAP_EXP calcRelativeErrors (
  * @param poses, Estimated poses
  * @return Gt to Map transform
  */
-Transform RTABMAP_EXP calcRMSE(
+Transform RTABMAP_CORE_EXPORT calcRMSE(
 		const std::map<int, Transform> &groundTruth,
 		const std::map<int, Transform> &poses,
 		float & translational_rmse,
@@ -117,7 +117,7 @@ Transform RTABMAP_EXP calcRMSE(
 		float & rotational_min,
 		float & rotational_max);
 
-void RTABMAP_EXP computeMaxGraphErrors(
+void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
 		float & maxLinearErrorRatio,
@@ -128,53 +128,53 @@ void RTABMAP_EXP computeMaxGraphErrors(
 		const Link ** maxAngularErrorLink = 0,
 		bool for3DoF = false);
 
-std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links);
+std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
 
-std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
+std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, Link> & links,
 		int from,
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
-std::multimap<int, int>::iterator RTABMAP_EXP findLink(
+std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
 		std::multimap<int, int> & links,
 		int from,
 		int to,
 		bool checkBothWays = true);
-std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
+std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, Link> & links,
 		int from,
 		int to,
 		bool checkBothWays = true,
 		Link::Type type = Link::kUndef);
-std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
+std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
 		const std::multimap<int, int> & links,
 		int from,
 		int to,
 		bool checkBothWays = true);
-std::list<Link> RTABMAP_EXP findLinks(
+std::list<Link> RTABMAP_CORE_EXPORT findLinks(
 		const std::multimap<int, Link> & links,
 		int from);
 
-std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
+std::multimap<int, Link> RTABMAP_CORE_EXPORT filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
 /**
  * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
  */
-std::multimap<int, Link> RTABMAP_EXP filterLinks(
+std::multimap<int, Link> RTABMAP_CORE_EXPORT filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
 /**
  * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
  */
-std::map<int, Link> RTABMAP_EXP filterLinks(
+std::map<int, Link> RTABMAP_CORE_EXPORT filterLinks(
 		const std::map<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
 
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
-std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
+std::map<int, Transform> RTABMAP_CORE_EXPORT frustumPosesFiltering(
 		const std::map<int, Transform> & poses,
 		const Transform & cameraPose,
 		float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
@@ -191,7 +191,7 @@ std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
  * @param keepLatest keep the latest node if true, otherwise the oldest node is kept
  * @return A map containing only most recent or older poses in the the defined radius
  */
-std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
+std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle,
@@ -204,7 +204,7 @@ std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
  * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
  * @return A map between each pose id and its neighbors found in the radius
  */
-std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
+std::multimap<int, int> RTABMAP_CORE_EXPORT radiusPosesClustering(
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle);
@@ -224,7 +224,7 @@ void reduceGraph(
  * @param updateNewCosts Keep up-to-date costs while traversing the graph.
  * @return the path ids from id "from" to id "to" including initial and final nodes.
  */
-std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
+std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			const std::map<int, rtabmap::Transform> & poses,
 			const std::multimap<int, int> & links,
 			int from,
@@ -241,7 +241,7 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
  * @param useSameCostForAllLinks Ignore distance between nodes
  * @return the path ids from id "from" to id "to" including initial and final nodes.
  */
-std::list<int> RTABMAP_EXP computePath(
+std::list<int> RTABMAP_CORE_EXPORT computePath(
 			const std::multimap<int, Link> & links,
 			int from,
 			int to,
@@ -257,7 +257,7 @@ std::list<int> RTABMAP_EXP computePath(
  * @param updateNewCosts Keep up-to-date costs while traversing the graph.
  * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).
  */
-std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
+std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 		int fromId,
 		int toId,
 		const Memory * memory,
@@ -273,7 +273,7 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
  * @param distance squared distance of the nearest node found (optional)
  * @return the node id.
  */
-int RTABMAP_EXP findNearestNode(
+int RTABMAP_CORE_EXPORT findNearestNode(
 		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance = 0);
@@ -286,54 +286,56 @@ int RTABMAP_EXP findNearestNode(
  * @param k max nearest neighbors (0=all inside the radius)
  * @return the nodes with squared distance to query node.
  */
-std::map<int, float> RTABMAP_EXP findNearestNodes(
+std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
 		int nodeId,
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle = 0.0f,
 		int k=0);
-std::map<int, float> RTABMAP_EXP findNearestNodes(
+std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
 		const Transform & targetPose,
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle = 0.0f,
 		int k=0);
-std::map<int, Transform> RTABMAP_EXP findNearestPoses(
+std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
 		int nodeId,
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle = 0.0f,
 		int k=0);
-std::map<int, Transform> RTABMAP_EXP findNearestPoses(
+std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
 		const Transform & targetPose,
 		const std::map<int, Transform> & poses,
 		float radius,
 		float angle = 0.0f,
 		int k=0);
 
-// typedef hack to avoid error with RTABMAP_DEPRECATED
+// Use new findNearestNodes() interface with radius=0, angle=0.
-typedef std::map<int, float> _mapIntFloat;
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
-typedef std::map<int, Transform> _mapIntTransform;
+// Renamed to findNearestNodes()
-RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.");
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius);
-RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+// Renamed to findNearestNodes()
-RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius);
-RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
+// Renamed to findNearestNodes()
-RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
+// Renamed to findNearestNodes()
+RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
 
-float RTABMAP_EXP computePathLength(
+float RTABMAP_CORE_EXPORT computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
 		unsigned int fromIndex = 0,
 		unsigned int toIndex = 0);
 
 // assuming they are all linked in map order
-float RTABMAP_EXP computePathLength(
+float RTABMAP_CORE_EXPORT computePathLength(
 		const std::map<int, Transform> & path);
 
-std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
+std::list<std::map<int, Transform> > RTABMAP_CORE_EXPORT getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);
 
-void RTABMAP_EXP computeMinMax(const std::map<int, Transform> & poses,
+void RTABMAP_CORE_EXPORT computeMinMax(const std::map<int, Transform> & poses,
 		cv::Vec3f & min,
 		cv::Vec3f & max);
 

corelib/include/rtabmap/core/IMUThread.h

@@ -27,9 +27,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -39,11 +40,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+class IMUFilter;
+
 /**
  * Class IMUThread
  *
  */
-class RTABMAP_EXP IMUThread :
+class RTABMAP_CORE_EXPORT IMUThread :
 	public UThread,
 	public UEventsSender
 {
@@ -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/Landmark.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
 
-#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/rtabmap_core_export.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
@@ -59,7 +59,8 @@ public:
         UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
         UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
     }
-    RTABMAP_DEPRECATED(Landmark(const int & id, const Transform & pose, const cv::Mat & covariance), "Use constructor with size=0 instead.");
+    // Use constructor with size=0 instead.
+    RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance);
 
 	virtual ~Landmark() {}
 

corelib/include/rtabmap/core/LaserScan.h

@@ -28,13 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Transform.h>
 
 namespace rtabmap {
 
-class RTABMAP_EXP LaserScan
+class RTABMAP_CORE_EXPORT LaserScan
 {
 public:
 	enum Format{kUnknown=0,
@@ -75,24 +75,26 @@ public:
 			int maxPoints,
 			float maxRange,
 			const Transform & localTransform = Transform::getIdentity());
-	RTABMAP_DEPRECATED(LaserScan(const LaserScan & data,
+	// Use version without \"format\" argument.
+	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
 			int maxPoints,
 			float maxRange,
 			Format format,
-			const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument.");
+			const Transform & localTransform = Transform::getIdentity());
 	LaserScan(const cv::Mat & data,
 			int maxPoints,
 			float maxRange,
 			Format format,
 			const Transform & localTransform = Transform::getIdentity());
-	RTABMAP_DEPRECATED(LaserScan(const LaserScan & data,
+	// Use version without \"format\" argument.
+	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
 			Format format,
 			float minRange,
 			float maxRange,
 			float angleMin,
 			float angleMax,
 			float angleIncrement,
-			const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument.");
+			const Transform & localTransform = Transform::getIdentity());
 	LaserScan(const LaserScan & data,
 			float minRange,
 			float maxRange,

corelib/include/rtabmap/core/Link.h

@@ -28,14 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef LINK_H_
 #define LINK_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Transform.h>
 #include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
-class RTABMAP_EXP Link
+class RTABMAP_CORE_EXPORT Link
 {
 public:
 	enum Type {

corelib/include/rtabmap/core/MarkerDetector.h

@@ -56,19 +56,20 @@ private:
     Transform pose_;
 };
 
-class RTABMAP_EXP MarkerDetector {
+class RTABMAP_CORE_EXPORT MarkerDetector {
     
 public:
 	MarkerDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~MarkerDetector();
 	void parseParameters(const ParametersMap & parameters);
 
-    RTABMAP_DEPRECATED(
+	// Use the other detect(), in which the returned map contains the length of each marker detected.
+    RTABMAP_DEPRECATED
     MapIdPose detect(const cv::Mat & image,
 			const CameraModel & model,
 			const cv::Mat & depth = cv::Mat(),
 			float * estimatedMarkerLength = 0,
-			cv::Mat * imageWithDetections = 0), "Use the other detect(), in which the returned map contains the length of each marker detected.");
+			cv::Mat * imageWithDetections = 0);
 
     std::map<int, MarkerInfo> detect(const cv::Mat & image,
 		    const std::vector<CameraModel> & models,

corelib/include/rtabmap/core/Memory.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef MEMORY_H_
 #define MEMORY_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
@@ -60,7 +60,7 @@ class Stereo;
 class OccupancyGrid;
 class MarkerDetector;
 
-class RTABMAP_EXP Memory
+class RTABMAP_CORE_EXPORT Memory
 {
 public:
 	static const int kIdStart;

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_SRC_OCCUPANCYGRID_H_
 #define CORELIB_SRC_OCCUPANCYGRID_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <pcl/point_cloud.h>
 #include <pcl/pcl_base.h>
@@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class RTABMAP_EXP OccupancyGrid
+class RTABMAP_CORE_EXPORT OccupancyGrid
 {
 public:
 	inline static float logodds(double probability)
@@ -132,6 +132,7 @@ private:
 	bool normalsSegmentation_;
 	bool grid3D_;
 	bool groundIsObstacle_;
+	bool labelUndergroundObstaclesAsGround_;
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;

corelib/include/rtabmap/core/OctoMap.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef SRC_OCTOMAP_H_
 #define SRC_OCTOMAP_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <octomap/ColorOcTree.h>
 #include <octomap/OcTreeKey.h>
@@ -171,7 +171,7 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTr
 
   };
 
-class RTABMAP_EXP OctoMap {
+class RTABMAP_CORE_EXPORT OctoMap {
 public:
 	OctoMap(const ParametersMap & parameters = ParametersMap());
 

corelib/include/rtabmap/core/Odometry.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef ODOMETRY_H_
 #define ODOMETRY_H_
 
-#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/rtabmap_core_export.h>
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
@@ -39,7 +39,7 @@ namespace rtabmap {
 class OdometryInfo;
 class ParticleFilter;
 
-class RTABMAP_EXP Odometry
+class RTABMAP_CORE_EXPORT Odometry
 {
 public:
 	enum Type {
@@ -75,7 +75,8 @@ public:
 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
-	RTABMAP_DEPRECATED(const Transform & previousVelocityTransform() const, "Use getVelocityGuess() instead.");
+	// Use getVelocityGuess() instead.
+	RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const;
 	const Transform & getVelocityGuess() const {return velocityGuess_;}
 	double previousStamp() const {return previousStamp_;}
 	unsigned int framesProcessed() const {return framesProcessed_;}

corelib/include/rtabmap/core/OdometryThread.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef ODOMETRYTHREAD_H_
 #define ODOMETRYTHREAD_H_
 
-#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/rtabmap_core_export.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
@@ -38,7 +38,7 @@ namespace rtabmap {
 
 class Odometry;
 
-class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
+class RTABMAP_CORE_EXPORT OdometryThread : public UThread, public UEventsHandler {
 public:
 	// take ownership of Odometry
 	OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);

corelib/include/rtabmap/core/Optimizer.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef OPTIMIZER_H_
 #define OPTIMIZER_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <map>
 #include <list>
@@ -58,7 +58,7 @@ public:
 ////////////////////////////////////////////
 // Graph optimizers
 ////////////////////////////////////////////
-class RTABMAP_EXP Optimizer
+class RTABMAP_CORE_EXPORT Optimizer
 {
 public:
 	enum Type {

corelib/include/rtabmap/core/Parameters.h

@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define PARAMETERS_H_
 
 // default parameters
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include "rtabmap/core/Version.h" // DLL export/import defines
 #include <rtabmap/utilite/UConversion.h>
 #include <opencv2/core/version.hpp>
@@ -167,7 +167,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
  * @see getDefaultParameters()
  * TODO Add a detailed example with simple classes
  */
-class RTABMAP_EXP Parameters
+class RTABMAP_CORE_EXPORT Parameters
 {
     // Rtabmap parameters
     RTABMAP_PARAM(Rtabmap, PublishStats,                 bool, true,  "Publishing statistics.");
@@ -183,7 +183,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
     RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
     RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "",          "Working directory.");
-    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum nodes retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRepublished,           unsigned int, 2, uFormat("Maximum nodes republished when requesting missing data. When %s=false, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if %s=false.", kRGBDEnabled().c_str(), kRtabmapPublishLastSignature().c_str()));
     RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true,  "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
     RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
     RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
@@ -375,6 +376,7 @@ class RTABMAP_EXP 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()));
 
     // Local/Proximity loop closure detection
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
@@ -662,7 +664,8 @@ class RTABMAP_EXP Parameters
 #else
     RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
 #endif
-    RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
+	RTABMAP_PARAM(Icp, ReciprocalCorrespondences, bool, true,   "To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence.");
+	RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
     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()));
@@ -757,6 +760,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #endif
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, UndergroundIsGround,        bool,   false,   uFormat("[%s=true] Label all underground points under largest flat surface detected as ground.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
     RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
     RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));

corelib/include/rtabmap/core/Recovery.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RECOVERY_H_
 #define RECOVERY_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <string>
 
@@ -44,7 +44,7 @@ class ProgressState;
  * @param errorMsg error message if the function returns false
  * @param progressState A ProgressState object used to get status of the recovery process
  */
-bool RTABMAP_EXP databaseRecovery(
+bool RTABMAP_CORE_EXPORT databaseRecovery(
 		const std::string & corruptedDatabase,
 		bool keepCorruptedDatabase = true,
 		std::string * errorMsg = 0,

corelib/include/rtabmap/core/Registration.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTABMAP_REGISTRATION_H_
 #define RTABMAP_REGISTRATION_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Signature.h>
@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class RTABMAP_EXP Registration
+class RTABMAP_CORE_EXPORT Registration
 {
 public:
 	enum Type {

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef REGISTRATIONICP_H_
 #define REGISTRATIONICP_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Registration.h>
 #include <rtabmap/core/Signature.h>
@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 // Geometrical registration
-class RTABMAP_EXP RegistrationIcp : public Registration
+class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration
 {
 public:
 	// take ownership of child
@@ -64,6 +64,7 @@ private:
 	float _rangeMin;
 	float _rangeMax;
 	float _maxCorrespondenceDistance;
+	bool _reciprocalCorrespondences;
 	int _maxIterations;
 	float _epsilon;
 	float _correspondenceRatio;

corelib/include/rtabmap/core/RegistrationVis.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef REGISTRATIONVIS_H_
 #define REGISTRATIONVIS_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Registration.h>
 #include <rtabmap/core/Signature.h>
@@ -42,7 +42,7 @@ class PyMatcher;
 #endif
 
 // Visual registration
-class RTABMAP_EXP RegistrationVis : public Registration
+class RTABMAP_CORE_EXPORT RegistrationVis : public Registration
 {
 public:
 	// take ownership of child

corelib/include/rtabmap/core/Rtabmap.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTABMAP_H_
 #define RTABMAP_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/SensorData.h"
@@ -51,7 +51,7 @@ class Signature;
 class Optimizer;
 class PythonInterface;
 
-class RTABMAP_EXP Rtabmap
+class RTABMAP_CORE_EXPORT Rtabmap
 {
 public:
 	enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
@@ -180,12 +180,13 @@ public:
 	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints);
 	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
-	RTABMAP_DEPRECATED(
+	// Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.
+	RTABMAP_DEPRECATED
 		void get3DMap(std::map<int, Signature> & signatures,
 				std::map<int, Transform> & poses,
 				std::multimap<int, Link> & constraints,
 				bool optimized,
-				bool global) const, "Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.");
+				bool global) const;
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			bool optimized,
@@ -223,6 +224,7 @@ public:
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
+	void addNodesToRepublish(const std::vector<int> & ids);
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -284,6 +286,7 @@ private:
 	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
+	unsigned int _maxRepublished;
 	bool _rawDataKept;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
@@ -323,6 +326,7 @@ private:
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;
+	bool _localizationSmoothing;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;
@@ -359,15 +363,17 @@ private:
 	Transform _mapCorrectionBackup; // used in localization mode when odom is lost
 	Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
 	int _lastLocalizationNodeId; // for localization mode
+	cv::Mat _localizationCovariance;
 	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
 	bool _currentSessionHasGPS;
 	LaserScan _globalScanMap;
 	std::map<int, Transform> _globalScanMapPoses;
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
-	std::vector<float> _odomCorrectionAcc;
 	std::map<int, Transform> _markerPriors;
 
+	std::set<int> _nodesToRepublish;
+
 	// Planning stuff
 	int _pathStatus;
 	std::vector<std::pair<int,Transform> > _path;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -59,15 +59,18 @@ class RtabmapEventCmd : public UEvent
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
 	enum Cmd {
+			kCmdUndef,
 			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
 			kCmdClose,            // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
+			kCmdUpdateParams,     // params: ParametersMap
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
 			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
+			kCmdRepublishData,    // params: [vector<int>] ids
 			kCmdTriggerNewMap,
 			kCmdPause,
 			kCmdResume,

corelib/include/rtabmap/core/RtabmapExp.h

@@ -1,50 +0,0 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef RTABMAPEXP_H
-#define RTABMAPEXP_H
-
-#if defined(_WIN32)
-  #if defined(rtabmap_core_EXPORTS)
-    #define RTABMAP_EXP   __declspec( dllexport )
-  #else
-    #define RTABMAP_EXP   __declspec( dllimport )
-  #endif
-#else
-  #define RTABMAP_EXP
-#endif
-
-#ifdef __GNUC__
-#define RTABMAP_DEPRECATED(func, msg) func __attribute__ ((deprecated(msg)))
-#elif defined(_MSC_VER)
-#define RTABMAP_DEPRECATED(func, msg) __declspec(deprecated(msg)) func
-#else
-#pragma message("WARNING: You need to implement DEPRECATED for this compiler")
-#define RTABMAP_DEPRECATED(func, msg) func
-#endif
-
-#endif // RTABMAPEXP_H

corelib/include/rtabmap/core/RtabmapThread.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTABMAPTHREAD_H_
 #define RTABMAPTHREAD_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/utilite/UThreadNode.h>
 #include <rtabmap/utilite/UEventsHandler.h>
@@ -48,28 +48,14 @@ namespace rtabmap {
 
 class Rtabmap;
 
-class RTABMAP_EXP RtabmapThread :
+class RTABMAP_CORE_EXPORT RtabmapThread :
 	public UThreadNode,
 	public UEventsHandler
 {
 public:
 	enum State {
-		kStateInit,
 		kStateDetecting,
-		kStateReseting,
+		kStateProcessCommand
-		kStateClose,
-		kStateChangingParameters,
-		kStateDumpingMemory,
-		kStateDumpingPrediction,
-		kStateExportingDOTGraph,
-		kStateExportingPoses,
-		kStateCleanDataBuffer,
-		kStatePublishingMap,
-		kStateTriggeringMap,
-		kStateSettingGoal,
-		kStateCancellingGoal,
-		kStateLabelling,
-		kStateRemovingLabel
 	};
 
 public:
@@ -105,13 +91,13 @@ private:
 	void process();
 	void addData(const OdometryEvent & odomEvent);
 	bool getData(OdometryEvent & data);
-	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
+	void pushNewState(State newState, const RtabmapEventCmd & cmdEvent = RtabmapEventCmd(RtabmapEventCmd::kCmdUndef));
 	void publishMap(bool optimized, bool full, bool graphOnly) const;
 
 private:
 	UMutex _stateMutex;
 	std::queue<State> _state;
-	std::queue<ParametersMap> _stateParam;
+	std::queue<RtabmapEventCmd> _stateParam;
 
 	std::list<OdometryEvent> _dataBuffer;
 	std::list<double> _newMapEvents;

corelib/include/rtabmap/core/SensorData.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef SENSORDATA_H_
 #define SENSORDATA_H_
 
-#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/rtabmap_core_export.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
@@ -48,7 +48,7 @@ namespace rtabmap
 /**
  * An id is automatically generated if id=0.
  */
-class RTABMAP_EXP SensorData
+class RTABMAP_CORE_EXPORT SensorData
 {
 public:
 	// empty constructor
@@ -210,10 +210,14 @@ public:
 	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
 	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
 
-	RTABMAP_DEPRECATED(void setImageRaw(const cv::Mat & image), "Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.");
+	// Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
-	RTABMAP_DEPRECATED(void setDepthOrRightRaw(const cv::Mat & image), "Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.");
+	RTABMAP_DEPRECATED void setImageRaw(const cv::Mat & image);
-	RTABMAP_DEPRECATED(void setLaserScanRaw(const LaserScan & scan), "Use setLaserScan() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.");
+	// Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
-	RTABMAP_DEPRECATED(void setUserDataRaw(const cv::Mat & data), "Use setUserData() or removeRawData() instead.");
+	RTABMAP_DEPRECATED void setDepthOrRightRaw(const cv::Mat & image);
+	// Use setLaserScan() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
+	RTABMAP_DEPRECATED void setLaserScanRaw(const LaserScan & scan);
+	// Use setUserData() or removeRawData() instead.
+	RTABMAP_DEPRECATED void setUserDataRaw(const cv::Mat & data);
 
 	void uncompressData();
 	void uncompressData(

corelib/include/rtabmap/core/Signature.h

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <pcl/point_types.h>
 #include <opencv2/core/core.hpp>
@@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class RTABMAP_EXP Signature
+class RTABMAP_CORE_EXPORT Signature
 {
 
 public:

corelib/include/rtabmap/core/Statistics.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef STATISTICS_H_
 #define STATISTICS_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -50,7 +50,7 @@ namespace rtabmap {
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME
 
-class RTABMAP_EXP Statistics
+class RTABMAP_CORE_EXPORT Statistics
 {
 	RTABMAP_STATS(Loop, Id,); // Combined loop or proximity detection
 	RTABMAP_STATS(Loop, RejectedHypothesis,);
@@ -67,7 +67,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Visual_inliers,);
 	RTABMAP_STATS(Loop, Visual_inliers_ratio,);
 	RTABMAP_STATS(Loop, Visual_matches,);
-	RTABMAP_STATS(Loop, Distance_since_last_loc,);
+	RTABMAP_STATS(Loop, Distance_since_last_loc, m);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
 	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
@@ -90,15 +90,6 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Odom_correction_roll, deg);
 	RTABMAP_STATS(Loop, Odom_correction_pitch, deg);
 	RTABMAP_STATS(Loop, Odom_correction_yaw, deg);
-	//Odom correction
-	RTABMAP_STATS(Loop, Odom_correction_acc_norm, m);
-	RTABMAP_STATS(Loop, Odom_correction_acc_angle, deg);
-	RTABMAP_STATS(Loop, Odom_correction_acc_x, m);
-	RTABMAP_STATS(Loop, Odom_correction_acc_y, m);
-	RTABMAP_STATS(Loop, Odom_correction_acc_z, m);
-	RTABMAP_STATS(Loop, Odom_correction_acc_roll, deg);
-	RTABMAP_STATS(Loop, Odom_correction_acc_pitch, deg);
-	RTABMAP_STATS(Loop, Odom_correction_acc_yaw, deg);
 	// Map to Odom
 	RTABMAP_STATS(Loop, MapToOdom_norm, m);
 	RTABMAP_STATS(Loop, MapToOdom_angle, deg);
@@ -115,6 +106,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, MapToBase_roll, deg);
 	RTABMAP_STATS(Loop, MapToBase_pitch, deg);
 	RTABMAP_STATS(Loop, MapToBase_yaw, deg);
+	RTABMAP_STATS(Loop, MapToBase_lin_std, m);
+	RTABMAP_STATS(Loop, MapToBase_lin_var, m2);
 
 	RTABMAP_STATS(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);
@@ -158,6 +151,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, RAM_usage, MB);
 	RTABMAP_STATS(Memory, RAM_estimated, MB);
 	RTABMAP_STATS(Memory, Triangulated_points, );
+	RTABMAP_STATS(Memory, Closest_node_distance, m);
+	RTABMAP_STATS(Memory, Closest_node_angle, rad);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
@@ -241,7 +236,10 @@ public:
 	void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
 	void setStamp(double stamp) {_stamp = stamp;}
 
-	void setLastSignatureData(const Signature & data) {_lastSignatureData = data;}
+	// Use addSignatureData() instead.
+	RTABMAP_DEPRECATED void setLastSignatureData(const Signature & data);
+	void addSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}
+	void setSignaturesData(const std::map<int, Signature> & data) {_signaturesData = data;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -270,7 +268,8 @@ public:
 	int proximityDetectionMapId() const {return _proximiyDetectionMapId;}
 	double stamp() const {return _stamp;}
 
-	const Signature & getLastSignatureData() const {return _lastSignatureData;}
+	const Signature & getLastSignatureData() const {return _signaturesData.empty()?_dummyEmptyData:_signaturesData.rbegin()->second;}
+	const std::map<int, Signature> & getSignaturesData() const {return _signaturesData;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -302,7 +301,8 @@ private:
 	int _proximiyDetectionMapId;
 	double _stamp;
 
-	Signature _lastSignatureData;
+	std::map<int, Signature> _signaturesData;
+	Signature _dummyEmptyData;
 
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;

corelib/include/rtabmap/core/Stereo.h

@@ -28,14 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef STEREO_H_
 #define STEREO_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
-class RTABMAP_EXP Stereo {
+class RTABMAP_CORE_EXPORT Stereo {
 public:
 	static Stereo * create(const ParametersMap & parameters = ParametersMap());
 
@@ -67,7 +67,7 @@ private:
 	bool winSSD_;
 };
 
-class RTABMAP_EXP StereoOpticalFlow : public Stereo {
+class RTABMAP_CORE_EXPORT StereoOpticalFlow : public Stereo {
 public:
 	StereoOpticalFlow(const ParametersMap & parameters = ParametersMap());
 	virtual ~StereoOpticalFlow() {}

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class RTABMAP_EXP StereoCameraModel
+class RTABMAP_CORE_EXPORT StereoCameraModel
 {
 public:
 	StereoCameraModel() : leftSuffix_("left"), rightSuffix_("right") {}
@@ -140,7 +140,7 @@ private:
 	cv::Mat F_;
 };
 
-RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model);
+RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model);
 
 } // rtabmap
 

corelib/include/rtabmap/core/StereoDense.h

@@ -28,14 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef STEREODENSE_H_
 #define STEREODENSE_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
-class RTABMAP_EXP StereoDense {
+class RTABMAP_CORE_EXPORT StereoDense {
 public:
 	enum Type {
 		kTypeBM = 0,

corelib/include/rtabmap/core/Transform.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef TRANSFORM_H_
 #define TRANSFORM_H_
 
-#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/rtabmap_core_export.h>
 #include <vector>
 #include <string>
 #include <map>
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class RTABMAP_EXP Transform
+class RTABMAP_CORE_EXPORT Transform
 {
 public:
 
@@ -168,16 +168,17 @@ public:
 	static Transform getTransform(
 				const std::map<double, Transform> & tfBuffer,
 				const double & stamp);
-	RTABMAP_DEPRECATED(static Transform getClosestTransform(
+	// Use Transform::getTransform() instead to get always accurate transforms.
+	RTABMAP_DEPRECATED static Transform getClosestTransform(
 				const std::map<double, Transform> & tfBuffer,
 				const double & stamp,
-				double * stampDiff), "Use Transform::getTransform() instead to get always accurate transforms.");
+				double * stampDiff);
 
 private:
 	cv::Mat data_;
 };
 
-RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
+RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const Transform& s);
 
 class TransformStamped
 {

corelib/include/rtabmap/core/VWDictionary.h

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/core/core.hpp>
@@ -43,7 +43,7 @@ class DBDriver;
 class VisualWord;
 class FlannIndex;
 
-class RTABMAP_EXP VWDictionary
+class RTABMAP_CORE_EXPORT VWDictionary
 {
 public:
 	enum NNStrategy{

corelib/include/rtabmap/core/VisualWord.h

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <opencv2/core/core.hpp>
 #include <map>
@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class RTABMAP_EXP VisualWord
+class RTABMAP_CORE_EXPORT VisualWord
 {
 public:
 	VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0);

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -43,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraDepthAI :
+class RTABMAP_CORE_EXPORT CameraDepthAI :
 	public Camera
 {
 public:
@@ -59,6 +57,10 @@ public:
 
 	void setOutputDepth(bool enabled, int confidence = 200);
 	void setIMUFirmwareUpdate(bool enabled);
+	void setIMUPublished(bool published);
+	void publishInterIMU(bool enabled);
+	void setLaserDotBrightness(float dotProjectormA = 0.0f);
+	void setFloodLightBrightness(float floodLightmA = 200.0f);
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -76,12 +78,16 @@ private:
 	int depthConfidence_;
 	int resolution_;
 	bool imuFirmwareUpdate_;
+	bool imuPublished_;
+	bool publishInterIMU_;
+	float dotProjectormA_;
+	float floodLightmA_;
 	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::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
+	UMutex imuMutex_;
 #endif
 };
 

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

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/Camera.h"
@@ -41,7 +41,7 @@ namespace rtabmap
 
 class FreenectDevice;
 
-class RTABMAP_EXP CameraFreenect :
+class RTABMAP_CORE_EXPORT CameraFreenect :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -45,7 +43,7 @@ class PacketPipeline;
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraFreenect2 :
+class RTABMAP_CORE_EXPORT CameraFreenect2 :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/utilite/UTimer.h"
 #include <list>
@@ -38,7 +36,7 @@ class UDirectory;
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraImages :
+class RTABMAP_CORE_EXPORT CameraImages :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -42,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraK4A :
+class RTABMAP_CORE_EXPORT CameraK4A :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -43,7 +41,7 @@ typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraK4W2 :
+class RTABMAP_CORE_EXPORT CameraK4W2 :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/USemaphore.h"
@@ -46,7 +44,7 @@ namespace rtabmap
  * Class CameraMyntEye
  *
  */
-class RTABMAP_EXP CameraMyntEye : public Camera
+class RTABMAP_CORE_EXPORT CameraMyntEye : public Camera
 {
 public:
 	static bool available();

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -41,7 +39,7 @@ class VideoStream;
 
 namespace rtabmap
 {
-class RTABMAP_EXP CameraOpenNI2 :
+class RTABMAP_CORE_EXPORT CameraOpenNI2 :
 	public Camera
 {
 

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

@@ -27,15 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
 
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraOpenNICV :
+class RTABMAP_CORE_EXPORT CameraOpenNICV :
 	public Camera
 {
 

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/USemaphore.h"
 #include "rtabmap/core/Camera.h"
@@ -56,7 +54,7 @@ class Grabber;
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraOpenni :
+class RTABMAP_CORE_EXPORT CameraOpenni :
 	public Camera
 {
 public:

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

@@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraRGBDImages :
+class RTABMAP_CORE_EXPORT CameraRGBDImages :
 	public CameraImages
 {
 public:

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

@@ -27,9 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/USemaphore.h"
 #include "rtabmap/core/CameraModel.h"
@@ -49,7 +46,7 @@ namespace rtabmap
 {
 
 class slam_event_handler;
-class RTABMAP_EXP CameraRealSense :
+class RTABMAP_CORE_EXPORT CameraRealSense :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -53,7 +51,7 @@ struct rs2_extrinsics;
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraRealSense2 :
+class RTABMAP_CORE_EXPORT CameraRealSense2 :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -38,7 +36,7 @@ namespace rtabmap
 
 class DC1394Device;
 
-class RTABMAP_EXP CameraStereoDC1394 :
+class RTABMAP_CORE_EXPORT CameraStereoDC1394 :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
 
@@ -40,7 +38,7 @@ class Camera;
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraStereoFlyCapture2 :
+class RTABMAP_CORE_EXPORT CameraStereoFlyCapture2 :
 	public Camera
 {
 public:

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

@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once
 
 #include <rtabmap/core/camera/CameraImages.h>
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/Version.h"
@@ -37,7 +36,7 @@ namespace rtabmap
 {
 
 class CameraImages;
-class RTABMAP_EXP CameraStereoImages :
+class RTABMAP_CORE_EXPORT CameraStereoImages :
 	public CameraImages
 {
 public:

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

@@ -32,8 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/camera/CameraVideo.h"
 #include "rtabmap/core/Version.h"
@@ -42,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraStereoTara :
+class RTABMAP_CORE_EXPORT CameraStereoTara :
 public Camera
 {
 public:

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

@@ -27,15 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/camera/CameraVideo.h"
 
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraStereoVideo :
+class RTABMAP_CORE_EXPORT CameraStereoVideo :
 	public Camera
 {
 public:

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/camera/CameraVideo.h"
 #include "rtabmap/core/Version.h"
@@ -42,16 +40,16 @@ namespace rtabmap
 {
 class ZedIMUThread;
 
-class RTABMAP_EXP CameraStereoZed :
+class RTABMAP_CORE_EXPORT CameraStereoZed :
 	public Camera
 {
 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,
@@ -63,7 +61,7 @@ public:
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
 	CameraStereoZed(
 			const std::string & svoFilePath,
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL
 			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,
 			bool computeOdometry = false,

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

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Version.h"
@@ -46,7 +44,7 @@ namespace rtabmap
 {
 class ZedOCThread;
 
-class RTABMAP_EXP CameraStereoZedOC :
+class RTABMAP_CORE_EXPORT CameraStereoZedOC :
 	public Camera
 {
 public:

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

@@ -27,15 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include <opencv2/highgui/highgui.hpp>
 #include "rtabmap/core/Camera.h"
 
 namespace rtabmap
 {
 
-class RTABMAP_EXP CameraVideo :
+class RTABMAP_CORE_EXPORT CameraVideo :
 	public Camera
 {
 public:

corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h

@@ -36,11 +36,11 @@ RTAB-Map integration: Mathieu Labbe
 #include <Eigen/Core>
 #include <opencv2/opencv.hpp>
 #include <rtabmap/utilite/UMutex.h>
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 namespace clams
 {
-  class RTABMAP_EXP DiscreteFrustum
+  class RTABMAP_CORE_EXPORT DiscreteFrustum
   {
   public:
     DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0, double max_dist = 10.0);
@@ -65,7 +65,7 @@ namespace clams
     friend class DiscreteDepthDistortionModel;
   };
 
-  class RTABMAP_EXP DiscreteDepthDistortionModel
+  class RTABMAP_CORE_EXPORT DiscreteDepthDistortionModel
   {
   public:
     // returns all divisors of num

corelib/include/rtabmap/core/clams/frame_projector.h

@@ -34,7 +34,6 @@ RTAB-Map integration: Mathieu Labbe
 #include <pcl/point_types.h>
 #include <opencv2/core/core.hpp>
 #include <rtabmap/core/CameraModel.h>
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #define MAX_MULT 1.3
 #define MIN_MULT 0.7
@@ -60,7 +59,7 @@ namespace clams
 
   //! This is essentially a pinhole camera model for an RGBD sensor, with
   //! some extra functions added on for use during calibration.
-  class RTABMAP_EXP FrameProjector
+  class RTABMAP_CORE_EXPORT FrameProjector
   {
   public:
     // For storing z values in meters.  This is not Euclidean distance.

corelib/include/rtabmap/core/clams/slam_calibrator.h

@@ -35,12 +35,10 @@ RTAB-Map integration: Mathieu Labbe
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 namespace clams
 {
   
-DiscreteDepthDistortionModel RTABMAP_EXP calibrate(
+DiscreteDepthDistortionModel RTABMAP_CORE_EXPORT calibrate(
 		const std::map<int, rtabmap::SensorData> & sequence,
 		const std::map<int, rtabmap::Transform> & trajectory,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,

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

@@ -44,6 +44,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		pcl::IndicesPtr & obstaclesIndices,
 		pcl::IndicesPtr * flatObstacles) const
 {
+	UDEBUG("cloudIn=%dx%d indicesIn=%ld", cloudIn->width, cloudIn->height, indicesIn->size());
+
 	groundIndices.reset(new std::vector<int>);
 	obstaclesIndices.reset(new std::vector<int>);
 	if(flatObstacles)
@@ -54,6 +56,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 	pcl::IndicesPtr indices(new std::vector<int>);
 
+	UDEBUG("preVoxelFiltering=%d", preVoxelFiltering_?1:0);
 	if(preVoxelFiltering_)
 	{
 		// voxelize to grid cell size
@@ -127,6 +130,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
 			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
 			UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
+			UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0);
+			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+			UDEBUG("cloud=%dx%d indices=%ld", cloud->width, cloud->height, indices->size());
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -140,8 +146,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 					maxGroundHeight_,
 					flatObstacles,
 					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
-					groundNormalsUp_);
+					groundNormalsUp_,
-			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+					labelUndergroundObstaclesAsGround_);
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
 			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
@@ -165,6 +171,42 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 
 		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 
+		if(!preVoxelFiltering_ && (!groundIndices->empty() || !obstaclesIndices->empty()))
+		{
+			// voxelize to grid cell size
+			typename pcl::PointCloud<PointT>::Ptr cloudWithTransform = cloud;
+			cloud.reset(new pcl::PointCloud<PointT>);
+			if(!groundIndices->empty())
+			{
+				*cloud += *util3d::voxelize(cloudWithTransform, groundIndices, cellSize_);
+				groundIndices->resize(cloud->size());
+				for(size_t i=0; i<groundIndices->size(); ++i)
+				{
+					groundIndices->at(i) = i;
+				}
+			}
+			if(!obstaclesIndices->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, obstaclesIndices, cellSize_);
+				obstaclesIndices->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<obstaclesIndices->size(); ++i)
+				{
+					obstaclesIndices->at(i) = previousSize+i;
+				}
+			}
+			if(flatObstacles && !(*flatObstacles)->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, *flatObstacles, cellSize_);
+				(*flatObstacles)->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<(*flatObstacles)->size(); ++i)
+				{
+					(*flatObstacles)->at(i) = previousSize+i;
+				}
+			}
+		}
+
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
 		{

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

@@ -50,6 +50,79 @@ typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 	return output;
 }
 
+void clusterIndicesFloodfill(std::vector<int> & cluster,
+	float * visitedIndices,
+	int width,
+	int height,
+	float clusterRadius,
+	int currentIndex,
+	float previousHeight);
+
+/**
+ * @brief Cluster indices of an organized cloud
+ * 
+ * @tparam PointT 
+ * @param cloud 
+ * @param indices 
+ * @param minClusterSize 
+ * @param maxClusterSize 
+ * @param biggestClusterIndex 
+ * @return std::vector<pcl::IndicesPtr> 
+ */
+template<typename PointT>
+std::vector<pcl::IndicesPtr> clusterIndices(
+	const typename pcl::PointCloud<PointT>::Ptr & cloud,
+	const typename pcl::IndicesPtr & indices,
+	float clusterRadius,
+	int minClusterSize,
+	int maxClusterSize,
+	int * biggestClusterIndex)
+{
+	std::vector<pcl::IndicesPtr> clusters;
+	if(cloud->empty())
+	{
+		return clusters;
+	}
+
+	UASSERT(cloud->isOrganized());
+
+	cv::Mat visitedIndices = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
+	float * ptr = visitedIndices.ptr<float>();
+	// init search image
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		ptr[indices->at(i)] = cloud->at(indices->at(i)).z;
+	}
+
+	int largestCluster = -1;
+	int largestClusterSize = 0;
+	int sum = 0;
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		if(ptr[indices->at(i)] != 0.0f)
+		{
+			pcl::IndicesPtr cluster(new pcl::Indices());
+			clusterIndicesFloodfill(*cluster, ptr, visitedIndices.cols, visitedIndices.rows, clusterRadius, indices->at(i), ptr[indices->at(i)]);
+			if(cluster->size()>0 && (int)cluster->size()>=minClusterSize && (int)cluster->size()<=maxClusterSize)
+			{
+				clusters.push_back(cluster);
+				if((int)cluster->size() > largestClusterSize)
+				{
+					sum+=cluster->size();
+					largestCluster = clusters.size()-1;
+					largestClusterSize = cluster->size();
+				}
+			}
+		}
+	}
+	if(biggestClusterIndex)
+	{
+		*biggestClusterIndex = largestCluster;
+	}
+
+	return clusters;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -64,7 +137,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -75,6 +149,8 @@ void segmentObstaclesFromGround(
 
 	if(cloud->size())
 	{
+		UDEBUG("Normal filtering.... cloud=%ld indices=%ld organized=%d",
+			cloud->size(), indices->size(), cloud->isOrganized()?1:0);
 		// Find the ground
 		pcl::IndicesPtr flatSurfaces = normalFiltering(
 				cloud,
@@ -84,22 +160,40 @@ void segmentObstaclesFromGround(
 				normalKSearch,
 				viewPoint,
 				groundNormalsUp);
+		UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)",
+			flatSurfaces->size(), indices->size(), cloud->size());
 
+		Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax(0,0,0,0);
 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
 			int biggestFlatSurfaceIndex;
-			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
+
+			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces;
+			if(cloud->isOrganized())
+			{
+				clusteredFlatSurfaces = clusterIndices<PointT>(
 					cloud,
 					flatSurfaces,
 					clusterRadius,
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);
+					UDEBUG("clusteredFlatSurfaces=%ld", clusteredFlatSurfaces.size());
+			}
+			else
+			{
+				clusteredFlatSurfaces = extractClusters(
+					cloud,
+					flatSurfaces,
+					clusterRadius,
+					minClusterSize,
+					std::numeric_limits<int>::max(),
+					&biggestFlatSurfaceIndex);
+			}
 
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
 			{
-				Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
 				if(maxGroundHeight != 0.0f)
 				{
 					// Search for biggest surface under max ground height
@@ -109,7 +203,7 @@ void segmentObstaclesFromGround(
 					{
 						Eigen::Vector4f min,max;
 						pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
-						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
+						if(min[2]<maxGroundHeight && clusteredFlatSurfaces.at(i)->size() > points)
 						{
 							points = clusteredFlatSurfaces.at(i)->size();
 							biggestFlatSurfaceIndex = i;
@@ -125,9 +219,12 @@ void segmentObstaclesFromGround(
 				if(biggestFlatSurfaceIndex>=0)
 				{
 					ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+					UDEBUG("Biggest flat surface size = %ld (%d%%) (z min=%f max=%f)",
+						ground->size(), 100*ground->size()/cloud->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]);
 				}
 
-				if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
+				if(!ground->empty() && 
+				  (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
@@ -135,7 +232,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
+							if(centroid[2] <= biggestSurfaceMax[2]) // relative to ground detected
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -145,9 +242,46 @@ void segmentObstaclesFromGround(
 							}
 						}
 					}
+
+					int groundRatio = 100*ground->size()/cloud->size();
+					int minGroundRatio = 10;
+					if(minGroundRatio != 0 && groundRatio<minGroundRatio)
+					{
+						if(labelUndergroundObstaclesAsGround && maxGroundHeight!=0.0f)
+						{
+							// just do passthrough (e.g. reflective floor)
+							UWARN("Failed normal segmentation (ground ratio=%d%%, ground height=%f), fallback to passThrough (label underground as ground is true).",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// passthrough filter
+							ground = rtabmap::util3d::passThrough(cloud, indices, "z",
+									std::numeric_limits<int>::min(),
+									maxGroundHeight!=0.0f?maxGroundHeight:std::numeric_limits<int>::max());
+
+							pcl::IndicesPtr notObstacles = ground;
+							if(indices->size())
+							{
+								notObstacles = util3d::extractIndices(cloud, indices, true);
+								notObstacles = util3d::concatenate(notObstacles, ground);
+							}
+							obstacles = rtabmap::util3d::extractIndices(cloud, notObstacles, true);
+							return;
+						}
+						else
+						{
+							UWARN("Failed normal segmentation, ground surface is too small (ground ratio=%d%%, ground height=%f)!",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// reject ground!
+							ground.reset(new std::vector<int>);
+							if(flatObstacles)
+							{
+								*flatObstacles = flatSurfaces;
+							}
+						}
+					}
 				}
 				else
 				{
+					UWARN("Failed normal segmentation, could not detect the ground!");
 					// reject ground!
 					ground.reset(new std::vector<int>);
 					if(flatObstacles)
@@ -168,28 +302,49 @@ void segmentObstaclesFromGround(
 			pcl::IndicesPtr notObstacles = ground;
 			if(indices->size())
 			{
+				// This will ignore all points not in input indices for obstacles.
 				notObstacles = util3d::extractIndices(cloud, indices, true);
 				notObstacles = util3d::concatenate(notObstacles, ground);
 			}
-			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
 
-			// If ground height is set, remove obstacles under it
+			// If ground height is set and if we label obstacles under it as ground
-			if(maxGroundHeight != 0.0f)
+			if(labelUndergroundObstaclesAsGround)
 			{
-				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+				float max = biggestSurfaceMax[2];
+				if(maxGroundHeight > 0)
+				{
+					max += maxGroundHeight;
+				}
+
+				pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+				pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits<int>::min(), max);
+				if(!underground->empty())
+				{
+					ground = util3d::concatenate(ground, underground);
+					notObstacles = util3d::concatenate(underground, notObstacles);
+				}
 			}
 
+			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+
 			//Cluster remaining stuff (obstacles)
 			if(otherStuffIndices->size())
 			{
-				std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+				if(minClusterSize>1)
-						cloud,
+				{
-						otherStuffIndices,
+					std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
-						clusterRadius,
+							cloud,
-						minClusterSize);
+							otherStuffIndices,
+							clusterRadius,
+							minClusterSize);
 
-				// merge indices
+					// merge indices
-				obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+					obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+				}
+				else
+				{
+					obstacles = otherStuffIndices;
+				}
 			}
 		}
 	}
@@ -208,7 +363,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -224,7 +380,8 @@ void segmentObstaclesFromGround(
 			maxGroundHeight,
 			flatObstacles,
 			viewPoint,
-			groundNormalsUp);
+			groundNormalsUp,
+			labelUndergroundObstaclesAsGround);
 }
 
 template<typename PointT>

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

@@ -40,7 +40,7 @@ class RgbdCameraPyramid;
 
 namespace rtabmap {
 
-class RTABMAP_EXP OdometryDVO : public Odometry
+class RTABMAP_CORE_EXPORT OdometryDVO : public Odometry
 {
 public:
 	OdometryDVO(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());

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

@@ -35,7 +35,7 @@ namespace rtabmap {
 
 class Registration;
 
-class RTABMAP_EXP OdometryF2F : public Odometry
+class RTABMAP_CORE_EXPORT OdometryF2F : public Odometry
 {
 public:
 	OdometryF2F(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());

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

@@ -41,7 +41,7 @@ class Signature;
 class Registration;
 class Optimizer;
 
-class RTABMAP_EXP OdometryF2M : public Odometry
+class RTABMAP_CORE_EXPORT OdometryF2M : public Odometry
 {
 public:
 	OdometryF2M(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());