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

@@ -3,7 +3,7 @@ name: CMake-ROS
 on:
   push:
     branches: 
-      - '**'
+      - master
   pull_request:
     branches: 
       - '**'
@@ -22,10 +22,8 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        ros_distribution: [melodic, noetic, foxy, humble, rolling]
+        ros_distribution: [ noetic, foxy, humble, rolling]
         include:
-        - ros_distribution: 'melodic' 
-          os: ubuntu-18.04
         - ros_distribution: 'noetic'
           os: ubuntu-20.04
         - ros_distribution: 'foxy'
@@ -36,17 +34,21 @@ jobs:
           os: ubuntu-22.04
 
     steps:
-    - uses: ros-tooling/setup-ros@v0.4
+    - name: Workaround dpkg grub-efi-amd64-signed error
+      run: |
+        sudo apt-mark hold grub-efi-amd64-signed
+    
+    - uses: ros-tooling/setup-ros@v0.5
       with:
         required-ros-distributions: ${{ matrix.ros_distribution }}
     
+    - uses: actions/checkout@v2
+
     - name: Install dependencies
       run: |
-        sudo apt-get update
-        sudo apt-get -y install ros-${{ matrix.ros_distribution }}-rtabmap
-        sudo apt-get -y remove ros-${{ matrix.ros_distribution }}-rtabmap
-
-    - uses: actions/checkout@v2
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        rosdep update
+        rosdep install --from-paths ${{github.workspace}} -y
 
     - name: Configure CMake
       run: |

.github/workflows/cmake.yml

@@ -3,7 +3,7 @@ name: CMake
 on:
   push:
     branches: 
-      - '**'
+      - master
   pull_request:
     branches: 
       - '**'
@@ -17,7 +17,7 @@ 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

.github/workflows/docker.yml

@@ -11,7 +11,7 @@ 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: |
@@ -43,6 +43,26 @@ jobs:
             linux/amd64
             linux/arm64
           docker_path: 'focal'
+        - docker_tag: focal-foxy
+          docker_tags: |
+            introlab3it/rtabmap:focal-foxy
+            introlab3it/rtabmap:20.04-foxy
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal-foxy'
+        - docker_tag: jammy
+          docker_tags: |
+            introlab3it/rtabmap:jammy
+            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_PATCH_VERSION 23)
+SET(RTABMAP_MINOR_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)
@@ -146,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)
@@ -183,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)
@@ -213,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()
@@ -221,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)
@@ -292,13 +297,23 @@ IF(WITH_QT)
     ENDIF(NOT VTK_FOUND)
 
     # If Qt is here, the GUI will be built
+    IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
+	  IF(NOT VTK_QT_VERSION)
+	    MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.")
+	  ENDIF()
+      MESSAGE(STATUS "VTK>=9 detected, will use VTK_QT_VERSION=${VTK_QT_VERSION} for Qt version.")
+      IF(${VTK_QT_VERSION} EQUAL 6)
+        FIND_PACKAGE(Qt6 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg)
+      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 QUIET)
-            IF(Qt5_FOUND)
-                FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
-            ENDIF(Qt5_FOUND)
+        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)
 
@@ -307,8 +322,9 @@ IF(WITH_QT)
           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()
         
-    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()
@@ -316,7 +332,7 @@ IF(WITH_QT)
             IF(value EQUAL -1)
                 list(FIND PCL_LIBRARIES vtkGUISupportQt value)
                 IF(value EQUAL -1)
-                    IF("${VTK_MAJOR_VERSION}" GREATER 8)
+                    IF(NOT(${VTK_MAJOR_VERSION} LESS 9))
                         SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::GUISupportQt")
                     ELSE()
                         SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
@@ -352,7 +368,7 @@ IF(WITH_QT)
 
         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)
@@ -446,7 +462,7 @@ 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})
@@ -477,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)
@@ -742,7 +766,18 @@ 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))
+      # Qt6 requires c++17
+      include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
+      IF(COMPILER_SUPPORTS_CXX17)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
+        set(CMAKE_CXX_STANDARD 17)
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler if you want to use Qt6.")
+      ENDIF()
+    ENDIF()
+    IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND ((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)))
      #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
      include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
@@ -754,7 +789,7 @@ IF(NOT MSVC)
       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
            GTSAM_FOUND OR
            CERES_FOUND OR
@@ -786,7 +821,7 @@ 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 "/")
@@ -814,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
@@ -839,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 "//")
    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)
@@ -860,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 "//")
@@ -889,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_DEPENDENCIES ${CONF_DEPENDENCIES} depthai::core depthai::opencv) 
+   SET(CONF_WITH_DEPTH_AI 1)
 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 "//")
@@ -1024,22 +1032,14 @@ IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
-   IF("${VTK_MAJOR_VERSION}" GREATER 8)
-      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} VTK::GUISupportQt)
-   ELSE()
-      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
-   ENDIF()
 ELSE()
    SET(CONF_VTK_QT false)
 ENDIF()
-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 )
@@ -1048,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 )
@@ -1076,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
-file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR}")
-file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
-   
-# ... 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)
+add_library(rtabmap INTERFACE)
+add_library(rtabmap::rtabmap ALIAS rtabmap)
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_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
-  "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY)
+install(TARGETS rtabmap EXPORT rtabmapTargets)
+export(EXPORT rtabmapTargets
+  FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake"
+  NAMESPACE rtabmap::
+)
+install(EXPORT rtabmapTargets
+  FILE
+    ${PROJECT_NAME}Targets.cmake
+  DESTINATION
+    ${INSTALL_CMAKE_DIR}
+  NAMESPACE rtabmap::
+  COMPONENT
+    devel
+)
 
-# ... for the install tree
-set(CONF_INCLUDE_DIRS "\${RTABMap_CMAKE_DIR}/${REL_INCLUDE_DIR}")
-set(CONF_LIB_DIR "\${RTABMap_CMAKE_DIR}/${REL_LIB_DIR}")
-configure_file(RTABMapConfig.cmake.in
-  "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake" @ONLY)
+####
+# Setup RTABMapConfig.cmake
+####
+include(CMakePackageConfigHelpers)
+write_basic_package_version_file(
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+  VERSION ${PROJECT_VERSION}
+  COMPATIBILITY AnyNewerVersion
+)
 
-# ... for both
-configure_file(RTABMapConfigVersion.cmake.in
-  "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake" @ONLY)
+# 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 the RTABMapConfig.cmake and RTABMapConfigVersion.cmake
-install(FILES
-  "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake"
-  "${PROJECT_BINARY_DIR}/RTABMapConfigVersion.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)
 
@@ -1141,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/" 
@@ -1268,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)")
@@ -1318,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()
@@ -1334,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()
@@ -1342,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()
@@ -1361,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)")
@@ -1380,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()
@@ -1406,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()
@@ -1416,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()
@@ -1537,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()
@@ -1553,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,11 +4,15 @@ rtabmap
 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
 
 [![Release][release-image]][releases]
+[![Downloads][downloads-image]][downloads]
 [![License][license-image]][license]
 
-[release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.21.0-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
+[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
+[downloads]: https://github.com/introlab/rtabmap/releases
+
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
 [license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
 
@@ -71,6 +75,12 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
             <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
-# 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)
+include(CMakeFindDependencyMacro)
 
-# Compute paths
-get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
-set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
+# Mandatory dependencies
+find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
 
-#core lib
-find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
+if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
     
-IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
-   SET(RTABMap_CORE
-      debug ${RTABMap_CORE_DEBUG}
-      optimized ${RTABMap_CORE_RELEASE}
-   )
-ELSEIF(RTABMap_CORE_DEBUG)
-   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
-ELSE()
-   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
-ENDIF()
-
-#utilite lib
-find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-
-IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
-   SET(RTABMap_UTILITE
-      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})
-
-list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
-set(WITH_GUI ON)
-if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
-   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
-   if (${_index} EQUAL -1)
-      set(WITH_GUI OFF)
+    if(@CONF_QT_VERSION@ EQUAL 6)
+        find_dependency(Qt6 COMPONENTS Widgets Core Gui OpenGL)
+    elseif(@CONF_QT_VERSION@ EQUAL 5)
+        find_dependency(Qt5 COMPONENTS Widgets Core Gui OpenGL)
+    else() # Qt4
+        find_dependency(Qt4 COMPONENTS QtCore QtGui)
     endif()
-endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+    set(RTABMap_QT_VERSION @CONF_QT_VERSION@)
+ELSE()
+    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()
 
-#gui lib (OFF if RTAB-Map is not built with Qt)
-if(@CONF_WITH_GUI@ AND ${WITH_GUI})
-   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
-   
-   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})
+IF(@CONF_WITH_REALSENSE2@)
+    IF(WIN32)
+        find_dependency(RealSense2)
+    ELSE()
+        find_dependency(realsense2)
     ENDIF()
+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()
+IF(@CONF_WITH_K4A@)
+    IF(WIN32)
+        find_dependency(K4A)
+    ELSE()
+        find_dependency(k4a)
+        find_dependency(k4arecord)
+    ENDIF()
+ENDIF()
 
-# Dependencies
-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@")
+IF(@CONF_WITH_DEPTH_AI@)
+ find_dependency(depthai 2)
+ENDIF()
 
-#backward compatibilities
-set(RTABMAP_CORE ${RTABMap_CORE})
-set(RTABMAP_UTILITE ${RTABMap_UTILITE})
-if(RTABMap_GUI)
-   set(RTABMAP_GUI ${RTABMap_GUI})
-   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
-endif(RTABMap_GUI)
+IF(@CONF_WITH_OCTOMAP@)
+  find_dependency(octomap)
+ENDIF()
 
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
-mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)
+# Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies)
+set(RTABMap_INCLUDE_DIRS "")
+set(RTABMap_LIBRARIES "")
+set(RTABMap_TARGETS "")
+
+set(_RTABMap_supported_components utilite core gui)
+
+foreach(_comp ${_RTABMap_supported_components})
+  if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    include("${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    set(RTABMap_${_comp}_FOUND True)
+    set(RTABMap_TARGETS
+      ${RTABMap_TARGETS}
+      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("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")
+
+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,6 +432,8 @@ LaserScan CameraMobile::scanFromPointCloudData(
             //get color from rgb image
             cv::Point3f org= pt;
             pt = util3d::transformPoint(pt, opticalRotationInv);
+            if(pt.z > 0)
+            {
                 int u,v;
                 model.reproject(pt.x, pt.y, pt.z, u, v);
                 unsigned char r=255,g=255,b=255;
@@ -448,7 +450,7 @@ LaserScan CameraMobile::scanFromPointCloudData(
                     *(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/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/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/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

@@ -556,7 +556,7 @@
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 			<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>
 			<key>Type</key>
@@ -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
@@ -52,38 +26,35 @@ IF(WIN32)
   ENDIF( MINGW )
 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})
-ELSEIF(MINGW)
-  ADD_EXECUTABLE(rtabmap WIN32 ${SRC_FILES})
+  ADD_EXECUTABLE(rtabmap_app MACOSX_BUNDLE ${SRC_FILES})
+ELSEIF(WIN32 AND BUILD_AS_BUNDLE)
+  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})
-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)
+TARGET_LINK_LIBRARIES(rtabmap_app rtabmap_gui)
 
 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
-  # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
-  # Exclude debug libraries
-  IF(QT_PLUGINS_DIR)
-    INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats"
-          DESTINATION ${plugin_dest_dir}/plugins 
-          COMPONENT runtime 
-          REGEX ".*d4.dll" EXCLUDE
-          REGEX ".*d4.a" EXCLUDE)
-  ELSE()
+  IF(Qt6_FOUND)
+	# Reference: https://doc-snapshots.qt.io/qt6-6.4/qt-deploy-runtime-dependencies.html
+	# The following script must only be executed at install time
+	set(deploy_script "${CMAKE_CURRENT_BINARY_DIR}/deploy_app$<CONFIG>.cmake")
+	file(GENERATE OUTPUT ${deploy_script} CONTENT "
+	# Including the file pointed to by QT_DEPLOY_SUPPORT ensures the generated
+	# deployment script has access to qt_deploy_runtime_dependencies()
+	include(\"${QT_DEPLOY_SUPPORT}\")
+	qt_deploy_runtime_dependencies(
+		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)
@@ -194,6 +181,15 @@ 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
@@ -207,9 +203,9 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 	" 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);

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");
-cv::Mat RTABMAP_EXP compressImage2(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_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");
 
-cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);
 
-std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
-cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
+std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(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_EXP 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 cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned long size);
 
-cv::Mat RTABMAP_EXP compressString(const std::string & str);
-std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str);
+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
@@ -57,7 +57,8 @@ public:
 			 bool landmarksIgnored = false,
 			 bool featuresIgnored = false,
 			 int startMapId = 0,
-			 int stopMapId = -1);
+			 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,
@@ -70,7 +71,8 @@ public:
 			 bool landmarksIgnored = false,
 			 bool featuresIgnored = false,
 			 int startMapId = 0,
-			 int stopMapId = -1);
+			 int stopMapId = -1,
+			 bool priorsIgnored = false);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -100,6 +102,7 @@ private:
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
+	bool _priorsIgnored;
 	int _startMapId;
 	int _stopMapId;
 

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
-typedef std::map<int, float> _mapIntFloat;
-typedef std::map<int, Transform> _mapIntTransform;
-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(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "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(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "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()");
+// Use new findNearestNodes() interface with radius=0, angle=0.
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
+// Renamed to findNearestNodes()
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, 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);
+// 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.");
@@ -376,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.");
@@ -663,6 +664,7 @@ class RTABMAP_EXP Parameters
 #else
     RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
 #endif
+	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.");
@@ -758,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,
@@ -325,6 +326,7 @@ private:
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;
+	bool _localizationSmoothing;
 	bool _createGlobalScanMap;
 	float _markerPriorsLinearVariance;
 	float _markerPriorsAngularVariance;
@@ -361,13 +363,13 @@ 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;

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,7 +48,7 @@ namespace rtabmap {
 
 class Rtabmap;
 
-class RTABMAP_EXP RtabmapThread :
+class RTABMAP_CORE_EXPORT RtabmapThread :
 	public UThreadNode,
 	public UEventsHandler
 {

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.");
-	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 setLaserScanRaw(const LaserScan & scan), "Use setLaserScan() 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.");
+	// 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);
+	// 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 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,8 @@ public:
 	void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
 	void setStamp(double stamp) {_stamp = stamp;}
 
-	RTABMAP_DEPRECATED(void setLastSignatureData(const Signature & data), "Use addSignatureData() instead.");
+	// 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;}
 

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:
@@ -60,6 +58,9 @@ 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;
@@ -78,12 +79,15 @@ private:
 	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_);
-			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+					groundNormalsUp_,
+					labelUndergroundObstaclesAsGround_);
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
 			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
@@ -165,6 +171,42 @@ typename pcl::PointCloud<PointT>::Ptr 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
@@ -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,19 +302,35 @@ void segmentObstaclesFromGround(
 			pcl::IndicesPtr notObstacles = ground;
 			if(indices->size())
 			{
+				// This will ignore all points not in input indices for obstacles.
 				notObstacles = util3d::extractIndices(cloud, indices, true);
 				notObstacles = util3d::concatenate(notObstacles, ground);
 			}
-			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
 
-			// If ground height is set, remove obstacles under it
-			if(maxGroundHeight != 0.0f)
+			// If ground height is set and if we label obstacles under it as ground
+			if(labelUndergroundObstaclesAsGround)
 			{
-				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+				float max = biggestSurfaceMax[2];
+				if(maxGroundHeight > 0)
+				{
+					max += maxGroundHeight;
 				}
 
+				pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+				pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits<int>::min(), max);
+				if(!underground->empty())
+				{
+					ground = util3d::concatenate(ground, underground);
+					notObstacles = util3d::concatenate(underground, notObstacles);
+				}
+			}
+
+			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+
 			//Cluster remaining stuff (obstacles)
 			if(otherStuffIndices->size())
+			{
+				if(minClusterSize>1)
 				{
 					std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
 							cloud,
@@ -191,6 +341,11 @@ void segmentObstaclesFromGround(
 					// merge indices
 					obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
 				}
+				else
+				{
+					obstacles = otherStuffIndices;
+				}
+			}
 		}
 	}
 }
@@ -208,7 +363,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -224,7 +380,8 @@ void segmentObstaclesFromGround(
 			maxGroundHeight,
 			flatObstacles,
 			viewPoint,
-			groundNormalsUp);
+			groundNormalsUp,
+			labelUndergroundObstaclesAsGround);
 }
 
 template<typename PointT>

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());

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

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

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

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

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

@@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class RTABMAP_EXP OdometryLOAM : public Odometry
+class RTABMAP_CORE_EXPORT OdometryLOAM : public Odometry
 {
 public:
 	OdometryLOAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());

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

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

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

@@ -36,7 +36,7 @@ namespace rtabmap {
 class Memory;
 class Feature2D;
 
-class RTABMAP_EXP OdometryMono : public Odometry
+class RTABMAP_CORE_EXPORT OdometryMono : public Odometry
 {
 public:
 	OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());

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

@@ -42,7 +42,7 @@ class ORBSLAMSystem;
 
 namespace rtabmap {
 
-class RTABMAP_EXP OdometryORBSLAM : public Odometry
+class RTABMAP_CORE_EXPORT OdometryORBSLAM : public Odometry
 {
 public:
 	OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());