Converted an assert to an error.

Optimizer: don't fix roll/pitch on root node if gravity constraints are fed
Working dir path: convert ~ to Home for convenience. Localization: don't show "cannot optimize" warning when RGBD/MaxOdomCacheSize=0
2026-09-13 23:10:20 +08:00 · 2021-12-26 14:53:22 -05:00 · 2021-12-25 16:57:03 -05:00 · 2021-12-24 18:24:44 -05:00 · 2021-12-20 17:01:08 -05:00 · 2021-12-18 18:53:59 -05:00
218 changed files with 5973 additions and 2076 deletions
@@ -0,0 +1,65 @@
+name: CMake
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-18.04]
+        include:
+        - os: ubuntu-20.04
+          ros_distro: 'noetic'
+        - os: ubuntu-18.04
+          ros_distro: 'melodic'
+
+    steps:
+    - uses: ros-tooling/setup-ros@v0.2
+      with:
+        required-ros-distributions: ${{ matrix.ros_distro }}
+    
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
+        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
+
+    - uses: actions/checkout@v2
+
+    - name: Configure CMake
+      # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
+      # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      # Build your program with the given configuration
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        ./rtabmap-console --version
+
+#    - name: Test
+#      working-directory: ${{github.workspace}}/build
+#      # Execute tests defined by the CMake configuration.  
+#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+#      run: ctest -C ${{env.BUILD_TYPE}}
+      
@@ -0,0 +1,90 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      matrix:
+        docker_tag: [xenial, bionic, focal, android23, android24, android26]
+        include:
+        - docker_tag: xenial
+          docker_tags: |
+            introlab3it/rtabmap:xenial
+            introlab3it/rtabmap:16.04
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'xenial'
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'bionic'
+        - docker_tag: focal
+          docker_tags: |
+            introlab3it/rtabmap:focal
+            introlab3it/rtabmap:20.04
+            introlab3it/rtabmap:latest
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api23'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api24'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api26'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v1
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v2
+        with:
+          context: ./docker/${{ matrix.docker_path }}
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          build-args: |
+            CACHE_DATE=${{ github.head_ref }}.${{ github.sha }}
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+
@@ -1,80 +0,0 @@
-language: cpp
-   
-jobs:
-  include:
-#    - name: osx
-#      compiler: clang
-#      os: osx
-#      install:
-#        - brew install sqlite
-#        - brew install pcl
-#        - brew install opencv@3
-    
-#    - name: linux-trusty
-#      compiler: gcc
-#      os: linux
-#      dist: trusty
-#      install:
-#        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-#        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-#        - sudo apt-get update
-#        - sudo apt-get update && sudo apt-get install dpkg
-#        - sudo apt-get -y install ros-indigo-rtabmap-ros
-#        - sudo apt-get -y remove ros-indigo-rtabmap
-#
-#      before_script: 
-#        - source /opt/ros/indigo/setup.bash
-
-    - name: linux-xenial
-      compiler: gcc
-      os: linux
-      dist: xenial
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get update && sudo apt-get install dpkg
-        - sudo apt-get -y install ros-kinetic-rtabmap-ros
-        - sudo apt-get -y remove ros-kinetic-rtabmap
-
-      before_script: 
-        - source /opt/ros/kinetic/setup.bash
-
-    - name: linux-bionic
-      compiler: gcc
-      os: linux
-      dist: bionic
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get update && sudo apt-get install dpkg
-        - sudo apt-get -y install ros-melodic-rtabmap-ros
-        - sudo apt-get -y remove ros-melodic-rtabmap
-
-      before_script: 
-        - source /opt/ros/melodic/setup.bash
-
-    - name: linux-focal
-      compiler: gcc
-      os: linux
-      dist: focal
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get update && sudo apt-get install dpkg
-        - sudo apt-get -y install ros-noetic-rtabmap-ros
-        - sudo apt-get -y remove ros-noetic-rtabmap
-
-      before_script: 
-        - source /opt/ros/noetic/setup.bash
-
-script:
-  - mkdir -p build && cd build
-  - cmake .. 
-  - make
-
-notifications:
-  email:
-    - matlabbe@gmail.com
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 13)
+SET(RTABMAP_PATCH_VERSION 16)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -123,9 +123,9 @@ OPTION( BUILD_SHARED_LIBS "Set to OFF to build static libraries" ON )


 ####### OUTPUT DIR #######
-SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
-SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
-SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin)
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin)
+SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib)

 # Avoid Visual Studio bin/Release and bin/Debug sub directories
 SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
@@ -181,12 +181,14 @@ option(WITH_DC1394        "Include dc1394 support"               ON)
 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"                ON)
+option(WITH_CERES         "Include Ceres support"                OFF)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
-option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_CVSBA         "Include cvsba support"                OFF)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
-option(WITH_CCCORELIB     "Include CCCoreLib support"            ON)
-option(WITH_LOAM          "Include LOAM support"                 ON)
+option(WITH_CCCORELIB     "Include CCCoreLib support"            OFF)
+option(WITH_OPEN3D        "Include Open3D support"               OFF)
+option(WITH_LOAM          "Include LOAM support"                 OFF)
+option(WITH_FLOAM         "Include FLOAM support"                OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
@@ -194,21 +196,22 @@ option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
-option(WITH_DEPTHAI       "Include depthai-core support"         ON)
+option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
-option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
-option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
+option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
+option(WITH_OPENCHISEL    "Include open_chisel support"          OFF)
 option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)
-option(WITH_FOVIS         "Include FOVIS support"                ON)
-option(WITH_VISO2         "Include VISO2 support"                ON)
-option(WITH_DVO           "Include DVO support"                  ON)
-option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" ON)
-option(WITH_OKVIS         "Include OKVIS support"                ON)
-option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"           OFF)
-option(WITH_VINS          "Include VINS-Fusion support"          ON)
-option(WITH_OPENVINS      "Include OpenVINS support"             ON)
+option(WITH_FOVIS         "Include FOVIS support"                OFF)
+option(WITH_VISO2         "Include VISO2 support"                OFF)
+option(WITH_DVO           "Include DVO support"                  OFF)
+option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" OFF)
+option(WITH_OKVIS         "Include OKVIS support"                OFF)
+option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
+option(WITH_VINS          "Include VINS-Fusion support"          OFF)
+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)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -253,7 +256,7 @@ endif()

 # OpenMP ("-fopenmp" should be added for flann included in PCL)
 # the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it
-if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang"))
+if(((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")) AND WITH_OPENMP)
  find_package(OpenMP COMPONENTS C CXX)
 endif()
 if(OPENMP_FOUND)
@@ -262,9 +265,10 @@ if(OPENMP_FOUND)
  set(CMAKE_INSTALL_OPENMP_LIBRARIES TRUE)
  message (STATUS "Found OpenMP: ${OpenMP_CXX_LIBRARIES}")
  if(PCL_OMP)
+    message (STATUS "Add PCL_OMP to definitions")
    add_definitions(-DPCL_OMP)
  endif(PCL_OMP)
-else(OPENMP_FOUND)
+elseif(WITH_OPENMP)
  message (STATUS "Not found OpenMP")
 endif()

@@ -486,6 +490,19 @@ IF(WITH_CCCORELIB)
    ENDIF(CCCoreLib_FOUND)
 ENDIF(WITH_CCCORELIB)

+IF(WITH_OPEN3D)
+    IF(${CMAKE_VERSION} VERSION_LESS "3.19.0") 
+       MESSAGE(WARNING "Open3D requires CMake version >=3.19 (current is ${CMAKE_VERSION})")
+    ELSE()
+       # Build Open3D like this to avoid linker errors in rtabmap:
+       # cmake -DBUILD_SHARED_LIBS=ON -DGLIBCXX_USE_CXX11_ABI=ON -DCMAKE_BUILD_TYPE=Release ..
+       find_package(Open3D QUIET)
+       IF(Open3D_FOUND)
+          MESSAGE(STATUS "Found Open3D: ${Open3DINCLUDE_DIRS}")
+       ENDIF(Open3D_FOUND)
+    ENDIF()
+ENDIF(WITH_OPEN3D)
+
 IF(WITH_LOAM)
    find_package(loam_velodyne QUIET)
    IF(loam_velodyne_FOUND)
@@ -493,6 +510,14 @@ IF(WITH_LOAM)
    ENDIF(loam_velodyne_FOUND)
 ENDIF(WITH_LOAM)

+IF(WITH_FLOAM)
+    find_package(floam QUIET)
+    IF(floam_FOUND)
+       MESSAGE(STATUS "Found floam: ${floam_INCLUDE_DIRS}")
+       FIND_PACKAGE(Ceres QUIET REQUIRED)
+    ENDIF(floam_FOUND)
+ENDIF(WITH_FLOAM)
+
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
    find_package(ZED 2 QUIET)
@@ -594,6 +619,7 @@ IF(WITH_ALICE_VISION)
        ENDIF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
        SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
        find_package(Geogram REQUIRED QUIET)
+        find_package(assimp QUIET)
        add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
        add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
        add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
@@ -635,9 +661,14 @@ IF(WITH_OKVIS)
 ENDIF(WITH_OKVIS)

 # If built with okvis, we found already ceres above
-IF(NOT okvis_FOUND AND WITH_CERES)
+IF(WITH_CERES)
+  IF(NOT okvis_FOUND AND NOT floam_FOUND)
    FIND_PACKAGE(Ceres QUIET)
-ENDIF(NOT okvis_FOUND AND WITH_CERES)
+    MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
+  ENDIF(NOT okvis_FOUND AND NOT floam_FOUND)
+ELSEIF(Ceres_FOUND)
+  MESSAGE(WARNING "WITH_CERES is OFF, but it still included by dependencies Okvis or FLOAM")
+ENDIF()

 IF(WITH_MSCKF_VIO)
    FIND_PACKAGE(msckf_vio QUIET)
@@ -678,7 +709,7 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)

 IF(NOT MSVC)
-	IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+	IF(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
 	 include(CheckCXXCompilerFlag)
 	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
@@ -688,7 +719,7 @@ IF(NOT MSVC)
 	  ELSE()
 	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
 	  ENDIF()
-	ENDIF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+	ENDIF(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( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
 	       G2O_FOUND OR
@@ -784,9 +815,9 @@ IF(NOT GTSAM_FOUND)
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
 ENDIF()
-IF(NOT CERES_FOUND)
+IF(NOT WITH_CERES OR NOT CERES_FOUND)
   SET(CERES "//")
-ENDIF(NOT CERES_FOUND)
+ENDIF(NOT WITH_CERES OR NOT CERES_FOUND)
 IF(NOT WITH_TORO)
   SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -804,6 +835,9 @@ ENDIF(NOT libpointmatcher_FOUND)
 IF(NOT CCCoreLib_FOUND)
   SET(CCCORELIB "//")
 ENDIF(NOT CCCoreLib_FOUND)
+IF(NOT Open3D_FOUND)
+   SET(OPEN3D "//")
+ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
   SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
@@ -813,6 +847,9 @@ ENDIF(NOT PDAL_FOUND)
 IF(NOT loam_velodyne_FOUND)
   SET(LOAM "//")
 ENDIF(NOT loam_velodyne_FOUND)
+IF(NOT floam_FOUND)
+   SET(FLOAM "//")
+ENDIF(NOT floam_FOUND)
 IF(NOT Freenect_FOUND)
   SET(FREENECT "//")
 ELSE()
@@ -875,8 +912,12 @@ IF(NOT mynteye_FOUND)
   SET(MYNTEYE "//")
 ENDIF(NOT mynteye_FOUND)
 IF(NOT depthai_FOUND)
+   SET(CONF_DEPTH_AI OFF)
   SET(DEPTHAI "//")
-ENDIF(NOT depthai_FOUND)
+ELSE()
+   SET(CONF_DEPTH_AI ON) 
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} depthai::core depthai::opencv) 
+ENDIF()
 IF(NOT octomap_FOUND)
   SET(OCTOMAP "//")
 ELSE()
@@ -941,7 +982,7 @@ ENDIF()
 IF(NOT TORCH_FOUND)
   SET(TORCH "//")
 ENDIF()
-IF(NOT Python3_FOUND)
+IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
   SET(PYTHON "//")
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
@@ -957,7 +998,7 @@ IF(NOT WITH_MADGWICK)
   SET(MADGWICK "//")
 ENDIF()

-CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
+CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)

 ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
@@ -1001,7 +1042,8 @@ file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_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_SOURCE_DIR}/corelib/include" 
+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}")
@@ -1212,7 +1254,7 @@ ELSE()
 MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
 ENDIF()

-IF(Python3_FOUND)
+IF(WITH_PYTHON AND Python3_FOUND)
 MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}            = YES (License: PSF)")
 ELSEIF(NOT WITH_PYTHON)
 MESSAGE(STATUS "  With Python3              = NO (WITH_PYTHON=OFF)")
@@ -1266,8 +1308,8 @@ ELSE()
 MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()

-IF(CERES_FOUND)
-MESSAGE(STATUS " *With Ceres                = YES (License: BSD)")
+IF(WITH_CERES AND CERES_FOUND)
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}          = YES (License: BSD)")
 ELSEIF(NOT WITH_CERES)
 MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
 ELSE()
@@ -1302,12 +1344,22 @@ ENDIF()

 IF(CCCoreLib_FOUND)
 MESSAGE(STATUS "  With CCCoreLib            = YES (License: GPLv2)")
-ELSEIF(NOT WITH_POINTMATCHER)
+ELSEIF(NOT WITH_CCCORELIB)
 MESSAGE(STATUS "  With CCCoreLib            = NO (WITH_CCCORELIB=OFF)")
 ELSE()
 MESSAGE(STATUS "  With CCCoreLib            = NO (CCCoreLib not found)")
 ENDIF()

+IF(Open3D_FOUND)
+MESSAGE(STATUS "  With Open3D               = YES (License: MIT)")
+ELSEIF(NOT WITH_OPEN3D)
+MESSAGE(STATUS "  With Open3D               = NO (WITH_OPEN3D=OFF)")
+ELSEIF(${CMAKE_VERSION} VERSION_LESS "3.19.0")
+MESSAGE(STATUS "  With Open3D               = NO (Open3D requires CMake>=3.19)")
+ELSE()
+MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)
@@ -1465,6 +1517,14 @@ ELSE()
 MESSAGE(STATUS "  With loam_velodyne        = NO (loam_velodyne not found)")
 ENDIF()

+IF(floam_FOUND)
+MESSAGE(STATUS "  With floam                = YES (License: BSD)")
+ELSEIF(NOT WITH_FLOAM)
+MESSAGE(STATUS "  With floam                = NO (WITH_FLOAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With floam                = NO (floam not found)")
+ENDIF()
+
 IF(libfovis_FOUND)
 MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
 ELSEIF(NOT WITH_FOVIS)
@@ -1534,12 +1594,12 @@ ENDIF()
 MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")
 MESSAGE(STATUS "--------------------------------------------")

-IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
+IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT WITH_CERES AND NOT CERES_FOUND)
 MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of these options:
       g2o (https://github.com/RainerKuemmerle/g2o)
       GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
       Ceres (http://ceres-solver.org)
       set -DWITH_TORO=ON")
-ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
+ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT WITH_CERES AND NOT CERES_FOUND)

 # vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :
@@ -1,13 +1,13 @@
-rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
+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]
 [![License][license-image]][license]
-Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
+Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)

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

 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
@@ -81,6 +81,9 @@ endif()
 if(@CONF_VTK_QT@ AND ${WITH_GUI})
   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
 endif(@CONF_VTK_QT@ AND ${WITH_GUI})
+if(@CONF_DEPTH_AI@)
+   FIND_PACKAGE(depthai 2 QUIET REQUIRED)
+endif(@CONF_DEPTH_AI@)
 SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")

 #backward compatibilities
@@ -52,9 +52,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@CVSBA@#define RTABMAP_CVSBA
@POINTMATCHER@#define RTABMAP_POINTMATCHER
@CCCORELIB@#define RTABMAP_CCCORELIB
+@OPEN3D@#define RTABMAP_OPEN3D
@FASTCV@#define RTABMAP_FASTCV
@PDAL@#define RTABMAP_PDAL
@LOAM@#define RTABMAP_LOAM
+@FLOAM@#define RTABMAP_FLOAM
@DC1394@#define RTABMAP_DC1394
@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
@ZED@#define RTABMAP_ZED
@@ -3,6 +3,7 @@ SET(INCLUDE_DIRS
    ${CMAKE_CURRENT_SOURCE_DIR}
    ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
    ${CMAKE_CURRENT_SOURCE_DIR}/third-party/include
+	${PROJECT_BINARY_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/utilite/include
    ${OpenCV_INCLUDE_DIRS}
@@ -448,7 +448,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			poses,
 			links,
 			true,
-			true,
+			false, // Make sure poses are the same than optimized mesh (in case we switched RGBD/OptimizedFromGraphEnd)
 			&signatures,
 			true,
 			true,
@@ -1399,7 +1399,14 @@ int RTABMapApp::Render()
                    }
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && landmark!=0)
 					{
-						main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        if(rejected)
+                        {
+                            main_scene_.setBackgroundColor(0.5, 0.325f, 0); // dark orange
+                        }
+                        else
+                        {
+                            main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        }
 					}
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && rejected>0)
 					{
@@ -594,6 +594,10 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+        else
+        {
+            trace_->ClearVertexArray();
+        }
 	}

 	if(gridVisible_ && !renderBackgroundCamera)
@@ -981,7 +981,7 @@
 				CLANG_CXX_LIBRARY = "libc++";
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 7;
+				CURRENT_PROJECT_VERSION = 8;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				EXCLUDED_ARCHS = "";
@@ -1006,7 +1006,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.20.16;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1038,7 +1038,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 7;
+				CURRENT_PROJECT_VERSION = 8;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				FRAMEWORK_SEARCH_PATHS = (
@@ -1063,7 +1063,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.20.16;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -55,9 +55,11 @@ cd $pwd

 # FLANN
 echo "wget flann..."
-curl -L http://www.cs.ubc.ca/research/flann/uploads/FLANN/flann-1.8.4-src.zip -o flann-1.8.4-src.zip
-unzip -qq flann-1.8.4-src.zip
-cd flann-1.8.4-src
+git clone https://github.com/flann-lib/flann.git
+cd flann
+git checkout 1.8.4
+curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
+git apply flann_ios.patch
 mkdir build
 cd build
 # comment "add_subdirectory( test )" in top CMakeLists.txt
@@ -66,7 +68,7 @@ cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_O
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
-#rm -r flann-1.8.4-src.zip flann-1.8.4-src
+#rm -r flann

 # GTSAM
 git clone https://bitbucket.org/gtborg/gtsam.git
@@ -134,6 +136,8 @@ cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.4.2
+curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
+git apply opencv_ios.patch
 mkdir build
 cd build
 # add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
@@ -145,6 +149,10 @@ cd $pwd

 mkdir rtabmap
 cd rtabmap
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../..
+cmake -DANDROID_PREBUILD=ON ../../../../..
+cmake --build . --config Release
+mkdir ios
+cd ios
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install
@@ -1340,6 +1340,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        rtabmap!.setMappingParameter(key: "Vis/FeatureType", value: defaults.string(forKey: "FeatureType")!);
        rtabmap!.setMappingParameter(key: "Mem/NotLinkedNodesKept", value: defaults.bool(forKey: "SaveAllFramesInDatabase") ? "true" : "false");
        rtabmap!.setMappingParameter(key: "RGBD/OptimizeFromGraphEnd", value: defaults.bool(forKey: "OptimizationfromGraphEnd") ? "true" : "false");
+        rtabmap!.setMappingParameter(key: "RGBD/MaxOdomCacheSize", value: defaults.string(forKey: "MaximumOdometryCacheSize")!);
        rtabmap!.setMappingParameter(key: "Optimizer/Strategy", value: defaults.string(forKey: "GraphOptimizer")!);
        rtabmap!.setMappingParameter(key: "RGBD/ProximityBySpace", value: defaults.string(forKey: "ProximityDetection")!);

@@ -552,6 +552,50 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
+        <dict>
+            <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>
+        </dict>
+        <dict>
+            <key>Type</key>
+            <string>PSMultiValueSpecifier</string>
+            <key>Title</key>
+            <string>Maximum odometry cache size</string>
+            <key>Key</key>
+            <string>MaximumOdometryCacheSize</string>
+            <key>DefaultValue</key>
+            <string>10</string>
+            <key>Titles</key>
+            <array>
+                <string>500</string>
+                <string>200</string>
+                <string>100</string>
+                <string>75</string>
+                <string>50</string>
+                <string>40</string>
+                <string>30</string>
+                <string>20</string>
+                <string>10</string>
+                <string>5</string>
+                <string>Disabled</string>
+            </array>
+            <key>Values</key>
+            <array>
+                <string>500</string>
+                <string>200</string>
+                <string>100</string>
+                <string>75</string>
+                <string>50</string>
+                <string>40</string>
+                <string>30</string>
+                <string>20</string>
+                <string>10</string>
+                <string>5</string>
+                <string>0</string>
+            </array>
+        </dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.12</string>
+			<string>0.20.16</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>
@@ -4,6 +4,7 @@ SET(SRC_FILES
 )

 SET(INCLUDE_DIRS
+    ${PROJECT_BINARY_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/guilib/include
@@ -1,5 +0,0 @@
-# Ignore everything in this directory
-*
-# Except this file
-!.gitignore
-!data
@@ -1 +0,0 @@
-/Version.h
@@ -133,7 +133,8 @@ std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
 		int to,
-		bool checkBothWays = true);
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
 std::multimap<int, int>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, int> & links,
 		int from,
@@ -228,6 +228,14 @@ public:
 	unsigned long getMemoryUsed() const; //Bytes

 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
+	int cleanupLocalGrids(
+			const std::map<int, Transform> & poses,
+			const cv::Mat & map,
+			float xMin,
+			float yMin,
+			float cellSize,
+			int cropRadius = 1,
+			bool filterScans = false);

 	//keypoint stuff
 	const VWDictionary * getVWDictionary() const;
@@ -58,7 +58,7 @@ public:
 	float getCellSize() const {return cellSize_;}
 	void setCloudAssembling(bool enabled);
 	float getMinMapSize() const {return minMapSize_;}
-	bool isGridFromDepth() const {return occupancyFromDepth_;}
+	bool isGridFromDepth() const {return occupancySensor_;}
 	bool isFullUpdate() const {return fullUpdate_;}
 	float getUpdateError() const {return updateError_;}
 	bool isMapFrameProjection() const {return projMapFrame_;}
@@ -81,7 +81,7 @@ public:
 			cv::Mat & groundCells,
 			cv::Mat & obstacleCells,
 			cv::Mat & emptyCells,
-			cv::Point3f & viewPoint) const;
+			cv::Point3f & viewPoint);

 	void createLocalMap(
 			const LaserScan & cloud,
@@ -118,7 +118,7 @@ private:
 	int scanDecimation_;
 	float cellSize_;
 	bool preVoxelFiltering_;
-	bool occupancyFromDepth_;
+	int occupancySensor_;
 	bool projMapFrame_;
 	float maxObstacleHeight_;
 	int normalKSearch_;
@@ -74,6 +74,8 @@ public:
 	void expandNode();
 	bool createChild(unsigned int i);

+	void updateOccupancyTypeChildren();
+
 private:
 	int nodeRefId_;
 	int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
@@ -54,7 +54,9 @@ public:
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
 		kTypeVINS = 9,
-		kTypeOpenVINS = 10
+		kTypeOpenVINS = 10,
+		kTypeFLOAM = 11,
+		kTypeOpen3D = 12
 	};

 public:
@@ -36,8 +36,8 @@ namespace rtabmap {
 std::string getPDALSupportedWriters();

 int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false, const std::vector<float> & intensities = std::vector<float>());
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false, const std::vector<float> & intensities = std::vector<float>());
 int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
 int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud,   const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);

@@ -202,7 +202,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Mem, ImageKept,                   bool, false,    "Keep raw images in RAM.");
    RTABMAP_PARAM(Mem, BinDataKept,                 bool, true,     "Keep binary data in db.");
    RTABMAP_PARAM(Mem, RawDescriptorsKept,          bool, true,     "Raw descriptors kept in memory.");
-    RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
+    RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labeled as \"map#\" where # is the map ID.");
    RTABMAP_PARAM(Mem, SaveDepth16Format,           bool, false,    "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
    RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
    RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
@@ -368,12 +368,12 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,    "Save scan matching IDs from one-to-many proximity detection in link's user data.");
    RTABMAP_PARAM(RGBD, NeighborLinkRefining,         bool, false,  uFormat("When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using registration approach selected (%s).", kRegStrategy().c_str()));
    RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess,     bool, false,  uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
-    RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes.");
+    RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes. Raw features are not saved in database.");
    RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
    RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for 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,  0,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false) and when %s!=0. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
+    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()));

    // Local/Proximity loop closure detection
    RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
@@ -432,7 +432,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");

    // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
    RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
    RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
@@ -535,6 +535,7 @@ class RTABMAP_EXP Parameters
    // Odometry LOAM
    RTABMAP_PARAM(OdomLOAM, Sensor,     int,    2,    "Velodyne sensor: 0=VLP-16, 1=HDL-32, 2=HDL-64E");
    RTABMAP_PARAM(OdomLOAM, ScanPeriod, float,  0.1,  "Scan period (s)");
+    RTABMAP_PARAM(OdomLOAM, Resolution, float,  0.2,   "Map resolution");
    RTABMAP_PARAM(OdomLOAM, LinVar,     float,  0.01,  "Linear output variance.");
    RTABMAP_PARAM(OdomLOAM, AngVar,     float,  0.01,  "Angular output variance.");
    RTABMAP_PARAM(OdomLOAM, LocalMapping, bool,  true,  "Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.");
@@ -571,6 +572,10 @@ class RTABMAP_EXP Parameters
    // Odometry VINS
    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");

+    // Odometry Open3D
+    RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
+    RTABMAP_PARAM(OdomOpen3D, Method,           int, 0,  "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");
+
    // Common registration parameters
    RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
    RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -722,15 +727,15 @@ class RTABMAP_EXP Parameters
 #endif

    // Occupancy Grid
-    RTABMAP_PARAM(Grid, FromDepth,               bool,   true,    "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
+    RTABMAP_PARAM(Grid, Sensor,                  int,    1,       "Create occupancy grid from selected sensor: 0=laser scan, 1=depth image(s) or 2=both laser scan and depth image(s).");
    RTABMAP_PARAM(Grid, DepthDecimation,         unsigned int,  4, uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
    RTABMAP_PARAM(Grid, RangeMin,                float,  0.0,     "Minimum range from sensor.");
    RTABMAP_PARAM(Grid, RangeMax,                float,  5.0,     "Maximum range from sensor. 0=inf.");
-    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));
+    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s>=1] Region of interest ratios [left, right, top, bottom].", kGridSensor().c_str()));
    RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
    RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
    RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
-    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=0 or 2] Decimation of the laser scan before creating cloud.", kGridSensor().c_str()));
    RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
    RTABMAP_PARAM(Grid, PreVoxelFiltering,       bool,   true,    uFormat("Input cloud is downsampled by voxel filter (voxel size is \"%s\") before doing segmentation of obstacles and ground.", kGridCellSize().c_str()));
    RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
@@ -744,9 +749,9 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
    RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
 #ifdef RTABMAP_OCTOMAP
-    RTABMAP_PARAM(Grid, 3D,                      bool,   true,    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 false.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, 3D,                      bool,   true,    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()));
 #else
-    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 false.", kGridFromDepth().c_str()));
+    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, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
@@ -829,7 +834,11 @@ public:
    static bool isFeatureParameter(const std::string & param);
    static ParametersMap getDefaultOdometryParameters(bool stereo = false, bool vis = true, bool icp = false);
    static ParametersMap getDefaultParameters(const std::string & group);
-    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);
+    /**
+     * If remove=false: keep only parameters of the specified group.
+     * If remove=true: remove parameters of the specified group.
+     */
+    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);

    static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
    static void writeINI(const std::string & configFile, const ParametersMap & parameters);
@@ -206,6 +206,19 @@ public:
 			bool interSession = true,
 			const ProgressState * state = 0,
 			float clusterRadiusMin = 0.0f);
+	bool globalBundleAdjustment(
+			int optimizerType = 1 /*g2o*/,
+			bool rematchFeatures = true,
+			int iterations = 0,
+			float pixelVariance = 0.0f);
+	int cleanupLocalGrids(
+			const std::map<int, Transform> & mapPoses,
+			const cv::Mat & map,
+			float xMin,
+			float yMin,
+			float cellSize,
+			int cropRadius = 1,
+			bool filterScans = false);
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
@@ -348,7 +361,6 @@ private:
 	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::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link
 	std::vector<float> _odomCorrectionAcc;

 	// Planning stuff
@@ -147,6 +147,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_id,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
+	RTABMAP_STATS(Memory, Odom_cache_poses,);
+	RTABMAP_STATS(Memory, Odom_cache_links,);
 	RTABMAP_STATS(Memory, Small_movement,);
 	RTABMAP_STATS(Memory, Fast_movement,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
@@ -254,6 +256,8 @@ public:
 	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 	void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
 	void setWmState(const std::vector<int> & state) {_wmState = state;}
+	void setOdomCachePoses(const std::map<int, Transform> & poses) {_odomCachePoses = poses;}
+	void setOdomCacheConstraints(const std::multimap<int, Link> & constraints) {_odomCacheConstraints = constraints;}

 	// getters
 	bool extended() const {return _extended;}
@@ -281,6 +285,8 @@ public:
 	int currentGoalId() const {return _currentGoalId;}
 	const std::map<int, int> & reducedIds() const {return _reducedIds;}
 	const std::vector<int> & wmState() const {return _wmState;}
+	const std::map<int, Transform> & odomCachePoses() const {return _odomCachePoses;}
+	const std::multimap<int, Link> & odomCacheConstraints() const {return _odomCacheConstraints;}

 	const std::map<std::string, float> & data() const {return _data;}

@@ -316,6 +322,9 @@ private:

 	std::vector<int> _wmState;

+	std::map<int, Transform> _odomCachePoses;
+	std::multimap<int, Link> _odomCacheConstraints;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}
@@ -69,6 +69,7 @@ protected:
 private:
 #ifdef RTABMAP_DEPTHAI
 	StereoCameraModel stereoModel_;
+	Transform imuLocalTransform_;
 	std::string deviceSerial_;
 	bool outputDepth_;
 	int depthConfidence_;
@@ -76,6 +77,9 @@ private:
 	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_;
 #endif
 };

@@ -68,6 +68,7 @@ public:
 	bool setMirroring(bool enabled);
 	void setOpenNI2StampsAndIDsUsed(bool used);
 	void setIRDepthShift(int horizontal, int vertical);
+	void setDepthDecimation(int decimation);

 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -85,6 +86,7 @@ private:
 	StereoCameraModel _stereoModel;
 	int _depthHShift;
 	int _depthVShift;
+	int _depthDecimation;
 #endif
 };

@@ -89,7 +89,7 @@ public:
 	void setJsonConfig(const std::string & json);
 	// T265 related parameters
 	void setImagesRectified(bool enabled);
-	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false);
+	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false, bool onlyLeftStream = false);

 #ifdef RTABMAP_REALSENSE2
 private:
@@ -116,8 +116,6 @@ private:
 	std::string deviceId_;
 	rs2::syncer syncer_;
 	float depth_scale_meters_;
-	rs2_intrinsics depthIntrinsics_;
-	rs2_intrinsics rgbIntrinsics_;
 	cv::Mat depthBuffer_;
 	cv::Mat rgbBuffer_;
 	CameraModel model_;
@@ -138,6 +136,7 @@ private:
 	bool rectifyImages_;
 	bool odometryProvided_;
 	bool odometryImagesDisabled_;
+	bool odometryOnlyLeftStream_;
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;
@@ -0,0 +1,64 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYFLOAM_H_
+#define ODOMETRYFLOAM_H_
+
+#include <rtabmap/core/Odometry.h>
+
+class LaserProcessingClass;
+class OdomEstimationClass;
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryFLOAM : public Odometry
+{
+public:
+	OdometryFLOAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryFLOAM();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeFLOAM;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_FLOAM
+	LaserProcessingClass * laserProcessing_;
+	OdomEstimationClass * odomEstimation_;
+
+	Transform lastPose_;
+	bool lost_;
+	float linVar_;
+	float angVar_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYFLOAM_H_ */
@@ -0,0 +1,59 @@
+/*
+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 ODOMETRYOPEN3D_H_
+#define ODOMETRYOPEN3D_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryOpen3D : public Odometry
+{
+public:
+	OdometryOpen3D(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryOpen3D();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeOpen3D;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_OPEN3D
+	rtabmap::SensorData keyFrame_;
+	Transform lastKeyFramePose_;
+	int method_;
+	float maxDepth_;
+	float keyFrameThr_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYOPEN3D_H_ */
@@ -253,7 +253,7 @@ cv::Mat RTABMAP_EXP mergeTextures(

 void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh);

-bool RTABMAP_EXP multiBandTexturing(
+RTABMAP_DEPRECATED(bool RTABMAP_EXP multiBandTexturing(
 		const std::string & outputOBJPath,
 		const pcl::PCLPointCloud2 & cloud,
 		const std::vector<pcl::Vertices> & polygons,
@@ -268,7 +268,58 @@ bool RTABMAP_EXP multiBandTexturing(
 		const std::map<int, std::map<int, cv::Vec4d> > & gains = std::map<int, std::map<int, cv::Vec4d> >(),       // optional output of util3d::mergeTextures()
 		const std::map<int, std::map<int, cv::Mat> > & blendingGains = std::map<int, std::map<int, cv::Mat> >(),   // optional output of util3d::mergeTextures()
 		const std::pair<float, float> & contrastValues = std::pair<float, float>(0,0),                             // optional output of util3d::mergeTextures()
-		bool gainRGB = true);
+		bool gainRGB = true), "Use the same method with 22 parameters instead.");
+
+/**
+ * Texture mesh with AliceVision's multiband texturing approach. See also https://meshroom-manual.readthedocs.io/en/bibtex1/node-reference/nodes/Texturing.html.
+ * @param outputOBJPath Output OBJ path
+ * @param cloud input Cloud of the mesh.
+ * @param polygons Input polygons of the mesh.
+ * @param cameraPoses Poses of the cameras.
+ * @param vertexToPixels Output from {@link #createTextureMesh()}.
+ * @param images Images corresponding to cameraPoses, raw or compressed, can be empty if memory or dbDriver should be used.
+ * @param cameraModels Camera calibrations corresponding to cameraPoses.
+ * @param memory Should be set if images and dbDriver are not set.
+ * @param dbDriver Should be set if images and memory are not set.
+ * @param textureSize Output texture size 1024, 2048, 4096, 8192, 16384.
+ * @param textureDownscale Downscaling to 4 or 8 will reduce the texture quality but speed up the computation time. Set Texture Downscale to 1 instead of 2 to get the maximum possible resolution with the resolution of your images. The output texture size will be divided by this value, e.g., with texture size of 8192 and downscale value of 2, the output will be 4096.
+ * @param nbContrib number of contributions per frequency band for the multi-band blending (should be 4 values)
+ * @param textureFormat Output texture format: "png" or "jpg".
+ * @param gains Optional output of {@link #mergeTextures()}.
+ * @param blendingGains Optional output of {@link #mergeTextures()}.
+ * @param contrastValues Optional output of {@link #mergeTextures()}.
+ * @param gainRGB Apply gain compensation on each RGB channels separately, otherwise it is apply equally to all channels.
+ * @param unwrapMethod Method to unwrap input mesh if it does not have UV coordinates 0=Basic (> 600k faces) fast and simple. Can generate multiple atlases 2=LSCM (<= 600k faces): optimize space. Generates one atlas 1=ABF (<= 300k faces): optimize space and stretch. Generates one atlas.
+ * @param fillHoles Fill Texture holes with plausible values True/False.
+ * @param padding Texture edge padding size in pixel (0-100).
+ * @param bestScoreThreshold 0.0 to disable filtering based on threshold to relative best score (0.0-1.0).
+ * @param angleHardThreshold 0.0 to disable angle hard threshold filtering (0.0, 180.0).
+ * @param forceVisibleByAllVertices Triangle visibility is based on the union of vertices visibility.
+ */
+bool RTABMAP_EXP multiBandTexturing(
+		const std::string & outputOBJPath,
+		const pcl::PCLPointCloud2 & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const std::map<int, Transform> & cameraPoses,
+		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
+		const std::map<int, cv::Mat> & images,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		const Memory * memory = 0,
+		const DBDriver * dbDriver = 0,
+		unsigned int textureSize = 8192,
+		unsigned int textureDownscale = 2,
+		const std::string & nbContrib = "1 5 10 0",
+		const std::string & textureFormat = "jpg",
+		const std::map<int, std::map<int, cv::Vec4d> > & gains = std::map<int, std::map<int, cv::Vec4d> >(),
+		const std::map<int, std::map<int, cv::Mat> > & blendingGains = std::map<int, std::map<int, cv::Mat> >(),
+		const std::pair<float, float> & contrastValues = std::pair<float, float>(0,0),
+		bool gainRGB = true,
+		unsigned int unwrapMethod = 0,
+		bool fillHoles = false,
+		unsigned int padding = 5,
+		double bestScoreThreshold = 0.1,
+		double angleHardThreshold = 90.0,
+		bool forceVisibleByAllVertices = false);

 cv::Mat RTABMAP_EXP computeNormals(
 		const cv::Mat & laserScan,
@@ -89,9 +89,11 @@ SET(SRC_FILES
    odometry/OdometryOkvis.cpp
    odometry/OdometryORBSLAM.cpp
    odometry/OdometryLOAM.cpp
+    odometry/OdometryFLOAM.cpp
    odometry/OdometryMSCKF.cpp
    odometry/OdometryVINS.cpp
    odometry/OdometryOpenVINS.cpp
+    odometry/OdometryOpen3D.cpp
    
    IMU.cpp
    IMUThread.cpp
@@ -141,6 +143,7 @@ IF(MSVC)
 ENDIF(MSVC)

 SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_BINARY_DIR}/../include
    ${PROJECT_SOURCE_DIR}/utilite/include
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 	${CMAKE_CURRENT_SOURCE_DIR}
@@ -192,7 +195,7 @@ IF(TORCH_FOUND)
 	)
 ENDIF(TORCH_FOUND)

-IF(Python3_FOUND)
+IF(WITH_PYTHON AND Python3_FOUND)
 	SET(LIBRARIES
 		${LIBRARIES}
 		Python3::Python
@@ -208,7 +211,7 @@ IF(Python3_FOUND)
 	    ${CMAKE_CURRENT_SOURCE_DIR}/python
 		${INCLUDE_DIRS}
 	)
-ENDIF(Python3_FOUND)
+ENDIF(WITH_PYTHON AND Python3_FOUND)



@@ -344,8 +347,8 @@ ENDIF(mynteye_FOUND)
 IF(depthai_FOUND)
 	SET(LIBRARIES
 		${LIBRARIES}
-		depthai::depthai-core
-		depthai::depthai-opencv
+		depthai::core
+		depthai::opencv
 	)
 ENDIF(depthai_FOUND)

@@ -419,7 +422,7 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)

-IF(CERES_FOUND)
+IF(WITH_CERES AND CERES_FOUND)
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
 		${CERES_INCLUDE_DIRS}
@@ -428,7 +431,7 @@ IF(CERES_FOUND)
 		${LIBRARIES}
 		${CERES_LIBRARIES}
 	)
-ENDIF(CERES_FOUND)
+ENDIF(WITH_CERES AND CERES_FOUND)

 IF(libpointmatcher_FOUND)
 	SET(INCLUDE_DIRS 
@@ -448,6 +451,13 @@ IF(CCCoreLib_FOUND)
 	)
 ENDIF(CCCoreLib_FOUND)

+IF(Open3D_FOUND)
+    SET(LIBRARIES
+        ${LIBRARIES}
+        Open3D::Open3D
+    )
+ENDIF(Open3D_FOUND)
+
 IF(FastCV_FOUND)
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
@@ -489,6 +499,17 @@ IF(loam_velodyne_FOUND)
 	)
 ENDIF(loam_velodyne_FOUND)

+IF(floam_FOUND)
+    SET(INCLUDE_DIRS 
+        ${INCLUDE_DIRS} 
+        ${floam_INCLUDE_DIRS}
+    )
+    SET(LIBRARIES
+        ${LIBRARIES}
+        ${floam_LIBRARIES}
+    )
+ENDIF(floam_FOUND)
+
 IF(ZED_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -697,10 +718,10 @@ endforeach(arg ${RESOURCES})
 #MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
 #MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")

-IF(ANDROID)
+IF(ANDROID OR IOS)

   IF(NOT RTABMAP_RES_TOOL)
-      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
+      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${PROJECT_BINARY_DIR}/../bin)
      IF(NOT RTABMAP_RES_TOOL)
         MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
      ENDIF(NOT RTABMAP_RES_TOOL)
@@ -754,3 +775,10 @@ install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/
 		FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 
 		PATTERN ".svn" EXCLUDE)
 		
+# For generated Version.h
+install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/../include/ 
+        DESTINATION "${INSTALL_INCLUDE_DIR}"
+        COMPONENT devel 
+        FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 
+        PATTERN ".svn" EXCLUDE)
+
@@ -99,7 +99,7 @@ SensorData Camera::takeImage(CameraInfo * info)
 	}

 	UTimer timer;
-	SensorData data  = this->captureImage(info);
+	SensorData data = this->captureImage(info);
 	double captureTime = timer.ticks();
 	if(warnFrameRateTooHigh)
 	{
@@ -350,7 +350,7 @@ bool CameraModel::load(const std::string & filePath)
 		}
 		catch(const cv::Exception & e)
 		{
-			UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
+			UERROR("Error reading calibration file \"%s\": %s (Make sure the first line of the yaml file is \"%YAML:1.0\")", filePath.c_str(), e.what());
 		}
 	}
 	else
@@ -365,8 +365,10 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 	if(_distortionModel && !data.depthRaw().empty())
 	{
 		UTimer timer;
-		if(_distortionModel->getWidth() == data.depthRaw().cols &&
-		   _distortionModel->getHeight() == data.depthRaw().rows	)
+		if(_distortionModel->getWidth() >= data.depthRaw().cols &&
+		   _distortionModel->getHeight() >= data.depthRaw().rows &&
+		   _distortionModel->getWidth() % data.depthRaw().cols == 0 &&
+		   _distortionModel->getHeight() % data.depthRaw().rows == 0)
 		{
 			cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
 			_distortionModel->undistort(depth);
@@ -374,7 +376,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 		else
 		{
-			UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
+			UERROR("Distortion model size is %dx%d but depth image is %dx%d!",
 					_distortionModel->getWidth(), _distortionModel->getHeight(),
 					data.depthRaw().cols, data.depthRaw().rows);
 		}
@@ -878,22 +878,22 @@ long DBDriverSqlite3::getFeaturesMemoryUsedQuery() const
 		std::string query;
 		if(uStrNumCmp(_version, "0.13.0") >= 0)
 		{
-			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) + ifnull(length(descriptor_size),0) + ifnull(length(descriptor),0)) "
 					 "FROM Feature";
 		}
 		else if(uStrNumCmp(_version, "0.12.0") >= 0)
 		{
-			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) + ifnull(length(descriptor_size),0) + ifnull(length(descriptor),0)) "
 					 "FROM Map_Node_Word";
 		}
 		else if(uStrNumCmp(_version, "0.11.2") >= 0)
 		{
-			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) + ifnull(length(descriptor_size),0) + ifnull(length(descriptor),0)) "
 					 "FROM Map_Node_Word";
 		}
 		else
 		{
-			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + ifnull(length(depth_x),0) + ifnull(length(depth_y),0) + ifnull(length(depth_z),0) "
 					 "FROM Map_Node_Word";
 		}

@@ -2089,7 +2089,18 @@ std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat &
 {
 #ifdef RTABMAP_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	UASSERT_MSG(roi.x==0 && roi.y ==0, "Not supporting ROI");
+	if(roi.x!=0 || roi.y !=0)
+	{
+		UERROR("SuperPoint: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
+				roi.x, roi.y, roi.width, roi.height,
+				Parameters::kKpRoiRatios().c_str(),
+				Parameters::kVisRoiRatios().c_str(),
+				Parameters::kVisGridRows().c_str(),
+				Parameters::kVisGridCols().c_str(),
+				Parameters::kKpGridRows().c_str(),
+				Parameters::kKpGridCols().c_str());
+		return std::vector<cv::KeyPoint>();
+	}
 	return superPoint_->detect(image, mask);
 #else
 	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
@@ -989,12 +989,13 @@ std::multimap<int, Link>::iterator findLink(
 		std::multimap<int, Link> & links,
 		int from,
 		int to,
-		bool checkBothWays)
+		bool checkBothWays,
+		Link::Type type)
 {
 	std::multimap<int, Link>::iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
-		if(iter->second.to() == to)
+		if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
 		{
 			return iter;
 		}
@@ -1007,7 +1008,7 @@ std::multimap<int, Link>::iterator findLink(
 		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
-			if(iter->second.to() == from)
+			if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
 			{
 				return iter;
 			}
@@ -2066,10 +2066,11 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 		bool ok = true;
 		std::map<int, Transform> poses = _dbDriver->loadOptimizedPoses(lastlocalizationPose);
 		// Make sure optimized poses match the working directory! Otherwise return nothing.
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && ok; ++iter)
+		for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end() && ok; ++iter)
 		{
 			if(_workingMem.find(iter->first)==_workingMem.end())
 			{
+				UWARN("Node %d not found in working memory", iter->first);
 				ok = false;
 			}
 		}
@@ -2080,7 +2081,7 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 				  "poses to force re-update. If you want to use the "
 				  "saved optimized poses, set %s to true",
 				  (int)poses.size(),
-				  (int)_workingMem.size(),
+				  (int)_workingMem.size()-1, // less virtual place
 				  Parameters::kMemInitWMWithAllNodes().c_str());
 			return std::map<int, Transform>();
 		}
@@ -2731,13 +2732,13 @@ Transform Memory::computeTransform(

 	// make sure we have all data needed
 	// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
-	if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
+	if(((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) && fromS.sensorData().imageCompressed().empty()) ||
 	   (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
 	{
 		fromS.sensorData() = getNodeData(fromS.id(), true, true, true, true);
 	}
-	if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
+	if(((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) && toS.sensorData().imageCompressed().empty()) ||
 	   (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
 	{
@@ -2747,27 +2748,27 @@ Transform Memory::computeTransform(
 	cv::Mat imgBuf, depthBuf, userBuf;
 	LaserScan laserBuf;
 	fromS.sensorData().uncompressData(
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&imgBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&depthBuf:0,
 			_registrationPipeline->isScanRequired()?&laserBuf:0,
 			_registrationPipeline->isUserDataRequired()?&userBuf:0);
 	toS.sensorData().uncompressData(
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&imgBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&depthBuf:0,
 			_registrationPipeline->isScanRequired()?&laserBuf:0,
 			_registrationPipeline->isUserDataRequired()?&userBuf:0);


 	// compute transform fromId -> toId
 	std::vector<int> inliersV;
-	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
+	if((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) ||
 		(fromS.getWords().size() && toS.getWords().size()) ||
 		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
 	{
 		Signature tmpFrom = fromS;
 		Signature tmpTo = toS;

-		if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
+		if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
 		{
 			UDEBUG("");
 			tmpFrom.removeAllWords();
@@ -4050,6 +4051,221 @@ void Memory::generateGraph(const std::string & fileName, const std::set<int> & i
 	_dbDriver->generateGraph(fileName, ids, _signatures);
 }

+int Memory::cleanupLocalGrids(
+		const std::map<int, Transform> & poses,
+		const cv::Mat & map,
+		float xMin,
+		float yMin,
+		float cellSize,
+		int cropRadius,
+		bool filterScans)
+{
+	if(!_dbDriver)
+	{
+		UERROR("A database must be loaded first...");
+		return -1;
+	}
+
+	if(poses.empty() || poses.lower_bound(1) == poses.end())
+	{
+		UERROR("Empty poses?!");
+		return -1;
+	}
+	if(map.empty())
+	{
+		UERROR("Map is empty!");
+		return -1;
+	}
+	UASSERT(cropRadius>=0);
+	UASSERT(cellSize>0.0f);
+
+	int maxPoses = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
+	{
+		++maxPoses;
+	}
+
+	UINFO("Processing %d grids...", maxPoses);
+	int processedGrids = 1;
+	int gridsScansModified = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter, ++processedGrids)
+	{
+		// local grid
+		cv::Mat gridGround;
+		cv::Mat gridObstacles;
+		cv::Mat gridEmpty;
+
+		// scan
+		SensorData data = this->getNodeData(iter->first, false, true, false, true);
+		LaserScan scan;
+		data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
+
+		if(!gridObstacles.empty())
+		{
+			UASSERT(data.gridCellSize() == cellSize);
+			cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
+			int oi = 0;
+			for(int i=0; i<gridObstacles.cols; ++i)
+			{
+				const float * ptr = gridObstacles.ptr<float>(0, i);
+				cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
+				pt = util3d::transformPoint(pt, iter->second);
+
+				int x = int((pt.x - xMin) / cellSize + 0.5f);
+				int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+				if(x>=0 && x<map.cols &&
+				   y>=0 && y<map.rows)
+				{
+					bool obstacleDetected = false;
+
+					for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+					{
+						for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+						{
+							if(x+j>=0 && x+j<map.cols &&
+							   y+k>=0 && y+k<map.rows &&
+							   map.at<unsigned char>(y+k,x+j) == 100)
+							{
+								obstacleDetected = true;
+							}
+						}
+					}
+
+					if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
+					{
+						// Verify that we don't have an obstacle on neighbor cells
+						cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+						++oi;
+					}
+				}
+			}
+
+			if(oi != gridObstacles.cols)
+			{
+				UINFO("Grid id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, gridObstacles.cols, oi);
+				gridsScansModified += 1;
+
+				// update
+				Signature * s = this->_getSignature(iter->first);
+				cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
+				bool modifyDb = true;
+				if(s)
+				{
+					s->sensorData().setOccupancyGrid(gridGround, newObstacles, gridEmpty, cellSize, data.gridViewPoint());
+					if(!s->isSaved())
+					{
+						// not saved in database yet
+						modifyDb = false;
+					}
+				}
+				if(modifyDb)
+				{
+					_dbDriver->updateOccupancyGrid(iter->first,
+							gridGround,
+							newObstacles,
+							gridEmpty,
+							cellSize,
+							data.gridViewPoint());
+				}
+			}
+		}
+
+		if(filterScans && !scan.isEmpty())
+		{
+			Transform mapToScan = iter->second * scan.localTransform();
+
+			cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
+			int oi = 0;
+			for(int i=0; i<scan.size(); ++i)
+			{
+				const float * ptr = scan.data().ptr<float>(0, i);
+				cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
+				pt = util3d::transformPoint(pt, mapToScan);
+
+				int x = int((pt.x - xMin) / cellSize + 0.5f);
+				int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+				if(x>=0 && x<map.cols &&
+				   y>=0 && y<map.rows)
+				{
+					bool obstacleDetected = false;
+
+					for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+					{
+						for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+						{
+							if(x+j>=0 && x+j<map.cols &&
+							   y+k>=0 && y+k<map.rows &&
+							   map.at<unsigned char>(y+k,x+j) == 100)
+							{
+								obstacleDetected = true;
+							}
+						}
+					}
+
+					if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
+					{
+						// Verify that we don't have an obstacle on neighbor cells
+						cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+						++oi;
+					}
+				}
+			}
+
+			if(oi != scan.size())
+			{
+				UINFO("Scan id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, (int)scan.size(), oi);
+				gridsScansModified += 1;
+
+				// update
+				if(scan.angleIncrement()!=0)
+				{
+					// copy meta data
+					scan = LaserScan(
+							cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+							scan.format(),
+							scan.rangeMin(),
+							scan.rangeMax(),
+							scan.angleMin(),
+							scan.angleMax(),
+							scan.angleIncrement(),
+							scan.localTransform());
+				}
+				else
+				{
+					// copy meta data
+					scan = LaserScan(
+							cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+							scan.maxPoints(),
+							scan.rangeMax(),
+							scan.format(),
+							scan.localTransform());
+				}
+
+				// update
+				Signature * s = this->_getSignature(iter->first);
+				bool modifyDb = true;
+				if(s)
+				{
+					s->sensorData().setLaserScan(scan, true);
+					if(!s->isSaved())
+					{
+						// not saved in database yet
+						modifyDb = false;
+					}
+				}
+				if(modifyDb)
+				{
+					_dbDriver->updateLaserScan(iter->first, scan);
+				}
+
+			}
+		}
+	}
+	return gridsScansModified;
+}
+
 int Memory::getNi(int signatureId) const
 {
 	int ni = 0;
@@ -4450,6 +4666,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				if(!imagesRectified && decimatedData.cameraModels().size())
 				{
+					UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
 					std::vector<cv::KeyPoint> keypointsValid;
 					keypointsValid.reserve(keypoints.size());
 					cv::Mat descriptorsValid;
@@ -4643,6 +4860,144 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 		}
+		else if(!imagesRectified && !data.cameraModels().empty())
+		{
+			std::vector<cv::KeyPoint> keypointsValid;
+			keypointsValid.reserve(keypoints.size());
+			cv::Mat descriptorsValid;
+			descriptorsValid.reserve(descriptors.rows);
+			std::vector<cv::Point3f> keypoints3DValid;
+			keypoints3DValid.reserve(keypoints3D.size());
+
+			//undistort keypoints before projection (RGB-D)
+			if(data.cameraModels().size() == 1)
+			{
+				std::vector<cv::Point2f> pointsIn, pointsOut;
+				cv::KeyPoint::convert(keypoints,pointsIn);
+				if(data.cameraModels()[0].D_raw().cols == 6)
+				{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+					// Equidistant / FishEye
+					// get only k parameters (k1,k2,p1,p2,k3,k4)
+					cv::Mat D(1, 4, CV_64FC1);
+					D.at<double>(0,0) = data.cameraModels()[0].D_raw().at<double>(0,0);
+					D.at<double>(0,1) = data.cameraModels()[0].D_raw().at<double>(0,1);
+					D.at<double>(0,2) = data.cameraModels()[0].D_raw().at<double>(0,4);
+					D.at<double>(0,3) = data.cameraModels()[0].D_raw().at<double>(0,5);
+					cv::fisheye::undistortPoints(pointsIn, pointsOut,
+							data.cameraModels()[0].K_raw(),
+							D,
+							data.cameraModels()[0].R(),
+							data.cameraModels()[0].P());
+				}
+				else
+#else
+					UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+							CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+				}
+#endif
+				{
+					//RadialTangential
+					cv::undistortPoints(pointsIn, pointsOut,
+							data.cameraModels()[0].K_raw(),
+							data.cameraModels()[0].D_raw(),
+							data.cameraModels()[0].R(),
+							data.cameraModels()[0].P());
+				}
+				UASSERT(pointsOut.size() == keypoints.size());
+				for(unsigned int i=0; i<pointsOut.size(); ++i)
+				{
+					if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<data.cameraModels()[0].imageWidth() &&
+					   pointsOut.at(i).y>=0 && pointsOut.at(i).y<data.cameraModels()[0].imageHeight())
+					{
+						keypointsValid.push_back(keypoints.at(i));
+						keypointsValid.back().pt.x = pointsOut.at(i).x;
+						keypointsValid.back().pt.y = pointsOut.at(i).y;
+						descriptorsValid.push_back(descriptors.row(i));
+						if(!keypoints3D.empty())
+						{
+							keypoints3DValid.push_back(keypoints3D.at(i));
+						}
+					}
+				}
+			}
+			else
+			{
+				float subImageWidth;
+				if(!data.imageRaw().empty())
+				{
+					UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+					subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+				}
+				else
+				{
+					UASSERT(data.cameraModels()[0].imageWidth()>0);
+					subImageWidth = data.cameraModels()[0].imageWidth();
+				}
+
+				for(unsigned int i=0; i<keypoints.size(); ++i)
+				{
+					int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+					UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)data.cameraModels().size(),
+							uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+									cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str());
+
+					std::vector<cv::Point2f> pointsIn, pointsOut;
+					pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+					if(data.cameraModels()[cameraIndex].D_raw().cols == 6)
+					{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+						// Equidistant / FishEye
+						// get only k parameters (k1,k2,p1,p2,k3,k4)
+						cv::Mat D(1, 4, CV_64FC1);
+						D.at<double>(0,0) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
+						D.at<double>(0,1) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+						D.at<double>(0,2) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+						D.at<double>(0,3) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+						cv::fisheye::undistortPoints(pointsIn, pointsOut,
+								data.cameraModels()[cameraIndex].K_raw(),
+								D,
+								data.cameraModels()[cameraIndex].R(),
+								data.cameraModels()[cameraIndex].P());
+					}
+					else
+#else
+						UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+								CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+					}
+#endif
+					{
+						//RadialTangential
+						cv::undistortPoints(pointsIn, pointsOut,
+								data.cameraModels()[cameraIndex].K_raw(),
+								data.cameraModels()[cameraIndex].D_raw(),
+								data.cameraModels()[cameraIndex].R(),
+								data.cameraModels()[cameraIndex].P());
+					}
+
+					if(pointsOut[0].x>=0 && pointsOut[0].x<data.cameraModels()[cameraIndex].imageWidth() &&
+					   pointsOut[0].y>=0 && pointsOut[0].y<data.cameraModels()[cameraIndex].imageHeight())
+					{
+						keypointsValid.push_back(keypoints.at(i));
+						keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+						keypointsValid.back().pt.y = pointsOut[0].y;
+						descriptorsValid.push_back(descriptors.row(i));
+						if(!keypoints3D.empty())
+						{
+							keypoints3DValid.push_back(keypoints3D.at(i));
+						}
+					}
+				}
+			}
+
+			keypoints = keypointsValid;
+			descriptors = descriptorsValid;
+			keypoints3D = keypoints3DValid;
+
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+			UDEBUG("time rectification = %fs", t);
+		}
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
 		UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
@@ -5285,7 +5640,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData));
 	}

-	s->setWords(words, wordsKpts, words3D, wordsDescriptors);
+	s->setWords(words, wordsKpts,
+			_reextractLoopClosureFeatures?std::vector<cv::Point3f>():words3D,
+			_reextractLoopClosureFeatures?cv::Mat():wordsDescriptors);

 	// set raw data
 	if(!cameraModels.empty())
@@ -5430,7 +5787,24 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
                }
                
            }
-			Link landmark(s->id(), landmarkId, Link::kLandmark, iter->second.pose(), iter->second.covariance().inv(), landmarkSize);
+
+			Transform landmarkPose = iter->second.pose();
+			if(_registrationPipeline->force3DoF())
+			{
+				// For 2D slam, make sure the landmark z axis is up
+				rtabmap::Transform tx = landmarkPose.rotation() * rtabmap::Transform(1,0,0,0,0,0);
+				rtabmap::Transform ty = landmarkPose.rotation() * rtabmap::Transform(0,1,0,0,0,0);
+				if(fabs(tx.z()) > 0.9)
+				{
+					landmarkPose*=rtabmap::Transform(0,0,0,0,(tx.z()>0?1:-1)*M_PI/2,0);
+				}
+				else if(fabs(ty.z()) > 0.9)
+				{
+					landmarkPose*=rtabmap::Transform(0,0,0,(ty.z()>0?-1:1)*M_PI/2,0,0);
+				}
+			}
+
+			Link landmark(s->id(), landmarkId, Link::kLandmark, landmarkPose, iter->second.covariance().inv(), landmarkSize);
 			s->addLandmark(landmark);

 			// Update landmark index
@@ -52,7 +52,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	scanDecimation_(Parameters::defaultGridScanDecimation()),
 	cellSize_(Parameters::defaultGridCellSize()),
 	preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
-	occupancyFromDepth_(Parameters::defaultGridFromDepth()),
+	occupancySensor_(Parameters::defaultGridSensor()),
 	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
 	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
 	normalKSearch_(Parameters::defaultGridNormalK()),
@@ -91,7 +91,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :

 void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromDepth_);
+	Parameters::parse(parameters, Parameters::kGridSensor(), occupancySensor_);
 	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
 	if(cloudDecimation_ == 0)
 	{
@@ -284,12 +284,12 @@ void OccupancyGrid::createLocalMap(
 		cv::Mat & groundCells,
 		cv::Mat & obstacleCells,
 		cv::Mat & emptyCells,
-		cv::Point3f & viewPoint) const
+		cv::Point3f & viewPoint)
 {
-	UDEBUG("scan format=%s, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
-			node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
+	UDEBUG("scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d",
+			node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancySensor_, normalsSegmentation_?1:0, grid3D_?1:0);

-	if((node.sensorData().laserScanRaw().is2d()) && !occupancyFromDepth_)
+	if((node.sensorData().laserScanRaw().is2d()) && occupancySensor_ == 0)
 	{
 		UDEBUG("2D laser scan");
 		//2D
@@ -328,7 +328,7 @@ void OccupancyGrid::createLocalMap(
 	else
 	{
 		// 3D
-		if(!occupancyFromDepth_)
+		if(occupancySensor_ == 0 || occupancySensor_ == 2)
 		{
 			if(!node.sensorData().laserScanRaw().isEmpty())
 			{
@@ -350,14 +350,35 @@ void OccupancyGrid::createLocalMap(
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());

 				UDEBUG("scan format=%d", scan.format());
+
+				bool normalSegmentationTmp = normalsSegmentation_;
+				float minGroundHeightTmp = minGroundHeight_;
+				float maxGroundHeightTmp = maxGroundHeight_;
+				if(scan.is2d())
+				{
+					// if 2D, assume the whole scan is obstacle
+					normalsSegmentation_ = false;
+					minGroundHeight_ = std::numeric_limits<int>::min();
+					maxGroundHeight_ = std::numeric_limits<int>::min()+100;
+				}
+
 				createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+
+				if(scan.is2d())
+				{
+					// restore
+					normalsSegmentation_ = normalSegmentationTmp;
+					minGroundHeight_ = minGroundHeightTmp;
+					maxGroundHeight_ = maxGroundHeightTmp;
+				}
 			}
 			else
 			{
-				UWARN("Cannot create local map, scan is empty (node=%d, %s=false).", node.id(), Parameters::kGridFromDepth().c_str());
+				UWARN("Cannot create local map, scan is empty (node=%d, %s=0).", node.id(), Parameters::kGridSensor().c_str());
 			}
 		}
-		else
+
+		if(occupancySensor_ >= 1)
 		{
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
@@ -407,7 +428,49 @@ void OccupancyGrid::createLocalMap(
 				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 			}
+
+			cv::Mat scanGroundCells;
+			cv::Mat scanObstacleCells;
+			cv::Mat scanEmptyCells;
+			if(occupancySensor_ == 2)
+			{
+				// backup
+				scanGroundCells = groundCells.clone();
+				scanObstacleCells = obstacleCells.clone();
+				scanEmptyCells = emptyCells.clone();
+			}
+
 			createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+
+			if(occupancySensor_ == 2)
+			{
+				if(grid3D_)
+				{
+					// We should convert scans to 4 channels (XYZRGB) to be compatible
+					scanGroundCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanGroundCells), Transform::getIdentity(), 255, 255, 255)).data();
+					scanObstacleCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanObstacleCells), Transform::getIdentity(), 255, 255, 255)).data();
+					scanEmptyCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanEmptyCells), Transform::getIdentity(), 255, 255, 255)).data();
+				}
+
+				UDEBUG("groundCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", groundCells.cols, groundCells.channels(), scanGroundCells.cols, scanGroundCells.channels());
+				UDEBUG("obstacleCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", obstacleCells.cols, obstacleCells.channels(), scanObstacleCells.cols, scanObstacleCells.channels());
+				UDEBUG("emptyCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", emptyCells.cols, emptyCells.channels(), scanEmptyCells.cols, scanEmptyCells.channels());
+
+				if(!groundCells.empty() && !scanGroundCells.empty())
+					cv::hconcat(groundCells, scanGroundCells, groundCells);
+				else if(!scanGroundCells.empty())
+					groundCells = scanGroundCells;
+
+				if(!obstacleCells.empty() && !scanObstacleCells.empty())
+					cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells);
+				else if(!scanObstacleCells.empty())
+					obstacleCells = scanObstacleCells;
+
+				if(!emptyCells.empty() && !scanEmptyCells.empty())
+					cv::hconcat(emptyCells, scanEmptyCells, emptyCells);
+				else if(!scanEmptyCells.empty())
+					emptyCells = scanEmptyCells;
+			}
 		}
 	}
 }
@@ -106,6 +106,23 @@ bool RtabmapColorOcTreeNode::createChild(unsigned int i) {
 #endif
 }

+void RtabmapColorOcTreeNode::updateOccupancyTypeChildren()
+{
+	if (children != NULL){
+		int type = kTypeUnknown;
+		for (int i=0; i<8 && type != kTypeObstacle; i++) {
+			RtabmapColorOcTreeNode* child = static_cast<RtabmapColorOcTreeNode*>(children[i]);
+
+			if (child != NULL && child->getOccupancyType() >= kTypeEmpty) {
+				if(type == kTypeUnknown) {
+					type = child->getOccupancyType();
+				}
+			}
+		}
+		type_ = type;
+	}
+}
+
 RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
 	: OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
 	RtabmapColorOcTreeMemberInit.ensureLinking();
@@ -231,6 +248,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
 		}
 		node->updateOccupancyChildren();
 		node->updateColorChildren();
+		node->updateOccupancyTypeChildren();
 	}
 #else
 	// only recurse and update for inner nodes:
@@ -245,6 +263,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
 	  }
 	  node->updateOccupancyChildren();
 	  node->updateColorChildren();
+	  node->updateOccupancyTypeChildren();
 	}
 #endif
 }
@@ -1209,21 +1228,23 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	int oi=0;
 	cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0);
 	cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0);
+	float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
 	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
 		octomap::point3d pt = octree_->keyToCoord(it.getKey());
-		if(octree_->isNodeOccupied(*it) && it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
+		if(octree_->isNodeOccupied(*it) &&
+		   it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
 		{
 			// projected on ground
-			oPtr[oi][0] = pt.x();
-			oPtr[oi][1] = pt.y();
+			oPtr[oi][0] = pt.x()-halfCellSize;
+			oPtr[oi][1] = pt.y()-halfCellSize;
 			++oi;
 		}
 		else
 		{
 			// projected on ground
-			gPtr[gi][0] = pt.x();
-			gPtr[gi][1] = pt.y();
+			gPtr[gi][0] = pt.x()-halfCellSize;
+			gPtr[gi][1] = pt.y()-halfCellSize;
 			++gi;
 		}
 	}
@@ -34,9 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryOkvis.h"
 #include "rtabmap/core/odometry/OdometryORBSLAM.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
+#include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINS.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
+#include "rtabmap/core/odometry/OdometryOpen3D.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -90,6 +92,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeLOAM:
 		odometry = new OdometryLOAM(parameters);
 		break;
+	case Odometry::kTypeFLOAM:
+		odometry = new OdometryFLOAM(parameters);
+		break;
 	case Odometry::kTypeMSCKF:
 		odometry = new OdometryMSCKF(parameters);
 		break;
@@ -99,6 +104,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeOpenVINS:
 		odometry = new OdometryOpenVINS(parameters);
 		break;
+	case Odometry::kTypeOpen3D:
+		odometry = new OdometryOpen3D(parameters);
+		break;
 	default:
 		UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
 		odometry = new OdometryF2M(parameters);
@@ -299,9 +307,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation*
 					data.imu().localTransform().rotation().inverse();

-			IMU imu2 = data.imu();
-			imu2.convertToBaseFrame();
-
 			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
 				this->framesProcessed() == 0)
 			{
@@ -216,15 +216,15 @@ void Optimizer::getConnectedGraph(

 	while(nextPoses.size())
 	{
-		int fromId = *nextPoses.rbegin(); // fill up all nodes before landmarks
+		int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
 		nextPoses.erase(*nextPoses.rbegin());

 		if(posesOut.empty())
 		{
-			posesOut.insert(std::make_pair(fromId, posesIn.find(fromId)->second));
+			posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));

 			// add prior links
-			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
+			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
 			{
 				if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
 				{
@@ -233,12 +233,12 @@ void Optimizer::getConnectedGraph(
 			}
 		}

-		for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
+		for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
 			int toId = iter->second;
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
-				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
+				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
 				if(nextPoses.find(toId) == nextPoses.end())
 				{
 					if(!uContains(posesOut, toId))
@@ -246,7 +246,7 @@ void Optimizer::getConnectedGraph(
 						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
 						{
 							Transform t;
-							if(kter->second.from()==fromId)
+							if(kter->second.from()==currentId)
 							{
 								t = kter->second.transform();
 							}
@@ -254,11 +254,11 @@ void Optimizer::getConnectedGraph(
 							{
 								t = kter->second.transform().inverse();
 							}
-							posesOut.insert(std::make_pair(toId, (posesOut.at(fromId) * t).to3DoF()));
+							posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
 						}
 						else
 						{
-							Transform t = posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse());
+							Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
 							posesOut.insert(std::make_pair(toId, t));
 						}
 						// add prior links
@@ -274,7 +274,7 @@ void Optimizer::getConnectedGraph(
 					}

 					// only add unique links
-					if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
+					if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
 					{
 						if(kter->second.to() < 0)
 						{
@@ -143,14 +143,40 @@ int savePDALFile(const std::string & filePath,
 int savePDALFile(const std::string & filePath,
 		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 		const std::vector<int> & cameraIds,
-		bool binary)
+		bool binary,
+		const std::vector<float> & intensities)
 {
 	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
 			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+	UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
+				uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());

 	pdal::PointTable table;

-	if(!cameraIds.empty())
+	if(!intensities.empty() && !cameraIds.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::PointSourceId,
+			pdal::Dimension::Id::Intensity});
+	}
+	else if(!intensities.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::Intensity});
+	}
+	else if(!cameraIds.empty())
 	{
 		table.layout()->registerDims({
 			pdal::Dimension::Id::X,
@@ -186,6 +212,10 @@ int savePDALFile(const std::string & filePath,
 		{
 			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
 		}
+		if(!intensities.empty())
+		{
+			view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i));
+		}
 	}
 	bufferReader.addView(view);

@@ -219,14 +249,46 @@ int savePDALFile(const std::string & filePath,
 int savePDALFile(const std::string & filePath,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const std::vector<int> & cameraIds,
-		bool binary)
+		bool binary,
+		const std::vector<float> & intensities)
 {
 	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
 			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+	UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
+			uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());

 	pdal::PointTable table;

-	if(!cameraIds.empty())
+	if(!intensities.empty() && !cameraIds.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ,
+			pdal::Dimension::Id::PointSourceId,
+			pdal::Dimension::Id::Intensity});
+	}
+	else if(!intensities.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ,
+			pdal::Dimension::Id::Intensity});
+	}
+	else if(!cameraIds.empty())
 	{
 		table.layout()->registerDims({
 			pdal::Dimension::Id::X,
@@ -271,6 +333,10 @@ int savePDALFile(const std::string & filePath,
 		{
 			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
 		}
+		if(!intensities.empty())
+		{
+			view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)intensities.at(i));
+		}
 	}
 	bufferReader.addView(view);

@@ -213,14 +213,15 @@ ParametersMap Parameters::getDefaultParameters(const std::string & groupIn)
 	return parameters;
 }

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

+		// 0.20.15
+		removedParameters_.insert(std::make_pair("Grid/FromDepth",           std::make_pair(true, Parameters::kGridSensor())));
+
 		// 0.20.9
 		removedParameters_.insert(std::make_pair("OdomORBSLAM2/VocPath",     std::make_pair(true, Parameters::kOdomORBSLAMVocPath())));
 		removedParameters_.insert(std::make_pair("OdomORBSLAM2/Bf",          std::make_pair(true, Parameters::kOdomORBSLAMBf())));
@@ -662,6 +666,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With PDAL:";
+#ifdef RTABMAP_PDAL
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With TORO:";
 #ifdef RTABMAP_TORO
@@ -824,6 +834,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With FLOAM:";
+#ifdef RTABMAP_FLOAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With FOVIS:";
 #ifdef RTABMAP_FOVIS
@@ -1053,7 +1069,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 						ignore = true;
 					}
 #endif
-#ifndef RTABMAP_LOAM
+#if not defined(RTABMAP_LOAM) and not defined(RTABMAP_FLOAM)
 					if(group.compare("OdomLOAM") == 0)
 					{
 						ignore = true;
@@ -118,7 +118,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
 }
 void Signature::addLink(const Link & link)
 {
-	UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
+	UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
 	UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
@@ -65,6 +65,12 @@ CameraDepthAI::CameraDepthAI(

 CameraDepthAI::~CameraDepthAI()
 {
+#ifdef RTABMAP_DEPTHAI
+	if(device_.get())
+	{
+		device_->close();
+	}
+#endif
 }

 void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
@@ -80,91 +86,6 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
 #endif
 }

-std::vector<unsigned char> convertCalibration(const StereoCameraModel & stereoModel)
-{
-	UDEBUG("");
-	// Calibration
-	// https://github.com/luxonis/depthai/blob/39852dcb9fe349476c30d0ed90d3750bb2a53e26/depthai_helpers/calibration_utils.py#L97-L109
-	std::vector<unsigned char> data;
-	cv::Mat tmp;
-	int ptr;
-	// R1_fp32
-	stereoModel.left().R().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// R2_fp32
-	stereoModel.right().R().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// M1_fp32
-	stereoModel.left().K_raw().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// M2_fp32
-	stereoModel.right().K_raw().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// R_fp32
-	stereoModel.R().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// T_fp32
-	stereoModel.T().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// M3_fp32
-	tmp = cv::Mat::zeros(3,3,CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// R_rgb_fp32
-	tmp = cv::Mat::eye(3,3,CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// T_rgb_fp32
-	tmp = cv::Mat::zeros(1,3,CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	// d1_coeff_fp32
-	stereoModel.left().D_raw().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-	data.resize(data.size() + (14-tmp.total())*sizeof(float), 0); // padding
-
-	// d2_coeff_fp32
-	stereoModel.right().D_raw().convertTo(tmp, CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-	data.resize(data.size() + (14-tmp.total())*sizeof(float), 0); // padding
-
-	// d3_coeff_fp32
-	tmp = cv::Mat::zeros(1,14,CV_32FC1);
-	ptr = data.size();
-	data.resize(data.size() + tmp.total()*tmp.elemSize());
-	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
-
-	return data;
-}
-
 bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -176,6 +97,13 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		return false;
 	}

+	if(device_.get())
+	{
+		device_->close();
+	}
+	accBuffer_.clear();
+	gyroBuffer_.clear();
+
 	dai::DeviceInfo deviceToUse;
 	if(deviceSerial_.empty())
 		deviceToUse = devices[0];
@@ -201,77 +129,21 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin

 	// look for calibration files
 	stereoModel_ = StereoCameraModel();
-	if(!calibrationFolder.empty())
-	{
-		std::string name = cameraName.empty()?deviceSerial_:cameraName;
-		if(!stereoModel_.load(calibrationFolder, name, false))
-		{
-			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-					name.c_str(), calibrationFolder.c_str());
-			outputDepth_ = false;
-		}
-		else
-		{
-			UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
-					stereoModel_.left().fx(),
-					stereoModel_.left().cx(),
-					stereoModel_.left().cy(),
-					stereoModel_.baseline());
-			stereoModel_.setLocalTransform(this->getLocalTransform());
-
-			cv::Size target(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);
-
-			if(stereoModel_.left().imageWidth() != target.width)
-			{
-				//adjust scale if resolution is not the same used than in calibration
-				UWARN("Loaded calibration has different resolution (%dx%d) than "
-					  "the selected device resolution (%dx%d). We will scale the calibration "
-					  "for convenience.",
-					  stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
-					  target.width, target.height);
-				stereoModel_.scale(double(target.width)/double(stereoModel_.left().imageWidth()));
-			}
-
-			if(stereoModel_.left().imageHeight() != target.height)
-			{
-				// Ratio not the same, adjust cy
-				cv::Rect roi(0, (stereoModel_.left().imageHeight()-target.height)/2, target.width, target.height);
-				UWARN("Loaded calibration has different height (%dx%d) than "
-					  "the selected device resolution (%dx%d). We will crop the calibration "
-					  "for convenience.",
-					  stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
-					  target.width, target.height);
-				stereoModel_.roi(roi);
-			}
-
-			if(ULogger::level() <= ULogger::kInfo)
-			{
-				UINFO("Calibration:");
-				std::cout << stereoModel_ << std::endl;
-			}
-		}
-	}
-
-	if(!stereoModel_.isValidForRectification())
-	{
-		UINFO("Disabling outputDepth as no valid calibration has been loaded.");
-		outputDepth_ = false;
-	}
-	else
-	{
-		stereoModel_.initRectificationMap();
-	}
+	cv::Size targetSize(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);

 	dai::Pipeline p;
 	auto monoLeft  = p.create<dai::node::MonoCamera>();
 	auto monoRight = p.create<dai::node::MonoCamera>();
 	auto stereo    = p.create<dai::node::StereoDepth>();
+	auto imu       = p.create<dai::node::IMU>();
 	auto xoutLeft = p.create<dai::node::XLinkOut>();
 	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
+	auto xoutIMU = p.create<dai::node::XLinkOut>();

 	 // XLinkOut
-	xoutLeft->setStreamName(outputDepth_/*stereoModel_.isValidForRectification()*/?"rectified_left":"left");
-	xoutDepthOrRight->setStreamName(outputDepth_?"depth"/*:stereoModel_.isValidForRectification()?"rectified_right"*/:"right");
+	xoutLeft->setStreamName("rectified_left");
+	xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
+	xoutIMU->setStreamName("imu");

 	// MonoCamera
 	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
@@ -285,9 +157,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	}

 	// StereoDepth
-	stereo->setOutputDepth(outputDepth_);
-	stereo->setOutputRectified(stereoModel_.isValidForRectification());
-	stereo->setConfidenceThreshold(depthConfidence_);
+	stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
 	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
 	stereo->setRectifyMirrorFrame(false);
 	stereo->setLeftRightCheck(false);
@@ -303,42 +173,55 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		stereo->rectifiedLeft.link(xoutLeft->input);
 		stereo->depth.link(xoutDepthOrRight->input);
 	}
-	/*else if(stereoModel_.isValidForRectification())
+	else
 	{
 		stereo->rectifiedLeft.link(xoutLeft->input);
 		stereo->rectifiedRight.link(xoutDepthOrRight->input);
-	}*/
-	else
-	{
-		stereo->syncedLeft.link(xoutLeft->input);
-		stereo->syncedRight.link(xoutDepthOrRight->input);
 	}

+	// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
+	imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
+	// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
+	imu->setBatchReportThreshold(1);
+	// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
+	// if lower or equal to batchReportThreshold then the sending is always blocking on device
+	// useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
+	imu->setMaxBatchReports(10);
+
+	// Link plugins IMU -> XLINK
+	imu->out.link(xoutIMU->input);

-	if(stereoModel_.isValidForRectification())
-	{
-		// FIXME: What is the exact format for the calibration stream?
-		//std::vector<unsigned char> data = convertCalibration(stereoModel_);
-		//stereo->loadCalibrationData(data);
-	}
 	device_.reset(new dai::Device(p, deviceToUse));

-	UDEBUG("");
-	if(outputDepth_)
-	{
-		leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
-		rightOrDepthQueue_ = device_->getOutputQueue("depth", 8, false);
-	}
-	else
-	{
-		UDEBUG("");
-		leftQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_left":*/"left", 8, false);
-		UDEBUG("");
-		rightOrDepthQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_right":*/"right", 8, false);
-		UDEBUG("");
-	}
+	UINFO("Loading eeprom calibration data");
+	dai::CalibrationHandler calibHandler = device_->readCalibration();
+	std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height));
+	double fx = matrix[0][0];
+	double fy = matrix[1][1];
+	double cx = matrix[0][2];
+	double cy = matrix[1][2];
+	matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT);
+	double baseline = matrix[0][3]/100.0;
+	UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
+	stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);

-	device_->startPipeline();
+	// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
+	//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
+	//imuLocalTransform_ = Transform(
+	//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
+	//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
+	//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
+	// Hard-coded acc: x->left, y->up, z->forward
+	// Hard-coded gyro: x->down, y->left, z->forward
+	imuLocalTransform_ = Transform(
+			0, 0, 1, 0,
+			1, 0, 0, 0,
+			0 ,1, 0, 0);
+	UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+
+	leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
+	rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false);
+	imuQueue_ = device_->getOutputQueue("imu", 50, false);

 	uSleep(2000); // avoid bad frames on start

@@ -374,6 +257,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 	cv::Mat left, depthOrRight;
 	auto rectifL = leftQueue_->get<dai::ImgFrame>();
 	auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
+
 	if(rectifL.get() && rectifRightOrDepth.get())
 	{
 		auto stampLeft = rectifL->getTimestamp().time_since_epoch().count();
@@ -399,10 +283,141 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 				data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
 			}

-			if(stampLeft != stampRight)
+			if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms
 			{
 				UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8);
 			}
+
+			//get imu
+			int added=  0;
+			while(1)
+			{
+				auto imuData = imuQueue_->get<dai::IMUData>();
+
+				auto imuPackets = imuData->packets;
+				double accStamp = 0.0;
+				double gyroStamp = 0.0;
+				for(auto& imuPacket : imuPackets) {
+					auto& acceleroValues = imuPacket.acceleroMeter;
+					auto& gyroValues = imuPacket.gyroscope;
+
+					accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
+					gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
+					accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
+					gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
+					if(accBuffer_.size() > 1000)
+					{
+						accBuffer_.erase(accBuffer_.begin());
+					}
+					if(gyroBuffer_.size() > 1000)
+					{
+						gyroBuffer_.erase(gyroBuffer_.begin());
+					}
+					++added;
+				}
+				if(accStamp >= stamp && gyroStamp >= stamp)
+				{
+					break;
+				}
+			}
+
+			cv::Vec3d acc, gyro;
+			bool valid = !accBuffer_.empty() && !gyroBuffer_.empty();
+			//acc
+			if(!accBuffer_.empty())
+			{
+				std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
+				std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
+				if(iterA != accBuffer_.begin())
+				{
+					iterA = --iterA;
+				}
+				if(iterB == accBuffer_.end())
+				{
+					iterB = --iterB;
+				}
+				if(iterA == iterB && stamp == iterA->first)
+				{
+					acc[0] = iterA->second[0];
+					acc[1] = iterA->second[1];
+					acc[2] = iterA->second[2];
+				}
+				else if(stamp >= iterA->first && stamp <= iterB->first)
+				{
+					float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+					acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
+					acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
+					acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
+				}
+				else
+				{
+					valid = false;
+					if(stamp < iterA->first)
+					{
+						UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+					}
+					else if(stamp > iterB->first)
+					{
+						UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
+					}
+					else
+					{
+						UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+					}
+				}
+			}
+			//gyro
+			if(!gyroBuffer_.empty())
+			{
+				std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
+				std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
+				if(iterA != gyroBuffer_.begin())
+				{
+					iterA = --iterA;
+				}
+				if(iterB == gyroBuffer_.end())
+				{
+					iterB = --iterB;
+				}
+				if(iterA == iterB && stamp == iterA->first)
+				{
+					gyro[0] = iterA->second[0];
+					gyro[1] = iterA->second[1];
+					gyro[2] = iterA->second[2];
+				}
+				else if(stamp >= iterA->first && stamp <= iterB->first)
+				{
+					float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+					gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
+					gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
+					gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
+				}
+				else
+				{
+					valid = false;
+					if(stamp < iterA->first)
+					{
+						UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+					}
+					else if(stamp > iterB->first)
+					{
+						UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
+					}
+					else
+					{
+						UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+					}
+				}
+				// Rotate gyro frame (x->down, y->left, z->forward) in acc frame (x->left, y->up, z->forward)
+				double tmp = gyro[0];
+				gyro[0] = gyro[1];
+				gyro[1] = -tmp;
+			}
+
+			if(valid)
+			{
+				data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
+			}
 		}
 	}
 	else
@@ -862,7 +862,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 						cv::cvtColor(img, out, CV_BGRA2BGR);
 						img = out;
 					}
-					else if(_bayerMode >= 0 && _bayerMode <=3)
+					else if(!img.empty() && _bayerMode >= 0 && _bayerMode <=3)
 					{
 						cv::Mat debayeredImg;
 						try
@@ -74,7 +74,8 @@ CameraOpenNI2::CameraOpenNI2(
 	_deviceId(deviceId),
 	_openNI2StampsAndIDsUsed(false),
 	_depthHShift(0),
-	_depthVShift(0)
+	_depthVShift(0),
+	_depthDecimation(1)
 #endif
 {
 }
@@ -177,13 +178,19 @@ void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
 void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
 {
 #ifdef RTABMAP_OPENNI2
-	UASSERT(horizontal >= 0);
-	UASSERT(vertical >= 0);
 	_depthHShift = horizontal;
 	_depthVShift = vertical;
 #endif
 }

+void CameraOpenNI2::setDepthDecimation(int decimation)
+{
+#ifdef RTABMAP_OPENNI2
+	UASSERT(decimation >= 1);
+	_depthDecimation = decimation;
+#endif
+}
+
 bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 #ifdef RTABMAP_OPENNI2
@@ -529,10 +536,19 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)

 				if(_type==kTypeColorDepth)
 				{
-					if (_depthHShift > 0 || _depthVShift > 0)
+					if (_depthHShift != 0 || _depthVShift != 0)
 					{
 						cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
-						depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
+						depth(cv::Rect(
+								_depthHShift>0?_depthHShift:0,
+								_depthVShift>0?_depthVShift:0,
+								depth.cols - abs(_depthHShift),
+								depth.rows - abs(_depthVShift))).copyTo(
+							   out(cv::Rect(
+								_depthHShift<0?-_depthHShift:0,
+								_depthVShift<0?-_depthVShift:0,
+								depth.cols - abs(_depthHShift),
+								depth.rows - abs(_depthVShift))));
 						depth = out;
 					}

@@ -544,8 +560,18 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 						if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
 						{
 							depth = _stereoModel.left().rectifyImage(depth, 0);
-							depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
+							CameraModel depthModel = _stereoModel.left().scaled(1.0 / double(_depthDecimation));
+							depth = util2d::decimate(depth, _depthDecimation);
+							depth = util2d::registerDepth(depth, depthModel.K(), rgb.size()/_depthDecimation, _stereoModel.right().scaled(1.0/double(_depthDecimation)).K(), _stereoModel.stereoTransform());
 						}
+						else if (_depthDecimation > 1)
+						{
+							depth = util2d::decimate(depth, _depthDecimation);
+						}
+					}
+					else if (_depthDecimation > 1)
+					{
+						depth = util2d::decimate(depth, _depthDecimation);
 					}
 				}
 				else // IR
@@ -70,6 +70,7 @@ CameraRealSense2::CameraRealSense2(
 	rectifyImages_(true),
 	odometryProvided_(false),
 	odometryImagesDisabled_(false),
+	odometryOnlyLeftStream_(false),
 	cameraWidth_(640),
 	cameraHeight_(480),
 	cameraFps_(30),
@@ -193,7 +194,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)

 void CameraRealSense2::frame_callback(rs2::frame frame)
 {
-	//UDEBUG("Frame callback! %f", frame.get_timestamp());
+	UDEBUG("Frame callback! %f", frame.get_timestamp());
 	syncer_(frame);
 }
 void CameraRealSense2::multiple_message_callback(rs2::frame frame)
@@ -694,8 +695,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	UDEBUG("");

 	model_ = CameraModel();
-	rs2::stream_profile depthStreamProfile;
-	rs2::stream_profile rgbStreamProfile;
 	std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(sensors.size());
 	 for (unsigned int i=0; i<sensors.size(); ++i)
 	 {
@@ -759,8 +758,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 								intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
 								intrinsic.model,
 								intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]);
-						rgbStreamProfile = profile;
-						rgbIntrinsics_ = intrinsic;
 						added = true;
 						if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2)
 						{
@@ -773,8 +770,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					{
 						profilesPerSensor[i].push_back(profile);
 						depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						depthIntrinsics_ = intrinsic;
 						added = true;
 						if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2)
 						{
@@ -828,20 +823,44 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 				{
 					UASSERT(i<2);
 					profilesPerSensor[i].push_back(profile);
-					auto intrinsic = video_profile.get_intrinsics();
 					if(pi==0)
 					{
 						// LEFT FISHEYE
 						rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						rgbStreamProfile = profile;
-						rgbIntrinsics_ = intrinsic;
+						if(odometryOnlyLeftStream_)
+						{
+							auto intrinsic = video_profile.get_intrinsics();
+							UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f",
+									intrinsic.width, intrinsic.height,
+									intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
+									intrinsic.model,
+									intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3]);
+							cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
+							K.at<double>(0,0) = intrinsic.fx;
+							K.at<double>(1,1) = intrinsic.fy;
+							K.at<double>(0,2) = intrinsic.ppx;
+							K.at<double>(1,2) = intrinsic.ppy;
+							UASSERT(intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4); // we expect fisheye 4 values
+							cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+							D.at<double>(0,0) = intrinsic.coeffs[0];
+							D.at<double>(0,1) = intrinsic.coeffs[1];
+							D.at<double>(0,4) = intrinsic.coeffs[2];
+							D.at<double>(0,5) = intrinsic.coeffs[3];
+							cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+							P.at<double>(0,0) = intrinsic.fx;
+							P.at<double>(1,1) = intrinsic.fy;
+							P.at<double>(0,2) = intrinsic.ppx;
+							P.at<double>(1,2) = intrinsic.ppy;
+							cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+							model_ = CameraModel(camera_name, cv::Size(intrinsic.width, intrinsic.height), K, D, R, P, this->getLocalTransform());
+							if(rectifyImages_)
+								model_.initRectificationMap();
+						}
 					}
-					else
+					else if(!odometryOnlyLeftStream_)
 					{
 						// RIGHT FISHEYE
 						depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						depthIntrinsics_ = intrinsic;
 					}
 					added = true;
 				}
@@ -961,7 +980,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 {
 			 serial = cameraName;
 		 }
-		if(!calibrationFolder.empty() && !serial.empty())
+		if(!odometryImagesDisabled_ &&
+		   !odometryOnlyLeftStream_ &&
+		   !calibrationFolder.empty() && !serial.empty())
 		{
 			if(!stereoModel_.load(calibrationFolder, serial, false))
 			{
@@ -1005,7 +1026,10 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st

 			Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 			this->setLocalTransform(this->getLocalTransform() * opticalTransform.inverse());
-			stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			if(odometryOnlyLeftStream_)
+				model_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			else
+				stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
 			imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;

 			if(odometryImagesDisabled_)
@@ -1027,9 +1051,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
 			 imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
 			 UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
-			 stereoModel_.setLocalTransform(this->getLocalTransform());
+			 if(odometryOnlyLeftStream_)
+				 model_.setLocalTransform(this->getLocalTransform());
+			 else
+				 stereoModel_.setLocalTransform(this->getLocalTransform());
 		}
-		if(rectifyImages_ && !stereoModel_.isValidForRectification())
+		if(!odometryImagesDisabled_ && rectifyImages_ && !model_.isValidForRectification() && !stereoModel_.isValidForRectification())
 		{
 			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 			return false;
@@ -1192,6 +1219,7 @@ void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
 	{
 		odometryProvided_ = true;
 		odometryImagesDisabled_ = false;
+		odometryOnlyLeftStream_ = false;
 	}
 #endif
 }
@@ -1210,7 +1238,7 @@ void CameraRealSense2::setImagesRectified(bool enabled)
 #endif
 }

-void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
+void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, bool onlyLeftStream)
 {
 #ifdef RTABMAP_REALSENSE2
 	if(dualMode_ && !enabled)
@@ -1220,6 +1248,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
 	}
 	odometryProvided_ = enabled;
 	odometryImagesDisabled_ = enabled && imageStreamsDisabled;
+	odometryOnlyLeftStream_ = enabled && !imageStreamsDisabled && onlyLeftStream;
 #endif
 }

@@ -1374,27 +1403,48 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
 				}
 			}
-			else if(is_left_fisheye_arrived && is_right_fisheye_arrived)
+			else if(is_left_fisheye_arrived)
 			{
-				auto from_image_frame = depth_frame.as<rs2::video_frame>();
-				cv::Mat left,right;
-				if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+				if(odometryOnlyLeftStream_)
 				{
-					left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
-					right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
-				}
-				else
-				{
-					left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
-					right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
-				}
+					cv::Mat left;
+					if(rectifyImages_ && model_.isValidForRectification())
+					{
+						left = model_.rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+					}
+					else
+					{
+						left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+					}

-				if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
-				{
-					stereoModel_.setImageSize(left.size());
-				}
+					if(model_.imageHeight() == 0 || model_.imageWidth() == 0)
+					{
+						model_.setImageSize(left.size());
+					}

-				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+					data = SensorData(left, cv::Mat(), model_, this->getNextSeqID(), stamp);
+				}
+				else if(is_right_fisheye_arrived)
+				{
+					cv::Mat left,right;
+					if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+					{
+						left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+						right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
+					}
+					else
+					{
+						left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+						right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
+					}
+
+					if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+					{
+						stereoModel_.setImageSize(left.size());
+					}
+
+					data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+				}
 			}
 			else
 			{
@@ -1471,6 +1521,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				lastImuStamp_ = imuStamp;
 			}
 		}
+		else if(frameset.size()==1 && frameset[0].get_profile().stream_type() == RS2_STREAM_FISHEYE)
+		{
+			UERROR("Missing frames (received %d, needed=%d). For T265 camera, "
+					"either use realsense sdk v2.42.0, or apply "
+					"this patch (https://github.com/IntelRealSense/librealsense/issues/9030#issuecomment-962223017) "
+					"to fix this problem.", (int)frameset.size(), desiredFramesetSize);
+		}
 		else
 		{
 			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
@@ -83,6 +83,8 @@ CameraStereoImages::~CameraStereoImages()

 bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
+	UINFO("Calibration folder: \"%s\", name=\"%s\"", calibrationFolder.c_str(), cameraName.c_str());
+
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
@@ -105,8 +107,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	stereoModel_.setName(cameraName);
 	if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
 	{
-		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
-		return false;
+		UWARN("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid for rectification. This can be ignored if input images are already rectified.");
 	}

 	//desactivate before init as we will do it in this class instead for convenience
@@ -165,6 +166,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
 	{
 		if(camera2_)
 		{
+			camera2_->setBayerMode(this->getBayerMode());
 			right = camera2_->takeImage(info);
 		}
 		else
@@ -275,19 +275,21 @@ namespace clams

  void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
  {
-    UASSERT(width_ == depth.cols);
-    UASSERT(height_ ==depth.rows);
+    UASSERT(width_ >= depth.cols && width_ % depth.cols == 0);
+    UASSERT(height_ >=depth.rows && height_ % depth.rows == 0);
+    UASSERT(height_ >= depth.rows && height_ % depth.rows == 0);
    UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
+    int factor = width_ / depth.cols;
    if(depth.type() == CV_32FC1)
    {
 		#pragma omp parallel for
-		for(int v = 0; v < height_; ++v) {
-		  for(int u = 0; u < width_; ++u) {
+		for(int v = 0; v < depth.rows; ++v) {
+		  for(int u = 0; u < depth.cols; ++u) {
 			 float & z = depth.at<float>(v, u);
 			if(uIsNan(z) || z == 0.0f)
 			  continue;
 			double zf = z;
-			frustum(v, u).interpolatedUndistort(&zf);
+			frustum(v * factor, u * factor).interpolatedUndistort(&zf);
 			z = zf;
 		  }
 		}
@@ -295,13 +297,13 @@ namespace clams
    else
    {
 		#pragma omp parallel for
-		for(int v = 0; v < height_; ++v) {
-		  for(int u = 0; u < width_; ++u) {
+		for(int v = 0; v < depth.rows; ++v) {
+		  for(int u = 0; u < depth.cols; ++u) {
 		    unsigned short & z = depth.at<unsigned short>(v, u);
 			if(uIsNan(z) || z == 0)
 			  continue;
 			double zf = z * 0.001;
-			frustum(v, u).interpolatedUndistort(&zf);
+			frustum(v * factor, u * factor).interpolatedUndistort(&zf);
 			z = zf*1000;
 		  }
 		}
@@ -240,7 +240,7 @@ Transform OdometryF2F::computeTransform(
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().rows;
-			if(!refFrame_.sensorData().isValid())
+			if(!refFrame_.sensorData().isValid() || (features==0 && registrationPipeline_->isImageRequired()))
 			{
 				newFrame = Signature(data);
 				// this will generate features only for the first frame or if optical flow was used (no 3d words)
@@ -0,0 +1,200 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/odometry/OdometryFLOAM.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/core/util3d.h"
+#include <pcl/common/transforms.h>
+
+#ifdef RTABMAP_FLOAM
+#include <laserProcessingClass.h>
+#include <odomEstimationClass.h>
+#endif
+
+namespace rtabmap {
+
+/**
+ * https://github.com/wh200720041/floam
+ */
+
+OdometryFLOAM::OdometryFLOAM(const ParametersMap & parameters) :
+	Odometry(parameters)
+#ifdef RTABMAP_FLOAM
+	,laserProcessing_(new LaserProcessingClass())
+	,odomEstimation_(new OdomEstimationClass())
+	,lastPose_(Transform::getIdentity())
+	,lost_(false)
+#endif
+{
+#ifdef RTABMAP_FLOAM
+	int sensor = Parameters::defaultOdomLOAMSensor();
+	double vertical_angle = 2.0; // seems not used by floam (https://github.com/wh200720041/floam/issues/31)
+	float scan_period= Parameters::defaultOdomLOAMScanPeriod();
+	float max_dis = Parameters::defaultIcpRangeMax();
+	float min_dis = Parameters::defaultIcpRangeMin();
+	float map_resolution = Parameters::defaultOdomLOAMResolution();
+	linVar_ = Parameters::defaultOdomLOAMLinVar();
+	angVar_ = Parameters::defaultOdomLOAMAngVar();
+
+	Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), sensor);
+	Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scan_period);
+	Parameters::parse(parameters, Parameters::kIcpRangeMax(), max_dis);
+	Parameters::parse(parameters, Parameters::kIcpRangeMin(), min_dis);
+	Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), map_resolution);
+
+	UASSERT(scan_period>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
+	UASSERT(linVar_>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_);
+	UASSERT(angVar_>0.0f);
+
+	lidar::Lidar lidar_param;
+	lidar_param.setScanPeriod(scan_period);
+	lidar_param.setVerticalAngle(vertical_angle);
+	lidar_param.setLines(sensor==2?64:sensor==1?32:16);
+	lidar_param.setMaxDistance(max_dis<=0?200:max_dis);
+	lidar_param.setMinDistance(min_dis);
+
+	laserProcessing_->init(lidar_param);
+	odomEstimation_->init(lidar_param, map_resolution);
+#endif
+}
+
+OdometryFLOAM::~OdometryFLOAM()
+{
+#ifdef RTABMAP_FLOAM
+	delete laserProcessing_;
+	delete odomEstimation_;
+#endif
+}
+
+void OdometryFLOAM::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_FLOAM
+	lastPose_.setIdentity();
+	lost_ = false;
+#endif
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryFLOAM::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+#ifdef RTABMAP_FLOAM
+	UTimer timer;
+	UTimer timerTotal;
+
+	if(data.laserScanRaw().isEmpty())
+	{
+		UERROR("LOAM works only with laser scans and the current input is empty. Aborting odometry update...");
+		return t;
+	}
+	else if(data.laserScanRaw().is2d())
+	{
+		UERROR("LOAM version used works only with 3D laser scans from Velodyne. Aborting odometry update...");
+		return t;
+	}
+
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
+	if(!lost_)
+	{
+		pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudInPtr = util3d::laserScanToPointCloudI(data.laserScanRaw());
+
+		UDEBUG("Scan conversion: %fs", timer.ticks());
+
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_edge(new pcl::PointCloud<pcl::PointXYZI>());
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_surf(new pcl::PointCloud<pcl::PointXYZI>());
+
+		laserProcessing_->featureExtraction(laserCloudInPtr,pointcloud_edge,pointcloud_surf);
+		UDEBUG("Feature extraction: %fs", timer.ticks());
+
+		if(this->framesProcessed() == 0){
+			odomEstimation_->initMapWithPoints(pointcloud_edge, pointcloud_surf);
+		}else{
+			odomEstimation_->updatePointsToMap(pointcloud_edge, pointcloud_surf);
+		}
+		UDEBUG("Update: %fs", timer.ticks());
+
+		Transform pose = Transform::fromEigen3d(odomEstimation_->odom);
+
+		if(!pose.isNull())
+		{
+			covariance = cv::Mat::eye(6,6,CV_64FC1);
+			covariance(cv::Range(0,3), cv::Range(0,3)) *= linVar_;
+			covariance(cv::Range(3,6), cv::Range(3,6)) *= angVar_;
+
+			t = lastPose_.inverse() * pose; // incremental
+			lastPose_ = pose;
+
+			const Transform & localTransform = data.laserScanRaw().localTransform();
+			if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+			{
+				// from laser frame to base frame
+				t = localTransform * t * localTransform.inverse();
+			}
+
+			if(info)
+			{
+				info->type = (int)kTypeLOAM;
+				if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
+				{
+					info->reg.covariance = covariance;
+				}
+
+				if(this->isInfoDataFilled())
+				{
+					pcl::PointCloud<pcl::PointXYZI>::Ptr localMap(new pcl::PointCloud<pcl::PointXYZI>());
+					odomEstimation_->getMap(localMap);
+					info->localScanMapSize = localMap->size();
+					info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(*localMap), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
+					UDEBUG("Fill info data: %fs", timer.ticks());
+				}
+			}
+		}
+		else
+		{
+			lost_ = true;
+			UWARN("FLOAM failed to register the latest scan, odometry should be reset.");
+		}
+	}
+	UINFO("Odom update time = %fs, lost=%s", timerTotal.elapsed(), lost_?"true":"false");
+
+#else
+	UERROR("RTAB-Map is not built with FLOAM support! Select another odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap
@@ -34,8 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include <pcl/common/transforms.h>

-float SCAN_PERIOD = 0.1f;
-
 namespace rtabmap {

 /**
@@ -54,10 +52,13 @@ OdometryLOAM::OdometryLOAM(const ParametersMap & parameters) :
 #endif
 {
 #ifdef RTABMAP_LOAM
-	int velodyneType = 0;
+	int velodyneType = Parameters::defaultOdomLOAMSensor();
+	float mapResolution  = Parameters::defaultOdomLOAMResolution();
 	Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), velodyneType);
 	Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scanPeriod_);
 	UASSERT(scanPeriod_>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMResolution(), mapResolution);
+	UASSERT(mapResolution>0.0f);
 	Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
 	UASSERT(linVar_>0.0f);
 	Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_);
@@ -77,6 +78,8 @@ OdometryLOAM::OdometryLOAM(const ParametersMap & parameters) :
 	}
 	laserOdometry_ =  new loam::BasicLaserOdometry(scanPeriod_);
 	laserMapping_ = new loam::BasicLaserMapping(scanPeriod_);
+	laserMapping_->downSizeFilterCorner().setLeafSize(mapResolution, mapResolution, mapResolution);
+	laserMapping_->downSizeFilterSurf().setLeafSize(mapResolution*2.0f, mapResolution*2.0f, mapResolution*2.0f);
 #endif
 }

@@ -177,7 +180,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZI> > OdometryLOAM::segmentScanRings(con
 		}

 		// calculate relative scan time based on point orientation
-		float relTime = SCAN_PERIOD * (ori - startOri) / (endOri - startOri);
+		float relTime = scanPeriod_ * (ori - startOri) / (endOri - startOri);
 		point.intensity = scanID + relTime;

 		// imu not used...
@@ -283,7 +286,7 @@ Transform OdometryLOAM::computeTransform(
 					Transform rot(0,0,1,0,1,0,0,0,0,1,0,0);
 					pcl::PointCloud<pcl::PointXYZI> out;
 					pcl::transformPointCloud(laserMapping_->laserCloudSurroundDS(), out, rot.toEigen3f());
-					info->localScanMap = LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(out), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
+					info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(out), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
 				}
 			}
 		}
@@ -813,6 +813,10 @@ public:
 			Verbose::SetTh(Verbose::VERBOSITY_QUIET);
 #endif

+		// Reset all static variables
+		Frame::mbInitialComputations = true;
+		mpTracker->Reset(true);
+
 		return true;
 	}

@@ -0,0 +1,306 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/odometry/OdometryOpen3D.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+
+#ifdef RTABMAP_OPEN3D
+#include <open3d/pipelines/odometry/Odometry.h>
+#include <open3d/geometry/RGBDImage.h>
+#include <open3d/t/pipelines/odometry/RGBDOdometry.h>
+#endif
+
+namespace rtabmap {
+
+/**
+ * https://github.com/laboshinl/loam_velodyne/pull/66
+ */
+
+OdometryOpen3D::OdometryOpen3D(const ParametersMap & parameters) :
+	Odometry(parameters)
+#ifdef RTABMAP_OPEN3D
+	,method_(Parameters::defaultOdomOpen3DMethod()),
+	maxDepth_(Parameters::defaultOdomOpen3DMaxDepth()),
+	keyFrameThr_(Parameters::defaultOdomKeyFrameThr())
+#endif
+{
+#ifdef RTABMAP_OPEN3D
+	Parameters::parse(parameters, Parameters::kOdomOpen3DMethod(), method_);
+	UASSERT(method_>=0 && method_<=2);
+	Parameters::parse(parameters, Parameters::kOdomOpen3DMaxDepth(), maxDepth_);
+	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
+#endif
+}
+
+OdometryOpen3D::~OdometryOpen3D()
+{
+}
+
+void OdometryOpen3D::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_OPEN3D
+	keyFrame_ = SensorData();
+	lastKeyFramePose_.setNull();
+#endif
+}
+
+#ifdef RTABMAP_OPEN3D
+open3d::geometry::Image toOpen3D(const cv::Mat & image)
+{
+	if(image.type() == CV_16UC1)
+	{
+		// convert to float
+		return toOpen3D(util2d::cvtDepthToFloat(image));
+	}
+	open3d::geometry::Image output;
+	output.width_ = image.cols;
+	output.height_ = image.rows;
+	output.num_of_channels_ = image.channels();
+	output.bytes_per_channel_ = image.elemSize()/image.channels();
+	output.data_.resize(image.total()*image.elemSize());
+	memcpy(output.data_.data(), image.data, output.data_.size());
+	return output;
+}
+
+open3d::geometry::RGBDImage toOpen3D(const SensorData & data)
+{
+	return open3d::geometry::RGBDImage(
+			toOpen3D(data.imageRaw()),
+			toOpen3D(data.depthRaw()));
+}
+
+open3d::camera::PinholeCameraIntrinsic toOpen3D(const CameraModel & model)
+{
+	return open3d::camera::PinholeCameraIntrinsic(
+			model.imageWidth(),
+			model.imageHeight(),
+			model.fx(),
+			model.fy(),
+			model.cx(),
+			model.cy());
+}
+//Tensor versions
+open3d::t::geometry::Image toOpen3Dt(const cv::Mat & image)
+{
+	if(image.type() == CV_16UC1)
+	{
+		// convert to float
+		return toOpen3Dt(util2d::cvtDepthToFloat(image));
+	}
+
+	if(image.type()==CV_32FC1)
+	{
+		return open3d::core::Tensor(
+			(const float_t*)image.data,
+			{image.rows, image.cols, image.channels()},
+			open3d::core::Float32);
+	}
+	else
+	{
+		return open3d::core::Tensor(
+			static_cast<const uint8_t*>(image.data),
+			{image.rows, image.cols, image.channels()},
+			open3d::core::UInt8);
+	}
+}
+
+open3d::t::geometry::RGBDImage toOpen3Dt(const SensorData & data)
+{
+	return open3d::t::geometry::RGBDImage(
+			toOpen3Dt(data.imageRaw()),
+			toOpen3Dt(data.depthRaw()));
+}
+
+open3d::core::Tensor toOpen3Dt(const CameraModel & model)
+{
+	return open3d::core::Tensor::Init<double>(
+			{{model.fx(), 0, model.cx()},
+			 {0, model.fy(), model.cy()},
+			 {0, 0, 1}});
+}
+
+open3d::core::Tensor toOpen3Dt(const Transform & t)
+{
+	return open3d::core::Tensor::Init<double>(
+			{{t.r11(), t.r12(), t.r13(), t.x()},
+			 {t.r21(), t.r22(), t.r23(), t.y()},
+			 {t.r31(), t.r32(), t.r33(), t.z()},
+			 {0,0,0,1}});
+}
+#endif
+
+// return not null transform if odometry is correctly computed
+Transform OdometryOpen3D::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+#ifdef RTABMAP_OPEN3D
+	UTimer timer;
+
+	if(data.imageRaw().empty() || data.depthRaw().empty() || data.cameraModels().size()!=1)
+	{
+		UERROR("Open3D works only with single RGB-D data. Aborting odometry update...");
+		return t;
+	}
+
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
+
+	bool updateKeyFrame = false;
+	if(lastKeyFramePose_.isNull())
+	{
+		lastKeyFramePose_ = this->getPose(); // reset to current pose
+	}
+	Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
+
+	if(keyFrame_.isValid())
+	{
+		/*bool tensor = true;
+		if(!tensor)
+		{
+			// Approach in open3d/pipelines/odometry
+			open3d::geometry::RGBDImage source = toOpen3D(data);
+			open3d::geometry::RGBDImage target = toOpen3D(keyFrame_);
+			open3d::camera::PinholeCameraIntrinsic intrinsics = toOpen3D(data.cameraModels()[0]);
+			UDEBUG("Data conversion to Open3D format: %fs", timer.ticks());
+			open3d::pipelines::odometry::OdometryOption option;
+			std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> ret = open3d::pipelines::odometry::ComputeRGBDOdometry(
+					source,
+					target,
+					intrinsics,
+					Eigen::Matrix4d::Identity(),
+					open3d::pipelines::odometry::RGBDOdometryJacobianFromHybridTerm(),
+					option);
+			UDEBUG("Compute Open3D odometry: %fs", timer.ticks());
+			if(std::get<0>(ret))
+			{
+				t = Transform::fromEigen4d(std::get<1>(ret));
+				// from camera frame to base frame
+				t = data.cameraModels()[0].localTransform() * t * data.cameraModels()[0].localTransform().inverse();
+				covariance = cv::Mat::eye(6,6,CV_64FC1)/100;
+			}
+			else
+			{
+				UWARN("Open3D odometry update failed!");
+			}
+		}
+		else*/
+		{
+			// Approach in open3d/t/pipelines/odometry
+			open3d::t::geometry::RGBDImage source = toOpen3Dt(data);
+			open3d::t::geometry::RGBDImage target = toOpen3Dt(keyFrame_);
+			open3d::core::Tensor intrinsics = toOpen3Dt(data.cameraModels()[0]);
+			Transform baseToCamera = data.cameraModels()[0].localTransform();
+			open3d::core::Tensor odomInit;
+			if(guess.isNull())
+			{
+				odomInit = open3d::core::Tensor::Eye(4, open3d::core::Float64, open3d::core::Device("CPU:0"));
+			}
+			else
+			{
+				odomInit = toOpen3Dt(baseToCamera.inverse() * (motionSinceLastKeyFrame*guess) * baseToCamera);
+			}
+			UDEBUG("Data conversion to Open3D format: %fs", timer.ticks());
+			open3d::t::pipelines::odometry::OdometryResult ret = open3d::t::pipelines::odometry::RGBDOdometryMultiScale(
+					source,
+					target,
+					intrinsics,
+					odomInit,
+					1.0f,
+					maxDepth_,
+				    {10, 5, 3},
+					(open3d::t::pipelines::odometry::Method)method_,
+				   open3d::t::pipelines::odometry::OdometryLossParams());
+			UDEBUG("Compute Open3D odometry: %fs", timer.ticks());
+			if(ret.fitness_!=0)
+			{
+				const double * ptr = (const double *)ret.transformation_.GetDataPtr();
+				t = Transform(
+						ptr[0], ptr[1], ptr[2], ptr[3],
+						ptr[4], ptr[5], ptr[6], ptr[7],
+						ptr[8], ptr[9], ptr[10],ptr[11]);
+				// from camera frame to base frame
+				t = baseToCamera * t * baseToCamera.inverse();
+
+				t = motionSinceLastKeyFrame.inverse() * t;
+
+				//based on values set in viso2_ros
+				covariance = cv::Mat::eye(6,6, CV_64FC1);
+				covariance.at<double>(0,0) = 0.002;
+				covariance.at<double>(1,1) = 0.002;
+				covariance.at<double>(2,2) = 0.05;
+				covariance.at<double>(3,3) = 0.09;
+				covariance.at<double>(4,4) = 0.09;
+				covariance.at<double>(5,5) = 0.09;
+
+				if(info)
+				{
+					info->reg.icpRMS = ret.inlier_rmse_;
+					info->reg.icpInliersRatio = ret.fitness_;
+				}
+
+				if(ret.fitness_ < keyFrameThr_)
+				{
+					updateKeyFrame = true;
+				}
+			}
+			else
+			{
+				UWARN("Open3D odometry update failed!");
+			}
+		}
+	}
+	else
+	{
+		t.setIdentity();
+		updateKeyFrame = true;
+	}
+
+	if(updateKeyFrame)
+	{
+		keyFrame_ = data;
+		lastKeyFramePose_.setNull();
+	}
+
+	if(info)
+	{
+		info->reg.covariance = covariance;
+		info->keyFrameAdded = updateKeyFrame;
+	}
+
+#else
+	UERROR("RTAB-Map is not built with Open3D support! Select another odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap
@@ -130,9 +130,6 @@ Transform OdometryOpenVINS::computeTransform(
 			params.state_options.num_cameras = 2;
 			//params.dt_slam_delay = 2;

-			params.stereo_pairs.emplace_back(0, 1);
-			params.state_options.num_unique_cameras = 1;
-
 			// Set what representation we should be using
 			//params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
 			//params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH");  // default GLOBAL_3D
@@ -339,6 +336,8 @@ Transform OdometryOpenVINS::computeTransform(
 		message.sensor_ids.push_back(1);
 		message.images.push_back(left);
 		message.images.push_back(right);
+		message.masks.push_back(cv::Mat::zeros(left.size(), CV_8UC1));
+		message.masks.push_back(cv::Mat::zeros(right.size(), CV_8UC1));

 		// send it to our VIO system
 		vioManager_->feed_measurement_camera(message);
@@ -310,14 +310,26 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		}
 #endif
 		// detect if there is a global pose prior set, if so remove rootId
-		if(!priorsIgnored())
+		bool hasGravityConstraints = false;
+		if(!priorsIgnored() || (!isSlam2d() && gravitySigma() > 0))
 		{
 			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 			{
-				if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
+				if(iter->second.from() == iter->second.to())
 				{
-					rootId = 0;
-					break;
+					if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
+					{
+						rootId = 0;
+						break;
+					}
+					else if(iter->second.type() == Link::kGravity)
+					{
+						hasGravityConstraints = true;
+						if(priorsIgnored())
+						{
+							break;
+						}
+					}
 				}
 			}
 		}
@@ -325,7 +337,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int landmarkVertexOffset = poses.rbegin()->first+1;
 		std::map<int, bool> isLandmarkWithRotation;

-		UDEBUG("fill poses to g2o...");
+		UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d)", rootId, hasGravityConstraints?1:0);
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			UASSERT(!iter->second.isNull());
@@ -339,6 +351,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
 					if(id == rootId)
 					{
+						UDEBUG("Set %d fixed", id);
 						v2->setFixed(true);
 					}
 					vertex = v2;
@@ -361,6 +374,11 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					{
 						g2o::VertexSE2 * v2 = new g2o::VertexSE2();
 						v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
+						if(id == rootId)
+						{
+							UDEBUG("Set %d fixed", id);
+							v2->setFixed(true);
+						}
 						vertex = v2;
 						isLandmarkWithRotation.insert(std::make_pair(id, true));
 						id = landmarkVertexOffset - id;
@@ -382,8 +400,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					pose = a.linear();
 					pose.translation() = a.translation();
 					v3->setEstimate(pose);
-					if(id == rootId)
+					if(id == rootId && !hasGravityConstraints)
 					{
+						UDEBUG("Set %d fixed", id);
 						v3->setFixed(true);
 					}
 					vertex = v3;
@@ -412,6 +431,11 @@ std::map<int, Transform> OptimizerG2O::optimize(
 						pose = a.linear();
 						pose.translation() = a.translation();
 						v3->setEstimate(pose);
+						if(id == rootId && !hasGravityConstraints)
+						{
+							UDEBUG("Set %d fixed", id);
+							v3->setFixed(true);
+						}
 						vertex = v3;
 						isLandmarkWithRotation.insert(std::make_pair(id, true));
 						id = landmarkVertexOffset - id;
@@ -422,8 +446,41 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					continue;
 				}
 			}
-			vertex->setId(id);
-			UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
+			if(vertex == 0)
+			{
+				UERROR("Could not create vertex for node %d", id);
+			}
+			else
+			{
+				vertex->setId(id);
+				UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
+				if(!isSlam2d() && id == rootId && hasGravityConstraints)
+				{
+					g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
+					g2o::VertexSE3* v1 = (g2o::VertexSE3*)vertex;
+					priorEdge->setVertex(0, v1);
+					Eigen::Affine3d a = iter->second.toEigen3d();
+					Eigen::Isometry3d pose;
+					pose = a.linear();
+					pose.translation() = a.translation();
+					priorEdge->setMeasurement(pose);
+					priorEdge->setParameterId(0, PARAM_OFFSET);
+					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
+					// pitch and roll not fixed
+					information(3,3) = information(4,4) = 1;
+					priorEdge->setInformation(information);
+					if (priorEdge && !optimizer.addEdge(priorEdge))
+					{
+						delete priorEdge;
+						UERROR("Map: Failed adding fixed constraint of rootid %d, set as fixed instead", id);
+						v1->setFixed(true);
+					}
+					else
+					{
+						UDEBUG("Set %d fixed with prior (have gravity constraints)", id);
+					}
+				}
+			}
 		}

 		UDEBUG("fill edges to g2o...");
@@ -106,28 +106,35 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		gtsam::NonlinearFactorGraph graph;

 		// detect if there is a global pose prior set, if so remove rootId
-		bool gpsPriorOnly = false;
-		bool hasPriorPoses = false;
-		if(!priorsIgnored())
+		bool hasGPSPrior = false;
+		bool hasGravityConstraints = false;
+		if(!priorsIgnored() || (!isSlam2d() && gravitySigma() > 0))
 		{
 			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 			{
-				if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
+				if(iter->second.from() == iter->second.to())
 				{
-					hasPriorPoses = true;
-					if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
-						(1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
-						 1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
-						 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0))
+					if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
 					{
-						// orientation is set, don't set root prior
-						gpsPriorOnly = false;
-						rootId = 0;
-						break;
+						hasGPSPrior = true;
+						if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
+							(1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
+							 1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
+							 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0))
+						{
+							// orientation is set, don't set root prior (it is no GPS)
+							rootId = 0;
+							hasGPSPrior = false;
+							break;
+						}
 					}
-					else if(gravitySigma()<=0)
+					if(iter->second.type() == Link::kGravity)
 					{
-						gpsPriorOnly = true;
+						hasGravityConstraints = true;
+						if(priorsIgnored())
+						{
+							break;
+						}
 					}
 				}
 			}
@@ -138,25 +145,25 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			UASSERT(uContains(poses, rootId));
 			const Transform & initialPose = poses.at(rootId);
-			UDEBUG("hasPriorPoses=%s, gpsPriorOnly=%s", hasPriorPoses?"true":"false", gpsPriorOnly?"true":"false");
+			UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
 			if(isSlam2d())
 			{
-				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasPriorPoses?1e-2:std::numeric_limits<double>::min()));
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
 				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
 			}
 			else
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
 						(gtsam::Vector(6) <<
-								1e-2, 1e-2, hasPriorPoses?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
-								(gpsPriorOnly?2:1e-2), gpsPriorOnly?2:1e-2, gpsPriorOnly?2:1e-2 // xyz
+								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), hasGPSPrior?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
+								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
 			}
 		}

-		UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d gpsPriorOnly=%d landmarksIgnored=%d)",
-				rootId, priorsIgnored()?1:0, gpsPriorOnly?1:0, landmarksIgnored()?1:0);
+		UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
+				rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
 		gtsam::Values initialEstimate;
 		std::map<int, bool> isLandmarkWithRotation;
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -2823,7 +2823,13 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
 	}
 }

-struct ProjectionInfo {
+class ProjectionInfo {
+public:
+	ProjectionInfo():
+		nodeID(-1),
+		cameraIndex(-1),
+		distance(-1)
+	{}
 	int nodeID;
 	int cameraIndex;
 	pcl::PointXY uv;
@@ -2845,6 +2851,12 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
+	UINFO("cloud=%d points", (int)cloud.size());
+	UINFO("cameraPoses=%d", (int)cameraPoses.size());
+	UINFO("cameraModels=%d", (int)cameraModels.size());
+	UINFO("maxDistance=%f", maxDistance);
+	UINFO("maxAngle=%f", maxAngle);
+	UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
 	std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;

 	if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
@@ -2859,11 +2871,11 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		return pointToPixel;
 	}

-	std::vector<std::vector<ProjectionInfo> > invertedIndex(cloud.size()); // For each point: list of cameras
+	std::vector<ProjectionInfo> invertedIndex(cloud.size()); // For each point: list of cameras
 	int cameraProcessed = 0;
 	for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
 	{
-		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.begin();
+		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.find(pter->first);
 		if(iter!=cameraModels.end() && !iter->second.empty())
 		{
 			for(size_t i=0; i<iter->second.size(); ++i)
@@ -2899,7 +2911,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 					// re-project in camera frame
 					float z = ptScan.z;
 					bool set = false;
-					if(z > 0.0f)
+					if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
 					{
 						float invZ = 1.0f/z;
 						float dx = (fx*ptScan.x)*invZ + cx;
@@ -2957,8 +2969,39 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 							info.cameraIndex = i;
 							info.uv.x = float(u)/float(imageSize.width);
 							info.uv.y = float(v)/float(imageSize.height);
-							info.distance = zReg[0]/1000.0f;
-							invertedIndex[zReg[1]].push_back(info);
+							const Transform & cam = cameraPoses.at(info.nodeID);
+							const PointT & pt = cloud.at(zReg[1]);
+							Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
+							Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
+							float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
+							float distanceToCam = zReg[0]/1000.0f;
+							if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
+								(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
+							{
+								float vx = info.uv.x-0.5f;
+								float vy = info.uv.y-0.5f;
+
+								float distanceToCenter = vx*vx+vy*vy;
+								float distance = distanceToCenter;
+								if(distanceToCamPolicy)
+								{
+									distance = distanceToCam;
+								}
+
+								info.distance = distance;
+
+								if(invertedIndex[zReg[1]].distance != -1.0f)
+								{
+									if(distance <= invertedIndex[zReg[1]].distance)
+									{
+										invertedIndex[zReg[1]] = info;
+									}
+								}
+								else
+								{
+									invertedIndex[zReg[1]] = info;
+								}
+							}
 						}
 					}
 				}
@@ -2991,50 +3034,14 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	// For each point
 	for(size_t i=0; i<invertedIndex.size(); ++i)
 	{
-		if((i+1)%10000 == 0)
-		{
-			UDEBUG("Point %d/%d", i+1, (int)cloud.size());
-			if(state && !state->callback(uFormat("%d/%d points projected to cameras (out of %d points)", colorized, i+1, (int)cloud.size())))
-			{
-				//cancelled!
-				UWARN("Projecting to camera cancelled!");
-				pointToPixel.clear();
-				return pointToPixel;
-			}
-		}
-
-		const PointT & pt = cloud.at(i);
 		int nodeID = -1;
 		int cameraIndex = -1;
-		float smallestWeight = std::numeric_limits<float>::max();
 		pcl::PointXY uv_coords;
-		for (size_t j = 0; j<invertedIndex[i].size(); ++j)
+		if(invertedIndex[i].distance > -1.0f)
 		{
-			const Transform & cam = cameraPoses.at(invertedIndex[i][j].nodeID);
-			Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
-			Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
-			float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
-			float distanceToCam = invertedIndex[i][j].distance;
-			if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
-				(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
-			{
-				float vx = invertedIndex[i][j].uv.x-0.5f;
-				float vy = invertedIndex[i][j].uv.y-0.5f;
-
-				float distanceToCenter = vx*vx+vy*vy;
-				float distance = distanceToCenter;
-				if(distanceToCamPolicy)
-				{
-					distance = distanceToCam;
-				}
-				if(distance <= smallestWeight)
-				{
-					nodeID = invertedIndex[i][j].nodeID;
-					cameraIndex = invertedIndex[i][j].cameraIndex;
-					smallestWeight = distance;
-					uv_coords = invertedIndex[i][j].uv;
-				}
-			}
+			nodeID = invertedIndex[i].nodeID;
+			cameraIndex = invertedIndex[i].cameraIndex;
+			uv_coords = invertedIndex[i].uv;
 		}

 		if(nodeID>-1 && cameraIndex> -1)
@@ -3046,7 +3053,12 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		}
 	}

-	UINFO("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
+	msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
+	UINFO(msg.c_str());
+	if(state)
+	{
+		state->callback(msg);
+	}

 	return pointToPixel;
 }
@@ -330,8 +330,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 	{
 		//Get map size
 		float margin = cellSize*10.0f;
-		xMin = minX-margin;
-		yMin = minY-margin;
+		xMin = minX-margin-cellSize/2.0f;
+		yMin = minY-margin-cellSize/2.0f;
 		float xMax = maxX+margin;
 		float yMax = maxY+margin;
 		if(fabs((yMax - yMin) / cellSize) > 30000 || // Max 1.5Km/1.5Km at 5 cm/cell -> 900MB
@@ -601,71 +601,72 @@ pcl::texture_mapping::CameraVector createTextureCameras(
 		const std::map<int, cv::Mat> & cameraDepths,
 		const std::vector<float> & roiRatios)
 {
-	UASSERT_MSG(poses.size() == cameraModels.size(), uFormat("%d vs %d", (int)poses.size(), (int)cameraModels.size()).c_str());
 	UASSERT(roiRatios.empty() || roiRatios.size() == 4);
 	pcl::texture_mapping::CameraVector cameras;
-	std::map<int, Transform>::const_iterator poseIter=poses.begin();
-	std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.begin();
-	for(; poseIter!=poses.end(); ++poseIter, ++modelIter)
+
+	for(std::map<int, Transform>::const_iterator poseIter=poses.begin(); poseIter!=poses.end(); ++poseIter)
 	{
-		UASSERT(poseIter->first == modelIter->first);
+		std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.find(poseIter->first);

-		std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);
-
-		// for each sub camera
-		for(unsigned int i=0; i<modelIter->second.size(); ++i)
+		if(modelIter!=cameraModels.end())
 		{
-			pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
-			// should be in camera frame
-			UASSERT(!modelIter->second[i].localTransform().isNull() && !poseIter->second.isNull());
-			Transform t = poseIter->second*modelIter->second[i].localTransform();
+			std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);

-			cam.pose = t.toEigen3f();
-
-			if(modelIter->second[i].imageHeight() <=0 || modelIter->second[i].imageWidth() <=0)
+			// for each sub camera
+			for(unsigned int i=0; i<modelIter->second.size(); ++i)
 			{
-				UERROR("Should have camera models with width/height set to create texture cameras!");
-				return pcl::texture_mapping::CameraVector();
-			}
+				pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
+				// should be in camera frame
+				UASSERT(!modelIter->second[i].localTransform().isNull() && !poseIter->second.isNull());
+				Transform t = poseIter->second*modelIter->second[i].localTransform();

-			UASSERT(modelIter->second[i].fx()>0 && modelIter->second[i].imageHeight()>0 && modelIter->second[i].imageWidth()>0);
-			cam.focal_length_w=modelIter->second[i].fx();
-			cam.focal_length_h=modelIter->second[i].fy();
-			cam.center_w=modelIter->second[i].cx();
-			cam.center_h=modelIter->second[i].cy();
-			cam.height=modelIter->second[i].imageHeight();
-			cam.width=modelIter->second[i].imageWidth();
-			if(modelIter->second.size() == 1)
-			{
-				cam.texture_file = uFormat("%d", poseIter->first); // camera index
-			}
-			else
-			{
-				cam.texture_file = uFormat("%d_%d", poseIter->first, (int)i); // camera index, sub camera model index
-			}
-			if(!roiRatios.empty())
-			{
-				cam.roi.resize(4);
-				cam.roi[0] = cam.width * roiRatios[0]; // left -> x
-				cam.roi[1] = cam.height * roiRatios[2]; // top -> y
-				cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0]; // right -> width
-				cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1]; // bottom -> height
-			}
+				cam.pose = t.toEigen3f();

-			if(depthIter != cameraDepths.end() && !depthIter->second.empty())
-			{
-				UASSERT(depthIter->second.type() == CV_32FC1 || depthIter->second.type() == CV_16UC1);
-				UASSERT(depthIter->second.cols % modelIter->second.size() == 0);
-				int subWidth = depthIter->second.cols/(modelIter->second.size());
-				cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
+				if(modelIter->second[i].imageHeight() <=0 || modelIter->second[i].imageWidth() <=0)
+				{
+					UERROR("Should have camera models with width/height set to create texture cameras!");
+					return pcl::texture_mapping::CameraVector();
+				}
+
+				UASSERT(modelIter->second[i].fx()>0 && modelIter->second[i].imageHeight()>0 && modelIter->second[i].imageWidth()>0);
+				cam.focal_length_w=modelIter->second[i].fx();
+				cam.focal_length_h=modelIter->second[i].fy();
+				cam.center_w=modelIter->second[i].cx();
+				cam.center_h=modelIter->second[i].cy();
+				cam.height=modelIter->second[i].imageHeight();
+				cam.width=modelIter->second[i].imageWidth();
+				if(modelIter->second.size() == 1)
+				{
+					cam.texture_file = uFormat("%d", poseIter->first); // camera index
+				}
+				else
+				{
+					cam.texture_file = uFormat("%d_%d", poseIter->first, (int)i); // camera index, sub camera model index
+				}
+				if(!roiRatios.empty())
+				{
+					cam.roi.resize(4);
+					cam.roi[0] = cam.width * roiRatios[0]; // left -> x
+					cam.roi[1] = cam.height * roiRatios[2]; // top -> y
+					cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0]; // right -> width
+					cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1]; // bottom -> height
+				}
+
+				if(depthIter != cameraDepths.end() && !depthIter->second.empty())
+				{
+					UASSERT(depthIter->second.type() == CV_32FC1 || depthIter->second.type() == CV_16UC1);
+					UASSERT(depthIter->second.cols % modelIter->second.size() == 0);
+					int subWidth = depthIter->second.cols/(modelIter->second.size());
+					cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
+				}
+
+				UDEBUG("%f", cam.focal_length);
+				UDEBUG("%f", cam.height);
+				UDEBUG("%f", cam.width);
+				UDEBUG("cam.pose=%s", t.prettyPrint().c_str());
+
+				cameras.push_back(cam);
 			}
-
-			UDEBUG("%f", cam.focal_length);
-			UDEBUG("%f", cam.height);
-			UDEBUG("%f", cam.width);
-			UDEBUG("cam.pose=%s", t.prettyPrint().c_str());
-
-			cameras.push_back(cam);
 		}
 	}
 	return cameras;
@@ -2231,6 +2232,52 @@ bool multiBandTexturing(
 		const std::pair<float, float> & contrastValues,               // optional output of util3d::mergeTextures()
 		bool gainRGB)
 {
+	return multiBandTexturing(
+			outputOBJPath,
+			cloud,
+			polygons,
+			cameraPoses,
+			vertexToPixels,
+			images,
+			cameraModels,
+			memory,
+			dbDriver,
+			textureSize,
+			2,
+			"1 5 10 0",
+			textureFormat,
+			gains,
+			blendingGains,
+			contrastValues,
+			gainRGB);
+}
+
+bool multiBandTexturing(
+		const std::string & outputOBJPath,
+		const pcl::PCLPointCloud2 & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const std::map<int, Transform> & cameraPoses,
+		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
+		const std::map<int, cv::Mat> & images,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		const Memory * memory,
+		const DBDriver * dbDriver,
+		unsigned int textureSize,
+		unsigned int textureDownScale,
+		const std::string & nbContrib,
+		const std::string & textureFormat,
+		const std::map<int, std::map<int, cv::Vec4d> > & gains,
+		const std::map<int, std::map<int, cv::Mat> > & blendingGains,
+		const std::pair<float, float> & contrastValues,
+		bool gainRGB,
+		unsigned int unwrapMethod,
+		bool fillHoles,
+		unsigned int padding,
+		double bestScoreThreshold,
+		double angleHardThreshold,
+		bool forceVisibleByAllVertices)
+{
+
 #ifdef RTABMAP_ALICE_VISION
 	if(ULogger::level() == ULogger::kDebug)
 	{
@@ -2265,8 +2312,29 @@ bool multiBandTexturing(
 	texturing.pointsVisibilities = new mesh::PointsVisibility();
 	texturing.pointsVisibilities->reserve(cloud2.size());
 #endif
-	texturing.texParams.textureSide = 8192;
-	texturing.texParams.downscale = 8192/textureSize;
+	texturing.texParams.textureSide = textureSize;
+	texturing.texParams.downscale = textureDownScale;
+	std::vector<int> multiBandNbContrib;
+	std::list<std::string> values = uSplit(nbContrib, ' ');
+	for(std::list<std::string>::iterator iter=values.begin(); iter!=values.end(); ++iter)
+	{
+		multiBandNbContrib.push_back(uStr2Int(*iter));
+	}
+	if(multiBandNbContrib.size() != 4)
+	{
+		UERROR("multiband: Wrong number of nb of contribution (vaue=\"%s\", should be 4), using default values instead.", nbContrib.c_str());
+	}
+	else
+	{
+		texturing.texParams.multiBandNbContrib = multiBandNbContrib;
+	}
+	texturing.texParams.padding = padding;
+	texturing.texParams.fillHoles = fillHoles;
+	texturing.texParams.bestScoreThreshold = bestScoreThreshold;
+	texturing.texParams.angleHardThreshold = angleHardThreshold;
+	texturing.texParams.forceVisibleByAllVertices = forceVisibleByAllVertices;
+	texturing.texParams.visibilityRemappingMethod = mesh::EVisibilityRemappingMethod::Pull;
+

 	for(size_t i=0;i<cloud2.size();++i)
 	{
@@ -2428,13 +2496,20 @@ bool multiBandTexturing(
 				imageRoi = output;
 			}

-			Transform t = iter->second * model.localTransform();
-			Eigen::Matrix<double, 3, 4> m = (t.inverse()).toEigen3d().matrix().block<3,4>(0, 0);
+			Transform t = (iter->second * model.localTransform()).inverse();
+			Eigen::Matrix<double, 3, 4> m = t.toEigen3d().matrix().block<3,4>(0, 0);
 			sfmData::CameraPose pose(geometry::Pose3(m), true);
 			sfmData.setAbsolutePose((IndexT)viewId, pose);

+			UDEBUG("%d %d %f %f %f %f", imageSize.width, imageSize.height, model.fx(), model.fy(), model.cx(), model.cy());
 			std::shared_ptr<camera::IntrinsicBase> camPtr = std::make_shared<camera::Pinhole>(
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=4)
+					//https://github.com/alicevision/AliceVision/commit/9fab5c79a1c65595fe5c5001267e1c5212bc93f0#diff-b0c0a3c30de50be8e4ed283dfe4c8ae4a9bc861aa9a83bd8bfda8182e9d67c08
+					// [all] the camera principal point is now defined as an offset relative to the image center
+					imageSize.width, imageSize.height, model.fx(), model.fy(), model.cx() - double(imageSize.width) * 0.5, model.cy() - double(imageSize.height) * 0.5);
+#else
 					imageSize.width, imageSize.height, model.fx(), model.cx(), model.cy());
+#endif
 			sfmData.intrinsics.insert(std::make_pair((IndexT)viewId, camPtr));

 			std::string imagePath = tmpImageDirectory+uFormat("/%d.jpg", viewId);
@@ -2456,14 +2531,18 @@ bool multiBandTexturing(

 	mvsUtils::MultiViewParams mp(sfmData);

-	UINFO("Unwrapping...");
-	texturing.unwrap(mp, mesh::EUnwrapMethod::Basic);
+	UINFO("Unwrapping (method=%d=%s)...", unwrapMethod, mesh::EUnwrapMethod_enumToString((mesh::EUnwrapMethod)unwrapMethod).c_str());
+	texturing.unwrap(mp, (mesh::EUnwrapMethod)unwrapMethod);
 	UINFO("Unwrapping done. %fs", timer.ticks());

 	// save final obj file
 	std::string baseName = uSplit(UFile::getName(outputOBJPath), '.').front();
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=4)
+	texturing.saveAs(outputDirectory, baseName, aliceVision::mesh::EFileType::OBJ, imageIO::EImageFileType::PNG);
+#else
 	texturing.saveAsOBJ(outputDirectory, baseName);
-	UINFO("Saved %s. %fs", outputOBJPath, timer.ticks());
+#endif
+	UINFO("Saved %s. %fs", outputOBJPath.c_str(), timer.ticks());

 	// generate textures
 	UINFO("Generating textures...");
@@ -2525,7 +2604,9 @@ bool multiBandTexturing(
 	UINFO("Rename/convert textures... done. %fs", timer.ticks());

 #if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+	UINFO("Cleanup sfmdata...");
 	sfmData.clear();
+	UINFO("Cleanup sfmdata... done. %fs", timer.ticks());
 #endif

 	return true;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	a3823594a4	Converted an assert to an error.	2021-12-26 14:53:22 -05:00
matlabbe	3071da42f3	Optimizer: don't fix roll/pitch on root node if gravity constraints are fed	2021-12-25 16:57:03 -05:00
matlabbe	93ee8f9b30	Working dir path: convert ~ to Home for convenience. Localization: don't show "cannot optimize" warning when RGBD/MaxOdomCacheSize=0	2021-12-24 18:24:44 -05:00
matlabbe	7ca881453e	RGBD/StartAtOrigin: set first node of the graph, not Identity	2021-12-20 17:01:08 -05:00
matlabbe	8662eb0dd7	Statistics: added OdomCache data for debugging	2021-12-18 18:53:59 -05:00
matlabbe	33130890fd	Localization: Improved resulting pose in case there are gravity constraints. iOS: clear odom trace when not visible, fixed opt mesh not correctly aligned with graph on loading when switching RGBD/OptimizeFromGraphEnd.	2021-12-18 15:37:49 -05:00
matlabbe	bca8b30832	Localization 2d Slam: automatically rotate landmark links to have z-axis up for correct 3DoF optimization. When RGBD/MaxOdomCacheSize is used, wait for at least 2 temporal localizations before adjusting the pose (to avoid big jumps when only one constraint is used).	2021-12-17 11:00:37 -05:00
matlabbe	6eb81cbb72	Added RGBD/MaxOdomCacheSize option to iOS App.	2021-12-12 20:40:28 -05:00
matlabbe	0e62050824	rtabmap: fixed graph re-optimized with only virtual links in localization mode (which can make gtsam crash because of under constrained covariance)	2021-12-12 13:07:02 -05:00
matlabbe	cf5e90238b	Statistics: added LoopOdom_correction stats for landmark detections.	2021-12-10 21:58:11 -05:00
matlabbe	8cf12c6135	Improved localization mode accuracy (decreasing jumps on consecutive loop closures or landmark detections). Updated usage of parameter RGBD/MaxOdomCacheSize (default 0->10)	2021-12-10 17:48:36 -05:00
matlabbe	4ab0090ecd	Support feature-only rectification when using external extracted features.	2021-12-06 09:26:10 -05:00
matlabbe	580e35afb1	Fixed missing 3D keypoints when RGBD/LoopClosureReextractFeatures=true and Reg/Strategy=1 (https://github.com/introlab/rtabmap_ros/issues/668 ). DBViewer: fixed wrong poses optimization with GTSAM when showing scans of loop closures by proximity by space (multiscan) while there are GPS priors.	2021-12-05 17:38:53 -05:00
matlabbe	12906f4490	DBViewer: export poses: added explicit otion for ground truth if available	2021-12-03 13:43:53 -05:00
matlabbe	baae713471	Fixed unknown lines for octomap 2D grid projection. (https://github.com/introlab/rtabmap_ros/issues/684 )	2021-11-30 20:34:51 -05:00
matlabbe	67710ef94c	Export: optimized camera projection RAM usage. Added --texture_angle and --cam_projection_decimation options.	2021-11-21 17:10:57 -05:00
matlabbe	090ae0c444	DbViewer: added option to show disparity instead of right image in main views for stereo data	2021-11-20 16:36:54 -05:00
matlabbe	6684bafe34	fixed typo	2021-11-16 18:22:45 -05:00
matlabbe	5fcbe2ed70	iOS: fixed install script (#785 #741 )	2021-11-16 18:11:40 -05:00
matlabbe	459c7b2bd7	Export: added --min_cluster option.	2021-11-16 11:58:55 -05:00
matlabbe	9ae2c46546	CMake: fixed build without Python and Ceres if WITH_PYTHON and WITH_CERES are OFF (even if found by third party libraries, related to #783 ).	2021-11-15 18:00:17 -05:00
matlabbe	7af2a27e89	Improved log error when ROI is set with SuperPoint (https://github.com/introlab/rtabmap_ros/issues/676 )	2021-11-14 20:26:42 -05:00
matlabbe	dbecaac809	Moved bin dir inside build directory (#784 ) * Moved bin directory inside build directory (to make easier different builds with same source directory) * Switched include order to avoid problems with remaining Version.h still in source directory taen before the one in binary dir. Fixed android build (updated res tool search path). * workflow-cmake: fixed path to bin directory	2021-11-14 19:29:37 -05:00
matlabbe	6c07a670ee	Added OdometryOpen3D	2021-11-13 19:45:57 -05:00
matlabbe	4ba805b5b3	Report: fixed landmarks not used during optimization. Reprocess: added --nolandmark option to ignore landmarks in input database.	2021-11-12 12:47:54 -05:00
matlabbe	b002e85e0f	Update ProgressDialog.h Typo param name should be in seconds, not milliseconds.	2021-11-09 17:36:34 -05:00
matlabbe	ec2aa5c952	OpenNI2: depth shift can be negative. MainWindow: postProcessing() refactoring (split with and without dialog).	2021-11-09 10:16:12 -05:00
matlabbe	06150c697f	Fixed #750	2021-11-08 14:20:21 -05:00
matlabbe	38bcb0060c	CMake: set default to OFF for some optional dependencies that require specific versions or patches before integrating with rtabmap, otherwise there could be seg faults on runtime even if compilation worked.	2021-11-08 13:47:27 -05:00
matlabbe	ddd5eb5a41	CameraRealSense2: Odom extrinsics against another sensor can be calibrated with having to calibrate stereo first. DBViewer: fixed local grid wrongly using global grid parameters.	2021-11-05 18:05:50 -04:00
matlabbe	20bc281db7	DbViewer: don't show landmark links for ignored ndoes (w=-9), auto-zoom grid map if there are a lot of unknowns. Updated Docker nvidia files to include default Documents/RTAB-Map directory.	2021-11-04 13:00:08 -04:00
matlabbe	7c5acd8970	Parameters: Changed Grid/FromDepth to Grid/Sensor to add a new choice to use both scan and depth for local grids. Increased version to 0.20.15.	2021-10-29 20:05:12 -04:00
matlabbe	1886f99cbf	Merge branch 'RobotnikAutomation-master'	2021-10-29 13:58:14 -04:00
matlabbe	2ad334df90	Added filter_floor and adjusted default values.	2021-10-29 13:58:00 -04:00
matlabbe	0ab5a8f43e	Merge branch 'master' of https://github.com/RobotnikAutomation/rtabmap into RobotnikAutomation-master	2021-10-29 13:57:16 -04:00
matlabbe	8682026396	Rtabmap: when optimizing graph, removed guess for rootid to avoid global rotation drift over time. CameraStereoImages: set BayerMode also to right image. GraphView: fixed 0 width for line inside NodeItem. Reprocess: added -loc_null and -gt options.	2021-10-23 11:17:42 -04:00
Ines	6cb741667d	Add ceiling filter to rtabmap-export	2021-10-22 12:36:53 +02:00
matlabbe	56c5622d20	docker: added "make -j12" for rtabmap build	2021-10-16 18:14:24 -04:00
matlabbe	e245782c6c	worflows: added back arm64	2021-10-15 14:58:56 -04:00
matlabbe	9895e4c162	workflow: test images without arm64	2021-10-15 09:29:34 -04:00
matlabbe	f67087075a	Updated docker images (removed amd64 script)	2021-10-14 21:49:50 -04:00
matlabbe	d0242b14cf	DetectMoreLoopClosures CLI: show overridden parameters	2021-10-11 13:52:55 -04:00
matlabbe	d16e24a1a3	Fixed compiler warning	2021-10-07 10:19:32 -04:00
matlabbe	ab5fd5018b	DbViewer: added update all landmark covariances menu action, also enabled edit constraint on landmark links. Covariance can be set to 9999.	2021-10-06 11:34:40 -04:00
matlabbe	44810a14e0	Show if built with PDAL on --version option	2021-10-01 11:49:45 -04:00
matlabbe	697fe5ebb3	Workflows: updated docker multi-arch (removed armv7)	2021-09-30 18:00:17 -04:00
matlabbe	54c0ee4244	Workflows: added multi-arch docker images	2021-09-30 15:37:05 -04:00
matlabbe	d284cd11cf	Updated package.xml version to 0.20.14	2021-09-30 10:14:24 -04:00
matlabbe	467ea42981	OdometryFLOAM: fixed published local scan map local transform	2021-09-30 10:05:56 -04:00
matlabbe	77d947d4fb	Fixed FLOAM not working when local transform is not Identity. DBViewer: warn user when scan from dpeth is checked and there are no depth images in db.	2021-09-30 09:53:50 -04:00
matlabbe	533d78d570	Fixed OdometryOpenVINS build errors with latest OpenVINS code.	2021-09-28 17:12:08 -04:00
matlabbe	aee034c5ed	Update CMakeLists.txt Setting WITH_FLOAM to OFF by default because floam binaries (this [version](https://github.com/flynneva/floam)) in ros is not compatible.	2021-09-27 20:20:11 -04:00
matlabbe	0092e15cd7	Update docker.yml	2021-09-27 20:04:12 -04:00
matlabbe	23d9e0e4bb	Docker: updated bionic/focal's geogram patch	2021-09-27 19:48:18 -04:00
matlabbe	8c56b5b1ce	CMake: added WITH_OPENMP option (to be able to disable it). CameraOpenNI2: Added depth decimation parameter. CLAMS: can apply distortion model to smaller images.	2021-09-24 17:31:16 -04:00
matlabbe	bccc5b13af	FLOAM: added some debug logs	2021-09-24 11:32:25 -04:00
matlabbe	5f65618d40	Added OdomLOAM/Resolution parameter	2021-09-22 10:48:59 -04:00
matlabbe	cff0d15460	Fixed "wrong scan number" error when OdomLOAM/Sensor is 0 (VLP16)	2021-09-22 10:06:34 -04:00
matlabbe	b6671f4d8c	workflow/docker: added CACHE_DATE to android builds to avoid caching rtabmap build	2021-09-20 16:02:37 -04:00
matlabbe	9db66600b3	workflow: added android23 docker image	2021-09-20 15:46:14 -04:00
matlabbe	67cd4b69c1	workflow: updated docker tags / cache var	2021-09-20 13:34:30 -04:00
matlabbe	cffb7981b6	docker: fixed bionic build with latest alicevision (cmake>=3.11 required)	2021-09-20 12:32:51 -04:00
matlabbe	71ffd922ed	workflow: fixed typo	2021-09-20 11:37:58 -04:00
matlabbe	870467393b	workflow: add docker buildcache	2021-09-20 11:33:19 -04:00
matlabbe	79f203a2c4	docker workflow: fixed build matrix	2021-09-20 11:16:12 -04:00
matlabbe	baa5b638ae	Added "docker" workflow	2021-09-20 11:11:21 -04:00
matlabbe	8f12463f71	Docker: upgraded alicevision version 2.4.0 in bionic/focal images	2021-09-19 20:42:56 -04:00
matlabbe	14b56813d3	Updated for AliceVision >=2.4.0 compatibility. Export: added --multiband_contrib option.	2021-09-19 20:20:45 -04:00
matlabbe	44b057b0d7	DbViewer: fixed icp from depth option not used when refining or adding loop closures automaticaly	2021-09-13 15:59:19 -04:00
matlabbe	a901f20d06	0.20.14: added globalBundleAdjustment CLI, added Rtabmap/Memory::cleanupLocalGrids function, init with optimizedPoses from db even in mapping mode, reprocess: added -db option to save optimized 2d grid in database.	2021-09-11 11:35:43 -04:00
matlabbe	3ba02d2ef6	Added OdometryFLOAM (Odom/Strategy=11)	2021-09-09 17:38:54 -04:00
matlabbe	263e0170f1	export: refactored ba (to support stereo data)	2021-09-09 14:22:02 -04:00
matlabbe	e017a0fcf4	DbViewer: fixed initial rootid with older databases, fixed RGBD/OptimizeFromGraphEnd not correctly used.	2021-09-08 18:13:20 -04:00
matlabbe	103db3181b	Update README.md	2021-09-08 17:13:13 -04:00
matlabbe	bc253df24a	Update README.md	2021-09-08 17:11:34 -04:00
matlabbe	544ea9dff2	Update cmake.yml	2021-09-08 13:31:00 -04:00
matlabbe	09c2c4bbcb	Removed travis config, now use Github actions (see .github/workflows/cmake.yml) fixed #768	2021-09-08 12:09:59 -04:00
matlabbe	c209cf1c9b	Update cmake.yml Added info after build	2021-09-08 12:06:14 -04:00
matlabbe	202d59b408	Update cmake.yml Added ros setup.bash before cmake	2021-09-08 11:59:04 -04:00
matlabbe	9671daf9c3	Update cmake.yml	2021-09-08 11:53:13 -04:00
matlabbe	b5518ff618	Update cmake.yml	2021-09-08 11:46:29 -04:00
matlabbe	2111b6497b	Update cmake.yml use setup-ros action	2021-09-08 11:37:03 -04:00
matlabbe	b51b2525a5	Added Github actions for melodic/focal	2021-09-08 11:24:11 -04:00
matlabbe	45d51808e3	💄	2021-09-08 09:38:16 -04:00
matlabbe	bc39b19517	projectCloudToCamerasImpl: fixed bug using wrong camera models	2021-09-08 09:34:21 -04:00
matlabbe	7baedf4c72	texturing: remove assert when poses and models are not the same size (just ignore poses without models, intermediate nodes issue)	2021-09-07 16:26:56 -04:00
matlabbe	20361400e1	export: output intensity channel with RGB when --cam_projection and --scan options are set (PDAL required)	2021-09-07 15:57:23 -04:00
matlabbe	c43118cde8	Fixed F2F-Optical flow not creating keyframes bug	2021-09-03 15:33:22 -04:00
matlabbe	371a3ef851	fixed CleanupLocalGrids install target	2021-09-01 10:23:45 -04:00
matlabbe	daefc5ff54	Added rtabmap-cleanupLocalGrids CLI	2021-08-28 22:19:59 -04:00
matlabbe	b932da6dcf	DbViewer: Adjusted GraphView's root id based on latest valid node on initialization	2021-08-28 20:23:44 -04:00
matlabbe	f88ce1618b	OdometryORBSLAM3: fixed re-initilisation of camera parameters when restarting camera with different config.	2021-08-26 15:34:48 -04:00
matlabbe	c5e4d67f80	export: don't assert if gain is zero (means disabled)	2021-08-16 20:27:31 -04:00
matlabbe	ed68fe777b	Added more options for multiband texturing	2021-08-16 13:14:02 -04:00
matlabbe	6fb553a5e7	Update README.md	2021-08-13 14:46:56 -04:00
matlabbe	db00e04cc1	rtabmap-info: updated to show parameters not in the database.	2021-08-12 10:55:08 -04:00
matlabbe	eeecb21793	export tool: added --camera_projection_keep_all option	2021-08-03 13:46:39 -04:00
matlabbe	a5685c3e31	Fixed build without DepthAI dep	2021-07-28 17:05:16 -04:00
matlabbe	138d4aa1be	If RGBD/LoopClosureReextractFeatures=true, don't save raw features (3d point, descriptor) to Feature table.	2021-07-28 14:32:32 -04:00
matlabbe	79d2b3fade	Merge branch 'master' of https://github.com/introlab/rtabmap	2021-07-28 14:19:49 -04:00
matlabbe	32bf0f9d61	Fixed CameraDepthAI with latest depthai version (using now camera eeprom calibration and added imu support)	2021-07-28 14:19:30 -04:00
matlabbe	22a771e29c	trigger travis.com	2021-07-20 14:09:40 -04:00
matlabbe	17d8a92614	Update README.md	2021-07-20 12:58:42 -04:00
matlabbe	a91cd0c659	Update README.md	2021-07-20 12:58:16 -04:00
matlabbe	ae92ec40d7	Fixed build with LOAM dependency	2021-07-14 13:22:47 -04:00