bump 0.20.18

CCCorelib: updated wrong output check. Updated RGBD/MaxOdomCacheSize approach to work properly with Rtabmap/CreateIntermediateNodes=true.
fixed build
2026-09-13 15:00:21 +08:00 · 2022-01-28 17:12:24 -05:00 · 2022-01-28 14:48:32 -05:00 · 2022-01-27 22:55:59 -05:00 · 2022-01-27 17:57:56 -05:00 · 2022-01-26 20:12:04 -05:00
267 changed files with 9131 additions and 3211 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,96 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      matrix:
+        docker_tag: [xenial, bionic, focal, android23, android24, android26, android30]
+        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'
+        - docker_tag: android30
+          docker_tags: |
+            introlab3it/rtabmap:android30
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api30'
+    
+    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 18)
 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()

@@ -429,16 +433,29 @@ IF(WITH_DC1394)
 ENDIF(WITH_DC1394)

 IF(WITH_G2O)
-	FIND_PACKAGE(g2o QUIET NO_MODULE)
+	  FIND_PACKAGE(g2o QUIET NO_MODULE)
    IF(g2o_FOUND)
       MESSAGE(STATUS "Found g2o (targets)")
-	   SET(G2O_FOUND ${g2o_FOUND})
-	   SET(G2O_CPP11 1)
+	     SET(G2O_FOUND ${g2o_FOUND})
+	     get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
+	     MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
+	     FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+         PATHS ${G2O_INCLUDES}
+         NO_DEFAULT_PATH)
+       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+       IF(${matchres} EQUAL -1)
+           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 2)
+       ELSE()
+           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 1)
+       ENDIF()
    ELSE()
-       FIND_PACKAGE(G2O QUIET)
-       IF(G2O_FOUND)
-		 MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
-       ENDIF(G2O_FOUND)
+        FIND_PACKAGE(G2O QUIET)
+        IF(G2O_FOUND)
+            MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+        ENDIF(G2O_FOUND)
    ENDIF()
 ENDIF(WITH_G2O)

@@ -471,7 +488,7 @@ ENDIF(WITH_POINTMATCHER)
 IF(libpointmatcher_FOUND OR GTSAM_FOUND)
    find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
    IF(Boost_MINOR_VERSION GREATER 47)
-        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer REQUIRED)
+        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED)
    ENDIF(Boost_MINOR_VERSION GREATER 47)
    IF(WIN32)
 	    MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
@@ -486,6 +503,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 +523,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 +632,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 +674,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 +722,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 +732,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
@@ -773,6 +817,7 @@ ENDIF()

 IF(NOT G2O_FOUND)
   SET(G2O "//")
+   SET(G2O_CPP_CONF "//")
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
   IF(NOT G2O_CPP11)
@@ -784,9 +829,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 +849,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 +861,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 +926,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 +996,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 +1012,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 +1056,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 +1268,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 +1322,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 +1358,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 +1531,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 +1608,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.16-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
@@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@NONFREE@#define RTABMAP_NONFREE
@TORO@#define RTABMAP_TORO
@G2O@#define RTABMAP_G2O
-@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
@GTSAM@#define RTABMAP_GTSAM
@CERES@#define RTABMAP_CERES
@VERTIGO@#define RTABMAP_VERTIGO
@@ -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
@@ -8,6 +8,7 @@
    <uses-permission android:name="android.permission.CAMERA" />
    <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
    <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
+    <uses-permission android:name="android.permission.ACCESS_MEDIA_LOCATION" />
    <uses-permission android:name="android.permission.INTERNET" />
    <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
@@ -17,6 +18,11 @@

    <!-- This is the platform API where depth16 support in android was introduced. -->
    <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
+    
+    <queries>
+        <package android:name="com.google.ar.core" />
+        <package android:name="com.huawei.ar.engine" /> 
+    </queries>

    <!-- This .apk has no Java code itself, so set hasCode to false. -->
    <application 
@@ -42,7 +48,16 @@
            <intent-filter>
                <action android:name="android.intent.action.MAIN" />
                <category android:name="android.intent.category.LAUNCHER" />
-            </intent-filter>
+		    </intent-filter>
+		    <intent-filter>
+		        <action android:name="android.intent.action.SEND" />
+		        <action android:name="android.intent.action.SEND_MULTIPLE" />
+		        <action android:name="android.intent.action.OPEN_DOCUMENT" />
+		        <category android:name="android.intent.category.DEFAULT" />
+		        <data android:mimeType="application/octet-stream" />
+		        <data android:pathPattern=".*\.db" />
+		    </intent-filter>
+		    
        </activity>

        <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>
@@ -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}
@@ -520,11 +520,11 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							cv::Mat yuv(height+height/2, width, CV_8UC1);
 							memcpy(yuv.data, plane_data, data_length);
 							memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
-							cv::cvtColor(yuv, rgb, CV_YUV2BGR_NV21);
+							cv::cvtColor(yuv, rgb, cv::COLOR_YUV2BGR_NV21);
 						}
 						else
 						{
-							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, cv::COLOR_YUV2BGR_NV21);
 						}

 						std::vector<cv::KeyPoint> kpts;
@@ -244,7 +244,7 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 				cv::Mat outputRGB;
 				if(imageData != nullptr && len>0)
 				{
-					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, cv::COLOR_YUV2BGR_NV21);
 				}

 				//Depth
@@ -171,7 +171,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
    if(textureId_ != 0 && texCoord != 0)
    {
        cv::Mat rgbImage;
-        cv::cvtColor(data.imageRaw(), rgbImage, CV_BGR2RGBA);
+        cv::cvtColor(data.imageRaw(), rgbImage, cv::COLOR_BGR2RGBA);
        
        glBindTexture(GL_TEXTURE_2D, textureId_);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
@@ -478,15 +478,15 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 				LOGD("tangoColorType=%d", tangoColorType);
 				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_RGBA2BGR);
 				}
 				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_YV12);
 				}
 				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_NV21);
 				}
 				else if(tangoColorType == 35)
 				{
@@ -64,6 +64,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/GainCompensator.h>
 #include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Recovery.h>
 #include <pcl/common/common.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
@@ -82,6 +83,43 @@ static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
 #endif

+#ifdef __ANDROID__
+#ifndef DISABLE_LOG
+//ref: https://codelab.wordpress.com/2014/11/03/how-to-use-standard-output-streams-for-logging-in-android-apps/
+static int pfd[2];
+static pthread_t thr;
+static void *thread_func(void*)
+{
+    ssize_t rdsz;
+    char buf[128];
+    while((rdsz = read(pfd[0], buf, sizeof buf - 1)) > 0) {
+        if(buf[rdsz - 1] == '\n') --rdsz;
+        buf[rdsz] = 0;  /* add null-terminator */
+        __android_log_write(ANDROID_LOG_DEBUG, LOG_TAG, buf);
+    }
+    return 0;
+}
+
+int start_logger()
+{
+    /* make stdout line-buffered and stderr unbuffered */
+    setvbuf(stdout, 0, _IOLBF, 0);
+    setvbuf(stderr, 0, _IONBF, 0);
+
+    /* create the pipe and redirect stdout and stderr */
+    pipe(pfd);
+    dup2(pfd[1], 1);
+    dup2(pfd[1], 2);
+
+    /* spawn the logging thread */
+    if(pthread_create(&thr, 0, thread_func, 0) == -1)
+        return -1;
+    pthread_detach(thr);
+    return 0;
+}
+#endif
+#endif
+
 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 {
 	rtabmap::ParametersMap parameters;
@@ -243,6 +281,12 @@ RTABMapApp::RTABMapApp() :

 	this->registerToEventsManager();
 	LOGI("RTABMapApp::RTABMapApp() end");
+
+#ifdef __ANDROID__
+#ifndef DISABLE_LOG
+	start_logger();
+#endif
+#endif
 }

 #ifndef __ANDROID__ // __APPLE__
@@ -305,9 +349,9 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	}
 }

-int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource)
+int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
-	LOGW("Opening database %s (inMemory=%d, optimize=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0);
+	LOGW("Opening database %s (inMemory=%d, optimize=%d, clearDatabase=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0, clearDatabase?1:0);
 	this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
 	rtabmapMutex_.lock();
@@ -360,11 +404,11 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	if(!databaseSource.empty())
+	if(!databasePath.empty() && UFile::exists(databasePath) && !clearDatabase)
 	{
 		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
 		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
-		if(driver->openConnection(databaseSource))
+		if(driver->openConnection(databasePath))
 		{
 			cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
@@ -416,13 +460,11 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	}

 	UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading database..."));
-	LOGI("Erasing database \"%s\"...", databasePath.c_str());
-	UFile::erase(databasePath);
-	if(!databaseSource.empty())
-	{
-		LOGI("Copying database source \"%s\" to \"%s\"...", databaseSource.c_str(), databasePath.c_str());
-		UFile::copy(databaseSource, databasePath);
-	}
+	if(clearDatabase)
+    {
+        LOGI("Erasing database \"%s\"...", databasePath.c_str());
+        UFile::erase(databasePath);
+    }

 	//Rtabmap
 	mapToOdom_.setIdentity();
@@ -448,7 +490,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,
@@ -593,7 +635,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
                                    if(renderingTextureDecimation_>1)
                                    {
                                        cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
-                                        cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+                                        cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
                                    }
                                    else
                                    {
@@ -1199,7 +1241,12 @@ int RTABMapApp::Render()
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
 	try
 	{
-		UTimer fpsTime;
+        if(camera_ == 0)
+        {
+            // We are not doing continous drawing, just measure single draw
+            fpsTime_.restart();
+        }
+        
 #ifdef DEBUG_RENDERING_PERFORMANCE
 		UTimer time;
 #endif
@@ -1399,7 +1446,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)
 					{
@@ -1439,12 +1493,12 @@ int RTABMapApp::Render()

 			main_scene_.setMeshRendering(main_scene_.hasMesh(g_optMeshId), main_scene_.hasTexture(g_optMeshId));

-			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
 			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix);
-			if(renderingTime_ < fpsTime.elapsed())
+            double fpsTime = fpsTime_.ticks();
+			if(renderingTime_ < fpsTime)
 			{
-				renderingTime_ = fpsTime.elapsed();
+				renderingTime_ = fpsTime;
 			}

 			// revert state
@@ -1571,7 +1625,7 @@ int RTABMapApp::Render()
 									{
 										cv::Size reducedSize(textureRaw.cols/renderingTextureDecimation_, textureRaw.rows/renderingTextureDecimation_);
 										LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
-										cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+										cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
 									}
 									else
 									{
@@ -1856,7 +1910,7 @@ int RTABMapApp::Render()
                                                if(renderingTextureDecimation_ > 1)
                                                {
                                                    cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
-                                                    cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+                                                    cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
 #ifdef DEBUG_RENDERING_PERFORMANCE
                                                    LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks());
 #endif
@@ -2046,13 +2100,13 @@ int RTABMapApp::Render()
 				notifyDataLoaded = true;
 			}

-			fpsTime.restart();
-			main_scene_.setFrustumVisible(camera_!=0);
+            main_scene_.setFrustumVisible(camera_!=0);
 			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh, true);
-			if(renderingTime_ < fpsTime.elapsed())
+            double fpsTime = fpsTime_.ticks();
+            if(renderingTime_ < fpsTime)
 			{
-				renderingTime_ = fpsTime.elapsed();
-			}
+				renderingTime_ = fpsTime;
+            }

 			if(rtabmapEvents.size())
 			{
@@ -2089,7 +2143,7 @@ int RTABMapApp::Render()
 			cv::Mat image(h, w, CV_8UC4);
 			glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data);
 			cv::flip(image, image, 0);
-			cv::cvtColor(image, image, CV_RGBA2BGRA);
+			cv::cvtColor(image, image, cv::COLOR_RGBA2BGRA);
 			cv::Mat roi;
 			if(w>h)
 			{
@@ -2108,16 +2162,6 @@ int RTABMapApp::Render()
 			screenshotReady_.release();
 		}

-		if((openingDatabase_ && !visualizingMesh_) || exporting_ || postProcessing_)
-		{
-			// throttle rendering max 5Hz if we are doing some processing
-			double renderTime = fpsTime.elapsed();
-			if(0.2 - renderTime > 0.0)
-			{
-				uSleep((0.2 - renderTime)*1000);
-			}
-		}
-
 		if((rtabmapThread_==0 || !rtabmapThread_->isRunning()) && lastPostRenderEventTime_ > 0.0)
 		{
 			double interval = UTimer::now() - lastPostRenderEventTime_;
@@ -2550,6 +2594,26 @@ void RTABMapApp::save(const std::string & databasePath)
 	}
 }

+bool RTABMapApp::recover(const std::string & from, const std::string & to)
+{
+    std::string errorMsg;
+    if(!databaseRecovery(from, false, &errorMsg, &progressionStatus_))
+    {
+        LOGE("Recovery Error: %s", errorMsg.c_str());
+        return false;
+    }
+    else
+    {
+        LOGI("Renaming %s to %s", from.c_str(), to.c_str());
+        if(UFile::rename(from, to) != 0)
+        {
+            LOGE("Failed renaming %s to %s", from.c_str(), to.c_str());
+            return false;
+        }
+        return true;
+    }
+}
+
 void RTABMapApp::cancelProcessing()
 {
 	UWARN("Processing canceled!");
@@ -3704,14 +3768,14 @@ void RTABMapApp::postOdometryEvent(
 					cv::Mat yuv(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1);
 					memcpy(yuv.data, yPlane, yPlaneLen);
 					memcpy(yuv.data+yPlaneLen, vPlane, rgbHeight/2*rgbWidth);
-					cv::cvtColor(yuv, outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(yuv, outputRGB, cv::COLOR_YUV2BGR_NV21);
 				}
 				else
 				{
 #ifdef __ANDROID__
-					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2BGR_NV21);
 #else // __APPLE__
-                    cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, CV_YUV2RGB_NV21);
+					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2RGB_NV21);
 #endif
 				}

@@ -75,7 +75,7 @@ class RTABMapApp : public UEventsHandler {

  void setScreenRotation(int displayRotation, int cameraRotation);

-  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);

  bool isBuiltWith(int cameraDriver) const;
 #ifdef __ANDROID__
@@ -153,6 +153,7 @@ class RTABMapApp : public UEventsHandler {
  void addEnvSensor(int type, float value);

  void save(const std::string & databasePath);
+  bool recover(const std::string & from, const std::string & to);
  void cancelProcessing();
  bool exportMesh(
 		  float cloudVoxelSize,
@@ -265,6 +266,8 @@ class RTABMapApp : public UEventsHandler {
  // main_scene_ includes all drawable object for visualizing Tango device's
  // movement and point cloud.
  Scene main_scene_;
+    
+    UTimer fpsTime_;

 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
@@ -89,13 +89,13 @@ Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(

 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
-    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize)
+    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
 	std::string databasePathC;
 	GetJStringContent(env,databasePath,databasePathC);
 	if(native_application)
 	{
-		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize);
+		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, clearDatabase);
 	}
 	else
 	{
@@ -104,17 +104,17 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	}
 }

-JNIEXPORT int JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
-    JNIEnv* env, jclass, jlong native_application, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_recover(
+    JNIEnv* env, jclass, jlong native_application, jstring from, jstring to)
 {
 	if(native_application)
 	{
-		std::string databasePathC;
-		GetJStringContent(env,databasePath,databasePathC);
-		std::string databaseSourceC;
-		GetJStringContent(env,databaseSource,databaseSourceC);
-		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+		std::string toC;
+		GetJStringContent(env,to,toC);
+		std::string fromC;
+		GetJStringContent(env,from,fromC);
+		return native(native_application)->recover(fromC, toC);
 	}
 	else
 	{
@@ -309,8 +309,8 @@ PointCloudDrawable::PointCloudDrawable(
 		float gainR,
 		float gainG,
 		float gainB) :
-				vertex_buffers_(0),
-				textures_(0),
+				vertex_buffer_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -320,14 +320,16 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(gainG),
 				gainB_(gainB)
 {
+    index_buffers_.resize(6, 0);
+    index_buffers_count_.resize(6, 0);
 	updateCloud(cloud, indices);
 }

 PointCloudDrawable::PointCloudDrawable(
 		const rtabmap::Mesh & mesh,
 		bool createWireframe) :
-				vertex_buffers_(0),
-				textures_(0),
+				vertex_buffer_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -337,64 +339,83 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(1.0f),
 				gainB_(1.0f)
 {
+    index_buffers_.resize(6, 0);
+    index_buffers_count_.resize(6, 0);
 	updateMesh(mesh, createWireframe);
 }

 PointCloudDrawable::~PointCloudDrawable()
 {
-	LOGI("Freeing cloud buffer %d", vertex_buffers_);
-	if (vertex_buffers_)
+	LOGI("Freeing cloud buffer %d", vertex_buffer_);
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
+		glDeleteBuffers(1, &vertex_buffer_);
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 	}

-	if (textures_)
+	if (texture_)
 	{
-		glDeleteTextures(1, &textures_);
+		glDeleteTextures(1, &texture_);
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		textures_ = 0;
+		texture_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+        }
+    }
 }

 void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
 {
+    for(int i=0; i<4; ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updatePolygons() clearing polygon buffers");
+        }
+    }
+    
 	//LOGD("Update polygons");
-	polygons_.clear();
-	polygonLines_.clear();
-	polygonsLowRes_.clear();
-	polygonLinesLowRes_.clear();
 	if(polygons.size() && organizedToDenseIndices_.size())
 	{
-		unsigned int polygonSize = polygons[0].vertices.size();
+		size_t polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
-		polygons_.resize(polygons.size() * polygonSize);
+        std::vector<std::vector<GLuint> > indexes(4);
+        indexes[0].resize(polygons.size() * polygonSize);
 		if(createWireframe)
-			polygonLines_.resize(polygons_.size()*2);
+            indexes[2].resize(indexes[0].size()*2);
 		int oi = 0;
 		int li = 0;
-		for(unsigned int i=0; i<polygons.size(); ++i)
+        for(size_t i=0; i<polygons.size(); ++i)
 		{
 			UASSERT(polygons[i].vertices.size() == polygonSize);
 			for(unsigned int j=0; j<polygonSize; ++j)
 			{
-				polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+                indexes[0][oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
 				if(createWireframe)
 				{
-					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
-					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
+                    indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+                    indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
 				}
 			}
 		}

 		if(polygonsLowRes.size())
 		{
-			unsigned int polygonSize = polygonsLowRes[0].vertices.size();
+			size_t polygonSize = polygonsLowRes[0].vertices.size();
 			UASSERT(polygonSize == 3);
-			polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
+            indexes[1].resize(polygonsLowRes.size() * polygonSize);
 			if(createWireframe)
-				polygonLinesLowRes_.resize(polygonsLowRes_.size()*2);
+                indexes[3].resize(indexes[1].size()*2);
 			int oi = 0;
 			int li = 0;
 			for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
@@ -402,15 +423,44 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 				UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
 				for(unsigned int j=0; j<polygonSize; ++j)
 				{
-					polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+                    indexes[1][oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
 					if(createWireframe)
 					{
-						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
-						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
+                        indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+                        indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
 					}
 				}
 			}
 		}
+        
+        // Generate index buffers
+        for(size_t i=0; i<indexes.size(); ++i)
+        {
+            if(!indexes[i].empty())
+            {
+                glGenBuffers(1, &index_buffers_[i]);
+                if(!index_buffers_[i])
+                {
+                    LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                    return;
+                }
+                
+                LOGD("Adding polygon index %ld size=%ld", i, indexes[i].size());
+                
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+                glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * indexes[i].size(), indexes[i].data(), GL_STATIC_DRAW);
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+                index_buffers_count_[i] = (int)indexes[i].size();
+
+                GLint error = glGetError();
+                if(error != GL_NO_ERROR)
+                {
+                    LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                    index_buffers_[i] = 0;
+                    return;
+                }
+            }
+        }
 	}
 }

@@ -418,43 +468,51 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 {
 	UASSERT(cloud.get() && !cloud->empty());
 	nPoints_ = 0;
-	polygons_.clear();
-	polygonsLowRes_.clear();
-	verticesLowRes_.clear();
-	verticesLowLowRes_.clear();
 	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
 	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);

-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		vertex_buffers_ = 0;
+		glDeleteBuffers(1, &vertex_buffer_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear vertex buffer");
+		vertex_buffer_ = 0;
 	}

-	if (textures_)
+	if (texture_)
 	{
-		glDeleteTextures(1, &textures_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		textures_ = 0;
+		glDeleteTextures(1, &texture_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear texture buffer");
+		texture_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear index buffer");
+        }
+    }

-	glGenBuffers(1, &vertex_buffers_);
-	if(!vertex_buffers_)
+	glGenBuffers(1, &vertex_buffer_);
+	if(!vertex_buffer_)
 	{
 		LOGE("OpenGL: could not generate vertex buffers\n");
 		return;
 	}

-	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	LOGI("Creating cloud buffer %d", vertex_buffer_);
 	std::vector<float> vertices;
-	int totalPoints = 0;
+	size_t totalPoints = 0;
+    std::vector<GLuint> verticesLowRes;
+    std::vector<GLuint> verticesLowLowRes;
 	if(indices.get() && indices->size())
 	{
 		totalPoints = indices->size();
 		vertices.resize(indices->size()*4);
-		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
 		int oi_low = 0;
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<indices->size(); ++i)
@@ -471,23 +529,23 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 			{
 				if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+					verticesLowRes[oi_low++] = i;
 				}
 				if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+					verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowRes.resize(oi_low);
+		verticesLowLowRes.resize(oi_lowlow);
 	}
 	else
 	{
 		totalPoints = cloud->size();
 		vertices.resize(cloud->size()*4);
-		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
 		int oi_low = 0;
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<cloud->size(); ++i)
@@ -504,19 +562,19 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 			{
 				if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+					verticesLowRes[oi_low++] = i;
 				}
 				if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+					verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowRes.resize(oi_low);
+		verticesLowLowRes.resize(oi_lowlow);
 	}

-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
 	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
 	glBindBuffer(GL_ARRAY_BUFFER, 0);

@@ -524,11 +582,40 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	if(error != GL_NO_ERROR)
 	{
 		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 		return;
 	}
+    
+    // vertex index buffers
+    for(size_t i=4; i<5; ++i)
+    {
+        if((i==4 && !verticesLowRes.empty()) ||
+           (i==5 && !verticesLowLowRes.empty()))
+        {
+            glGenBuffers(1, &index_buffers_[i]);
+            if(!index_buffers_[i])
+            {
+                LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                return;
+            }
+            
+            index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
+            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+            glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
+            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+            

-	nPoints_ = totalPoints;
+            GLint error = glGetError();
+            if(error != GL_NO_ERROR)
+            {
+                LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                index_buffers_[i] = 0;
+                return;
+            }
+        }
+    }
+
+	nPoints_ = (int)totalPoints;
 }

 void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
@@ -538,12 +625,22 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
 	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);

-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		vertex_buffers_ = 0;
+		glDeleteBuffers(1, &vertex_buffer_);
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear vertex buffer");
+		vertex_buffer_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear index buffer");
+        }
+    }

 	gainR_ = mesh.gains[0];
 	gainG_ = mesh.gains[1];
@@ -552,17 +649,17 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	bool textureUpdate = false;
 	if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
 	{
-		if (textures_)
+		if (texture_)
 		{
-			glDeleteTextures(1, &textures_);
-			tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-			textures_ = 0;
+			glDeleteTextures(1, &texture_);
+			tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear texture buffer");
+			texture_ = 0;
 		}
 		textureUpdate = true;
 	}

-	glGenBuffers(1, &vertex_buffers_);
-	if(!vertex_buffers_)
+	glGenBuffers(1, &vertex_buffer_);
+	if(!vertex_buffer_)
 	{
 		LOGE("OpenGL: could not generate vertex buffers\n");
 		return;
@@ -570,10 +667,10 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref

 	if(textureUpdate)
 	{
-		glGenTextures(1, &textures_);
-		if(!textures_)
+		glGenTextures(1, &texture_);
+		if(!texture_)
 		{
-			vertex_buffers_ = 0;
+			vertex_buffer_ = 0;
 			LOGE("OpenGL: could not generate texture buffers\n");
 			return;
 		}
@@ -591,11 +688,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
 		organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
 		totalPoints = (int)mesh.indices->size();
-		verticesLowRes_.resize(totalPoints);
-		verticesLowLowRes_.resize(totalPoints);
+        std::vector<GLuint> verticesLowRes;
+        std::vector<GLuint> verticesLowLowRes;
+        verticesLowRes.resize(totalPoints);
+        verticesLowLowRes.resize(totalPoints);
 		int oi_low = 0;
 		int oi_lowlow = 0;
-		if(textures_ && polygons.size())
+		if(texture_ && polygons.size())
 		{
 			int items = hasNormals_?9:6;
 			vertices = std::vector<float>(mesh.indices->size()*items);
@@ -628,11 +727,11 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref

 				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+                    verticesLowRes[oi_low++] = i;
 				}
 				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+                    verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
@@ -663,20 +762,48 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref

 				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+                    verticesLowRes[oi_low++] = i;
 				}
 				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+                    verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+        verticesLowRes.resize(oi_low);
+        verticesLowLowRes.resize(oi_lowlow);
+        
+        // vertex index buffers
+        for(size_t i=4; i<5; ++i)
+        {
+            if((i==4 && !verticesLowRes.empty()) ||
+               (i==5 && !verticesLowLowRes.empty()))
+            {
+                glGenBuffers(1, &index_buffers_[i]);
+                if(!index_buffers_[i])
+                {
+                    LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                    return;
+                }
+                
+                index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+                glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+
+                GLint error = glGetError();
+                if(error != GL_NO_ERROR)
+                {
+                    LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                    index_buffers_[i] = 0;
+                    return;
+                }
+            }
+        }
 	}
 	else // assume dense mesh with texCoords set to polygons
 	{
-		if(textures_ && polygons.size())
+		if(texture_ && polygons.size())
 		{
 			//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
 			//		(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
@@ -743,7 +870,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		}
 		else
 		{
-			totalPoints = mesh.cloud->size();
+			totalPoints = (int)mesh.cloud->size();
 			//LOGD("Dense mesh");
 			int items = hasNormals_?7:4;
 			organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
@@ -771,7 +898,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		}
 	}

-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
 	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
 	glBindBuffer(GL_ARRAY_BUFFER, 0);

@@ -779,11 +906,11 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	if(error != GL_NO_ERROR)
 	{
 		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 		return;
 	}

-	if(textures_ && textureUpdate)
+	if(texture_ && textureUpdate)
 	{
 		//GLint maxTextureSize = 0;
 		//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
@@ -793,13 +920,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		//LOGW("maxTextureUnits=%d", maxTextureUnits);

 		// gen texture from image
-		glBindTexture(GL_TEXTURE_2D, textures_);
+		glBindTexture(GL_TEXTURE_2D, texture_);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		cv::Mat rgbImage;
-		cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGBA);
+		cv::cvtColor(mesh.texture, rgbImage, cv::COLOR_BGR2RGBA);

 		glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
 		//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
@@ -811,20 +938,17 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		if(error != GL_NO_ERROR)
 		{
 			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
-			textures_ = 0;
+			texture_ = 0;

-			glDeleteBuffers(1, &vertex_buffers_);
-			vertex_buffers_ = 0;
+			glDeleteBuffers(1, &vertex_buffer_);
+			vertex_buffer_ = 0;
 			return;
 		}
 	}

 	nPoints_ = totalPoints;

-	if(polygons_.size() != polygons.size())
-	{
-		updatePolygons(polygons, polygonsLowRes, createWireframe);
-	}
+    updatePolygons(polygons, polygonsLowRes, createWireframe);

 	if(!pose_.isNull())
 	{
@@ -886,14 +1010,14 @@ void PointCloudDrawable::Render(
 		bool packDepthToColorChannel,
 		bool wireFrame) const
 {
-	if(vertex_buffers_ && nPoints_ && visible_ && !shaderPrograms_.empty())
+	if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
 		if(packDepthToColorChannel || !hasNormals_)
 		{
 			lighting = false;
 		}

-		if(packDepthToColorChannel || !(meshRendering && textureRendering && textures_))
+		if(packDepthToColorChannel || !(meshRendering && textureRendering && texture_))
 		{
 			textureRendering = false;
 		}
@@ -996,7 +1120,7 @@ void PointCloudDrawable::Render(
 				// Texture activate unit 0
 				glActiveTexture(GL_TEXTURE0);
 				// Bind the texture to this unit.
-				glBindTexture(GL_TEXTURE_2D, textures_);
+				glBindTexture(GL_TEXTURE_2D, texture_);
 				// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
 				GLuint texture_handle = glGetUniformLocation(program, "uTexture");
 				glUniform1i(texture_handle, 0);
@@ -1012,8 +1136,8 @@ void PointCloudDrawable::Render(
 		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");

-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-		if(textures_)
+		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
+		if(texture_)
 		{
 			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
 			if(textureRendering)
@@ -1044,53 +1168,49 @@ void PointCloudDrawable::Render(
 		tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");

 		UTimer drawTime;
-		if(textureRendering)
+		if((textureRendering || meshRendering) && index_buffers_[0])
 		{
-			if(distanceToCameraSqr<16.0f || polygonsLowRes_.empty())
+            float dist = meshRendering?50.0f:16.0f;
+			if(distanceToCameraSqr<dist || index_buffers_[1]==0)
 			{
-				wireFrame = wireFrame && polygonLines_.size();
+				wireFrame = wireFrame && index_buffers_[2];
 				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[2]);
+                    glDrawElements(GL_LINES, index_buffers_count_[2], GL_UNSIGNED_INT, 0);
+                }
 				else
-					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[0]);
+                    glDrawElements(GL_TRIANGLES, index_buffers_count_[0], GL_UNSIGNED_INT, 0);
+                }
 			}
 			else
 			{
-				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				wireFrame = wireFrame && index_buffers_[3];
 				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[3]);
+                    glDrawElements(GL_LINES, index_buffers_count_[3], GL_UNSIGNED_INT, 0);
+                }
 				else
-					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[1]);
+                    glDrawElements(GL_TRIANGLES, index_buffers_count_[1], GL_UNSIGNED_INT, 0);
+                }
 			}
 		}
-		else if(meshRendering && polygons_.size())
+		else if(index_buffers_[4])
 		{
-			if(distanceToCameraSqr<50.0f || polygonsLowRes_.empty())
+			if(distanceToCameraSqr>600.0f && index_buffers_[5])
 			{
-				wireFrame = wireFrame && polygonLines_.size();
-				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
-				else
-					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
-			}
-			else
-			{
-				wireFrame = wireFrame && polygonLinesLowRes_.size();
-				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
-				else
-					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
-			}
-		}
-		else if(!verticesLowRes_.empty())
-		{
-			if(distanceToCameraSqr>600.0f)
-			{
-				glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[5]);
+                glDrawElements(GL_POINTS, index_buffers_count_[5], GL_UNSIGNED_INT, 0);
 			}
 			else if(distanceToCameraSqr>150.0f)
 			{
-				glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[4]);
+                glDrawElements(GL_POINTS, index_buffers_count_[4], GL_UNSIGNED_INT, 0);
 			}
 			else
 			{
@@ -1106,6 +1226,7 @@ void PointCloudDrawable::Render(

 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
+        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);

 		glUseProgram(0);
 		tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");
@@ -70,8 +70,8 @@ private:
  rtabmap::Transform getPose() const {return pose_;}
  const glm::mat4 & getPoseGl() const {return poseGl_;}
  bool isVisible() const {return visible_;}
-  bool hasMesh() const {return polygons_.size()!=0;}
-  bool hasTexture() const {return textures_ != 0;}
+  bool hasMesh() const {return index_buffers_[0] != 0;}
+  bool hasTexture() const {return texture_ != 0;}
  float getMinHeight() const {return minHeight_;}
  const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
  const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
@@ -115,14 +115,10 @@ private:

 private:
  // Vertex buffer of the point cloud geometry.
-  GLuint vertex_buffers_;
-  GLuint textures_;
-  std::vector<GLuint> polygons_;
-  std::vector<GLuint> polygonsLowRes_;
-  std::vector<GLuint> polygonLines_;
-  std::vector<GLuint> polygonLinesLowRes_;
-  std::vector<GLuint> verticesLowRes_;
-  std::vector<GLuint> verticesLowLowRes_;
+  GLuint vertex_buffer_;
+  GLuint texture_;
+  std::vector<GLuint> index_buffers_;
+  std::vector<int> index_buffers_count_;
  int nPoints_;
  rtabmap::Transform pose_;
  glm::mat4 poseGl_;
@@ -102,7 +102,7 @@ Scene::Scene() :
 		screenHeight_(0),
 		doubleTapOn_(false)
 {
-    depthTextures_[0] = depthTextures_[1] = 0;
+    depthTexture_ = 0;
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
 	      tango_gl::GestureCamera::kThirdPersonFollow);
@@ -179,8 +179,8 @@ void Scene::DeleteResources() {
 		fboId_ = 0;
 		glDeleteRenderbuffers(1, &rboId_);
 		rboId_ = 0;
-        glDeleteTextures(2, depthTextures_);
-        depthTextures_[0] = depthTextures_[1] = 0;
+        glDeleteTextures(1, &depthTexture_);
+        depthTexture_ = 0;
 	}

 	clear();
@@ -234,8 +234,8 @@ void Scene::SetupViewPort(int w, int h) {
 			fboId_ = 0;
 			glDeleteRenderbuffers(1, &rboId_);
 			rboId_ = 0;
-			glDeleteTextures(2, depthTextures_);
-            depthTextures_[0] = depthTextures_[1] = 0;
+			glDeleteTextures(1, &depthTexture_);
+            depthTexture_ = 0;
 		}

        GLint originid = 0;
@@ -247,8 +247,8 @@ void Scene::SetupViewPort(int w, int h) {
 		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);

 		// Create depth texture
-		glGenTextures(2, depthTextures_);
-		glBindTexture(GL_TEXTURE_2D, depthTextures_[0]);
+		glGenTextures(1, &depthTexture_);
+		glBindTexture(GL_TEXTURE_2D, depthTexture_);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@@ -256,21 +256,13 @@ void Scene::SetupViewPort(int w, int h) {
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
 		glBindTexture(GL_TEXTURE_2D, 0);
        
-        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
-        glBindTexture(GL_TEXTURE_2D, 0);
-
 		glGenRenderbuffers(1, &rboId_);
 		glBindRenderbuffer(GL_RENDERBUFFER, rboId_);
 		glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, w, h);
 		glBindRenderbuffer(GL_RENDERBUFFER, 0);

 		// Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
-		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTextures_[0], 0);
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
 		glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);

 		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
@@ -349,8 +341,9 @@ std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normaliz
 /**
 * Tells whether or not b is intersecting f.
 * http://www.txutxi.com/?p=584
- * @param f Viewing frustum.
- * @param b An axis aligned bounding box.
+ * @param planes Viewing frustum.
+ * @param boxMin The axis aligned bounding box min.
+ * @param boxMax The axis aligned bounding box max.
 * @return True if b intersects f, false otherwise.
 */
 bool intersectFrustumAABB(
@@ -389,7 +382,8 @@ bool intersectFrustumAABB(
 }

 //Should only be called in OpenGL thread!
-int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping) {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping)
+{
 	UASSERT(gesture_camera_ != 0);

 	if(currentPose_ == 0)
@@ -428,7 +422,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat

 	if(renderBackgroundCamera)
 	{
-        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.2)
+        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.3)
        {
            projectionMatrix = arProjectionMatrix;
            viewMatrix = arViewMatrix;
@@ -486,9 +480,15 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		glDisable(GL_CULL_FACE);
 	}

-	UTimer timer;
+	bool onlineBlending =
+        (!meshRendering_ &&
+         occlusionMesh.cloud.get() &&
+         occlusionMesh.cloud->size()) ||
+        (blending_ &&
+         gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho &&
+         mapRendering_ && meshRendering_ &&
+         (cloudsToDraw.size() > 1 || (renderBackgroundCamera && wireFrame_)));

-	bool onlineBlending = (!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_);
 	if(onlineBlending && fboId_)
 	{
        GLint originid = 0;
@@ -499,7 +499,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat

 		glClearColor(0, 0, 0, 0);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
+        
        // Draw scene
        for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
        {
@@ -511,16 +511,12 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
            (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
        }
        
-        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
-        glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, screenWidth_, screenHeight_);
-        glBindTexture(GL_TEXTURE_2D, 0);
-        
        if(!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
        {
            PointCloudDrawable drawable(occlusionMesh);
            drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 0, 0, 0, 0, 0, 0, true);
        }
-
+        
 		// back to normal window-system-provided framebuffer
 		glBindFramebuffer(GL_FRAMEBUFFER, originid); // unbind
 	}
@@ -559,7 +555,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    
-    if(renderBackgroundCamera && !onlineBlending)
+    if(renderBackgroundCamera && (!onlineBlending || !meshRendering_))
    {
        background_renderer_->Draw(uvsTransformed, 0, screenWidth_, screenHeight_, false);

@@ -594,6 +590,10 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+        else
+        {
+            trace_->ClearVertexArray();
+        }
 	}

 	if(gridVisible_ && !renderBackgroundCamera)
@@ -629,14 +629,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 				cloud->getPose().z() - openglCamera.z());
 		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];

-		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTextures_[0]:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}

 	if(onlineBlending)
 	{
-        if(renderBackgroundCamera)
+        if(renderBackgroundCamera && meshRendering_)
        {
-            background_renderer_->Draw(uvsTransformed, depthTextures_[1], screenWidth_, screenHeight_, meshRendering_?mapping:false);
+            background_renderer_->Draw(uvsTransformed, depthTexture_, screenWidth_, screenHeight_, mapping);
        }
        
 		glDisable (GL_BLEND);
@@ -208,7 +208,7 @@ class Scene {
  float b_;
  GLuint fboId_;
  GLuint rboId_;
-  GLuint depthTextures_[2]; // 0=objects+occlusion 1=objects only
+  GLuint depthTexture_; // 0=objects+occlusion
  GLsizei screenWidth_;
  GLsizei screenHeight_;
  bool doubleTapOn_;
@@ -183,7 +183,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
    case kFirstPerson:
      SetOrthoMode(false);
      SetFieldOfView(kLowestFov);
-      SetNearFarClipPlanes(0.1, 50);
+      SetNearFarClipPlanes(0.25, 25);
      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
      cam_cur_dist_ = 0.0f;
@@ -19,7 +19,7 @@
    <string name="light_off">Lighting</string>
    <string name="wireframe">Wireframe</string>
    <string name="close_visualization">Close Visualization</string>
-    <string name="save_to_file">Export to File&#8230;</string>
+    <string name="save_to_file">Export OBJ/PLY&#8230;</string>
    <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
    <string name="start">Start</string>
    <string name="nodes">"Nodes (WM): "</string>
@@ -80,7 +80,7 @@
    <string name="pref_default_fisheye">false</string>
    
    <string name="pref_key_camera_driver">pref_key_camera_driver</string>
-    <string name="pref_default_camera_driver">0</string>
+    <string name="pref_default_camera_driver">-1</string>
    <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
    <string name="pref_default_depth_from_motion">false</string>
    <string name="pref_key_arcore_localization_filtering_speed">pref_key_arcore_localization_filtering_speed</string>
@@ -108,7 +108,7 @@
    <string name="pref_key_features_type">pref_key_features_type</string>
    <string name="pref_default_features_type">6</string>
    <string name="pref_key_optimizer">pref_key_optimizer</string>
-    <string name="pref_default_optimizer">1</string>
+    <string name="pref_default_optimizer">2</string>
    <string name="pref_key_optimize_end">pref_key_optimize_end</string>
    <string name="pref_default_optimize_end">true</string>
    <string name="pref_key_marker_detection">pref_key_marker_detection</string>
@@ -381,12 +381,14 @@
    <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>

    <string-array name="pref_camera_driver_keys">
+        <item>"Auto"</item>
        <item>"Google Tango NDK"</item>
        <item>"ARCore NDK"</item>
        <item>"AREngine NDK"</item>
        <item>"ARCore Java"</item>
    </string-array>
    <string-array name="pref_camera_driver_values">
+        <item>"-1"</item>
        <item>"0"</item>
        <item>"1"</item>
        <item>"2"</item>
@@ -688,7 +688,7 @@ public class ARCoreSharedCamera {
 					if(mToast!=null && previousAnchorPose != null)
 					{
 						String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
-						if(!mToast.getView().isShown())
+						if(mToast.getView() == null || !mToast.getView().isShown())
 						{
 							mToast.makeText(mActivity.getApplicationContext(), 
 									msg, Toast.LENGTH_LONG).show();
@@ -736,7 +736,7 @@ public class ARCoreSharedCamera {
 										+ "because of high speed detected (%f m/s) causing a jump! You can change "
 										+ "ARCore localization filtering speed in Settings->Mapping if you are "
 										+ "indeed moving as fast.", speed);
-								if(!mToast.getView().isShown())
+								if(mToast.getView() == null || !mToast.getView().isShown())
 								{
 									mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
 								}
@@ -2,6 +2,7 @@ package com.introlab.rtabmap;

 import java.io.File;
 import java.io.FileInputStream;
+import java.io.FileNotFoundException;
 import java.io.FileOutputStream;
 import java.io.IOException;
 import java.io.InputStream;
@@ -25,6 +26,8 @@ import android.app.NotificationManager;
 import android.app.PendingIntent;
 import android.app.ProgressDialog;
 import android.content.ComponentName;
+import android.content.ContentResolver;
+import android.content.ContentValues;
 import android.content.Context;
 import android.content.DialogInterface;
 import android.content.DialogInterface.OnShowListener;
@@ -35,6 +38,7 @@ import android.content.pm.ApplicationInfo;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
+import android.database.Cursor;
 import android.hardware.Camera;
 import android.hardware.Sensor;
 import android.hardware.SensorEvent;
@@ -46,6 +50,7 @@ import android.hardware.display.DisplayManager;
 import android.location.Location;
 import android.location.LocationListener;
 import android.location.LocationManager;
+import android.media.MediaScannerConnection;
 import android.net.Uri;
 import android.net.wifi.WifiInfo;
 import android.net.wifi.WifiManager;
@@ -56,6 +61,8 @@ import android.os.Handler;
 import android.os.IBinder;
 import android.os.Message;
 import android.preference.PreferenceManager;
+import android.provider.MediaStore;
+import android.provider.OpenableColumns;
 import android.support.v4.app.FragmentActivity;
 import android.support.v4.content.FileProvider;
 import android.text.InputType;
@@ -256,6 +263,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener

 	ARCoreSharedCamera mArCoreCamera = null;
 	int mCameraDriver = 0;
+	
+	private String mIntentDbToOpen = null;

 	//Tango Service connection.
 	boolean mCameraServiceConnectionUsed = false; 
@@ -294,7 +303,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}

 		public void onServiceDisconnected(ComponentName name) {
-			// Handle this if you need to gracefully shutdown/retry
+			// Handle this if you need to gracefully shutsaveDatabasedown/retry
 			// in the event that Tango itself crashes/gets upgraded while running.
 			mToast.makeText(getApplicationContext(), 
 					String.format("Tango disconnected!"), mToast.LENGTH_LONG).show();
@@ -494,10 +503,11 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mWorkingDirectoryHuman = "";
 		mTotalLoopClosures = 0;
 		mLastFastMovementNotificationStamp = System.currentTimeMillis()/1000;
-
-		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0)
+		
+		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0 && 
+				getActivity().getExternalFilesDirs(null).length >=1)
 		{
-			File extStore = Environment.getExternalStorageDirectory();
+			File extStore = getActivity().getExternalFilesDirs(null)[0];
 			mWorkingDirectory = extStore.getAbsolutePath() + "/" + getString(R.string.app_name) + "/";
 			extStore = new File(mWorkingDirectory);
 			extStore.mkdirs();
@@ -507,7 +517,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		{
 			// show warning that data cannot be saved!
 			mToast.makeText(getApplicationContext(), 
-					String.format("Failed to get external storage path (SD-CARD, state=%s). Saving disabled.", 
+					String.format("Failed to get external storage path (state=%s). Saving disabled.", 
 							Environment.getExternalStorageState()), mToast.LENGTH_LONG).show();
 		}

@@ -574,33 +584,141 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener

 		DISABLE_LOG =  !( 0 != ( getApplicationInfo().flags & ApplicationInfo.FLAG_DEBUGGABLE ) );

-		if (!PermissionHelper.hasPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
-			PermissionHelper.requestPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE);
-		}
-		else
-		{
-			postCreate();
-		}
-
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 		
 		isArCoreAvailable();
 		isArEngineAvailable();
+		
+		if (!PermissionHelper.hasPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
+			PermissionHelper.requestPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE);
+		}
+		else
+		{
+			// Get intent, action and MIME type
+		    Intent intent = getIntent();
+		    String action = intent.getAction();
+		    String type = intent.getType();
+
+		    if (Intent.ACTION_SEND.equals(action) && type != null) {
+		        if ("application/octet-stream".equals(type)) {
+		        	Uri imageUri = (Uri) intent.getParcelableExtra(Intent.EXTRA_STREAM);
+		            if (imageUri != null) {
+		               String fileName = getFileName(imageUri);
+		               Log.i(TAG, "Intent received: " + imageUri.getPath() + " Name:" + fileName);
+		               if(fileName.endsWith(".db"))
+		               {
+			               File file = new File(mWorkingDirectory+fileName);
+			               if(file.exists())
+			               {
+			            	   mToast.makeText(this, fileName + " already exists in RTAB-Map's library! Cannot be copied.", mToast.LENGTH_LONG).show();
+			               }
+			               else
+			               {
+				               copy(imageUri, file);
+				               mIntentDbToOpen = fileName;
+			               }
+		               }
+		            }
+		        }
+		    } else if (Intent.ACTION_SEND_MULTIPLE.equals(action) && type != null) {
+		    	if (type.startsWith("application/")) {
+		    		ArrayList<Uri> imageUris = intent.getParcelableArrayListExtra(Intent.EXTRA_STREAM);
+		    		if (imageUris != null) {
+		    			boolean added = false;
+		    			for(Uri imageUri: imageUris)
+		    			{
+		    				String fileName = getFileName(imageUri);
+		    				Log.i(TAG, "Intent received: " + imageUri.getPath() + " Name:" + fileName);
+		    				if(fileName.endsWith(".db"))
+		    				{
+		    					File file = new File(mWorkingDirectory+"/"+getFileName(imageUri));
+		    					if(!file.exists())
+		    					{
+			    					copy(imageUri, file);
+			    					added = true;
+		    					}
+		    					else
+		    					{
+		    						Log.e(TAG,  fileName + " already exists in RTAB-Map's library! Cannot be copied.");
+		    					}
+		    				}
+		    			}
+		    			if(added)
+		    			{
+		    				openDatabase();
+		    			}
+		    		}
+
+		    	}
+		    }
+
+			postCreate();
+		}
 	}
 	
+	public void copy(File src, File dst) throws IOException {
+		InputStream in = new FileInputStream(src);
+		OutputStream out = new FileOutputStream(dst);
+
+		// Transfer bytes from in to out
+		byte[] buf = new byte[1024];
+		int len;
+		while ((len = in.read(buf)) > 0) {
+			out.write(buf, 0, len);
+		}
+		in.close();
+		out.close();
+	}
+	
+	public void copy(Uri uri, File file)
+	{
+		InputStream in;
+		try {
+			in = getApplicationContext().getContentResolver().openInputStream(uri);
+			
+			OutputStream out = new FileOutputStream(file);
+    		byte[] buf = new byte[1024];
+    		int len;
+    		while ((len = in.read(buf)) > 0) {
+    			out.write(buf, 0, len);
+    		}
+    		in.close();
+    		out.close();
+		} catch (IOException e) {
+			Log.e(TAG, e.getMessage());
+		}
+	}
+	
+	public String getFileName(Uri uri) {
+		  String result = null;
+		  if (uri.getScheme().equals("content")) {
+		    Cursor cursor = getContentResolver().query(uri, null, null, null, null);
+		    try {
+		      if (cursor != null && cursor.moveToFirst()) {
+		        result = cursor.getString(cursor.getColumnIndex(OpenableColumns.DISPLAY_NAME));
+		      }
+		    } finally {
+		      cursor.close();
+		    }
+		  }
+		  if (result == null) {
+		    result = uri.getPath();
+		    int cut = result.lastIndexOf('/');
+		    if (cut != -1) {
+		      result = result.substring(cut + 1);
+		    }
+		  }
+		  return result;
+		}
+
+	
 	// Should be called only if read/write permissions are granted!
 	private void postCreate()
 	{
 		Log.i(TAG, "postCreate()");
 		
-		String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-		(new File(tmpDatabase)).delete();
-		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
-		boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
-		RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
-		
 		final String[] files = Util.loadFileList(mWorkingDirectory, true);
 		if(files.length == 0)
 		{
@@ -613,6 +731,17 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Log.i(TAG, String.format("updateCameraDriverSettings() mCameraDriver=%d RTABMapLib.isBuiltWith(%d)=%d", mCameraDriver, mCameraDriver, RTABMapLib.isBuiltWith(nativeApplication, mCameraDriver)?1:0));
 		
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
+		mCameraDriver = Integer.parseInt(cameraDriverStr);
+		
+		if(mCameraDriver == -1)
+		{
+			// Prioritize tango if available
+			mCameraDriver = 0;
+			SharedPreferences.Editor editor = sharedPref.edit();
+			editor.putString(getString(R.string.pref_key_camera_driver), "0");
+			editor.commit();
+		}
 		
 		if(mCameraDriver == 0 && (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION) || !RTABMapLib.isBuiltWith(nativeApplication, 0)))
 		{
@@ -1167,7 +1296,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		final boolean depthFromMotion = sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion)));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);

-		if(!DISABLE_LOG) Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
+		Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
 		if(mCameraDriver == 0) // Tango
 		{
 			// Check if the Tango Core is out dated.
@@ -1490,7 +1619,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}

 		updateState(mState);
-
+		
 		return true;
 	}

@@ -1992,6 +2121,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				final int loopDetected = RTABMapLib.postProcessing(nativeApplication, -1);
 				runOnUiThread(new Runnable() {
 					public void run() {
+						updateState(State.STATE_IDLE);
 						if(mExportProgressDialog.isShowing())
 						{
 							mExportProgressDialog.dismiss();
@@ -2024,8 +2154,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							mProgressDialog.dismiss();
 							mToast.makeText(getActivity(), String.format("Optimization canceled"), mToast.LENGTH_LONG).show();
 						}
-
-						updateState(State.STATE_IDLE);
 					}
 				});
 			} 
@@ -2144,7 +2272,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			mButtonCloseVisualization.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
 			mButtonCloseVisualization.setEnabled(true);
 			mButtonSaveOnDevice.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
-			mButtonShareOnSketchfab.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
+			//mButtonShareOnSketchfab.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
 			mButtonLibrary.setVisibility(View.INVISIBLE);
 			mButtonNewScan.setVisibility(View.INVISIBLE);
 			mItemSave.setEnabled(mState != State.STATE_VISUALIZING_CAMERA);
@@ -2217,6 +2345,12 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		{
 			mGLView.setRenderMode(GLSurfaceView.RENDERMODE_CONTINUOUSLY);
 		}
+		
+		if(mState == State.STATE_WELCOME && mIntentDbToOpen != null)
+		{
+			openDatabase(mIntentDbToOpen, false);
+			mIntentDbToOpen = null;
+		}
 	}

 	private void startMapping() {
@@ -2626,7 +2760,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
 							boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
 							String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-							RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
+							RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);

 							mItemLocalizationMode.setEnabled(!mItemDataRecorderMode.isChecked());		   

@@ -2780,16 +2914,110 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mOpenedDatabasePath = "";
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
-		String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-		RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
-
-		if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))
+		final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
+		
+		File newFile = new File(tmpDatabase);
+		final int fileSizeMB = (int)newFile.length()/(1024 * 1024);
+		if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING) &&
+			newFile.exists() && 
+			fileSizeMB>1) // >1MB
 		{
-			setCamera(1);
-			startCamera(String.format("Hold Tight! Initializing Camera Service...\n"
-					+ "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\"."));
+			AlertDialog d2 = new AlertDialog.Builder(getActivity())
+					.setCancelable(false)
+					.setTitle("Recovery")
+					.setMessage(String.format("The previous session (%d MB) was not correctly saved, do you want to recover it?", fileSizeMB))
+					.setNegativeButton("Ignore", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							(new File(tmpDatabase)).delete();
+							newScan();
+						}
+					})
+					.setNeutralButton("Cancel", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							// do nothing
+						}
+					})
+					.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							final String fileName = new SimpleDateFormat("yyMMdd-HHmmss").format(new Date()) + ".db";
+							final String outputDbPath = mWorkingDirectory + fileName;
+							
+							mExportProgressDialog.setTitle("Recovering");
+							mExportProgressDialog.setMessage(String.format("Please wait while recovering data..."));
+							mExportProgressDialog.setProgress(0);
+
+							final State previousState = mState;
+
+							mExportProgressDialog.show();
+							updateState(State.STATE_PROCESSING);
+
+							Thread exportThread = new Thread(new Runnable() {
+								public void run() {
+
+									final long startTime = System.currentTimeMillis()/1000;
+
+									final boolean success = RTABMapLib.recover(
+											nativeApplication, 
+											tmpDatabase,
+											outputDbPath);
+									runOnUiThread(new Runnable() {
+										public void run() {
+											if(mExportProgressDialog.isShowing())
+											{							
+												if(success)
+												{								
+													AlertDialog d2 = new AlertDialog.Builder(getActivity())
+															.setCancelable(false)
+															.setTitle("Database saved!")
+															.setMessage(String.format("Database \"%s\" (%d MB) successfully saved!", fileName, fileSizeMB))
+															.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+																public void onClick(DialogInterface dialog, int which) {
+																	openDatabase(fileName, false);
+																}
+															})
+															.create();
+													d2.setCanceledOnTouchOutside(true);
+													d2.show();
+												}
+												else
+												{
+													updateState(previousState);
+													mToast.makeText(getActivity(), String.format("Recovery failed!"), mToast.LENGTH_LONG).show();
+												}
+												mExportProgressDialog.dismiss();
+											}
+											else
+											{
+												mToast.makeText(getActivity(), String.format("Recovery canceled"), mToast.LENGTH_LONG).show();
+												updateState(previousState);
+											}
+										}
+									});
+								} 
+							});
+							exportThread.start();
+							
+							refreshSystemMediaScanDataBase(getActivity(), outputDbPath);
+						}
+					})
+					.create();
+			d2.setCanceledOnTouchOutside(false);
+			d2.show();
+		}
+		else
+		{
+		
+			RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);
+	
+			if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))
+			{
+				setCamera(0);
+				startCamera(String.format("Hold Tight! Initializing Camera Service...\n"
+						+ "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\"."));
+			}
 		}
 	}
+		

 	private void openDatabase()
 	{
@@ -3215,7 +3443,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		context.sendBroadcast(mediaScanIntent);
 	}

-	private void saveDatabase(String fileName)
+	private void saveDatabase(final String fileName)
 	{
 		final String newDatabasePath = mWorkingDirectory + fileName + ".db";
 		final String newDatabasePathHuman = mWorkingDirectoryHuman + fileName + ".db";
@@ -3247,7 +3475,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							mSavedStamp = System.currentTimeMillis();
 							msg = String.format("Database saved to \"%s\".", newDatabasePathHuman);	
 						}
-
+									            
 						// build notification
 						Intent intent = new Intent(getActivity(), RTABMapActivity.class);
 						// use System.currentTimeMillis() to have a unique ID for the pending intent
@@ -3280,7 +3508,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 						AlertDialog d2 = new AlertDialog.Builder(getActivity())
 								.setCancelable(false)
 								.setTitle("Database saved!")
-								.setMessage(String.format("Database \"%s\" (%d MB) successfully saved on the SD-CARD!", newDatabasePathHuman, fileSizeMB))
+								.setMessage(String.format("Database \"%s\" (%d MB) successfully saved!", newDatabasePathHuman, fileSizeMB))
 								.setPositiveButton("OK", new DialogInterface.OnClickListener() {
 									public void onClick(DialogInterface dialog, int which) {
 										resetNoTouchTimer(true);
@@ -3334,30 +3562,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				dialog.dismiss();
 				if(!fileName.isEmpty())
 				{
-					File newFile = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + fileName + ".zip");
-					if(newFile.exists())
-					{
-						AlertDialog ad = new AlertDialog.Builder(getActivity())
-								.setCancelable(false)
-								.setTitle("File Already Exists")
-								.setMessage("Do you want to overwrite the existing file?")
-								.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-									public void onClick(DialogInterface dialog, int which) {
-										writeExportedFiles(fileName);
-									}
-								})
-								.setNegativeButton("No", new DialogInterface.OnClickListener() {
-									public void onClick(DialogInterface dialog, int which) {
-										saveOnDevice();
-									}
-								}).create();
-						ad.setCanceledOnTouchOutside(false);
-						ad.show();
-					}
-					else
-					{
-						writeExportedFiles(fileName);
-					}
+					writeExportedFiles(fileName);
 				}
 			}
 		});
@@ -3371,7 +3576,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 	{		
 		Log.i(TAG, String.format("Write exported mesh to \"%s\"", fileName));

-		mProgressDialog.setTitle("Saving to sd-card");
+		mProgressDialog.setTitle("Exporting");
 		mProgressDialog.setMessage(String.format("Compressing the files..."));
 		mProgressDialog.show();

@@ -3397,8 +3602,23 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				}
 				File exportDir = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR);
 				exportDir.mkdirs();
+				
+				// cleanup old zip
+				fileNames = Util.loadFileList(mWorkingDirectory + RTABMAP_EXPORT_DIR, false);
+				if(!DISABLE_LOG) Log.i(TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMAP_EXPORT_DIR));
+				for(int i=0; i<fileNames.length; ++i)
+				{
+					File f = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + "/" + fileNames[i]);
+					if(f.delete())
+					{
+						if(!DISABLE_LOG) Log.i(TAG, String.format("Deleted \"%s\"", f.getPath()));
+					}
+					else
+					{
+						if(!DISABLE_LOG) Log.i(TAG, String.format("Failed deleting \"%s\"", f.getPath()));
+					}
+				}

-				final String pathHuman = mWorkingDirectoryHuman + RTABMAP_EXPORT_DIR + fileName + ".zip";
 				final String zipOutput = mWorkingDirectory+RTABMAP_EXPORT_DIR+fileName+".zip";
 				if(RTABMapLib.writeExportedMesh(nativeApplication, mWorkingDirectory + RTABMAP_TMP_DIR, RTABMAP_TMP_FILENAME))
 				{							
@@ -3424,7 +3644,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							final String msg = e.getMessage();
 							runOnUiThread(new Runnable() {
 								public void run() {
-									mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! Error=%s", pathHuman, msg), mToast.LENGTH_LONG).show();
+									mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! Error=%s", fileName, msg), mToast.LENGTH_LONG).show();
 								}
 							});
 						}
@@ -3440,29 +3660,41 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							final File f = new File(zipOutput);
 							final int fileSizeMB = (int)f.length()/(1024 * 1024);

-							AlertDialog d = new AlertDialog.Builder(getActivity())
-									.setCancelable(false)
-									.setTitle("Mesh Saved!")
-									.setMessage(String.format("Mesh \"%s\" (%d MB) successfully exported on the SD-CARD! Share it?", pathHuman, fileSizeMB))
-									.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-										public void onClick(DialogInterface dialog, int which) {
-											// Send to...
-											Intent shareIntent = new Intent();
-											shareIntent.setAction(Intent.ACTION_SEND);
-											shareIntent.putExtra(Intent.EXTRA_STREAM, Uri.fromFile(f));
-											shareIntent.setType("application/zip");
-											startActivity(Intent.createChooser(shareIntent, "Sharing..."));
+							// Save to public Documents/RTAB-Map folder
+							/*ContentValues values = new ContentValues();
+				            values.put(MediaStore.MediaColumns.DISPLAY_NAME, fileName); //file name                     
+				            values.put(MediaStore.MediaColumns.MIME_TYPE, "application/zip");        //file extension, will automatically add to file
+				            values.put(MediaStore.MediaColumns.RELATIVE_PATH, Environment.DIRECTORY_DOCUMENTS + "/RTAB-Map");     //end "/" is not mandatory
+				            Uri uri = getContentResolver().insert(MediaStore.Files.getContentUri("external"),values);
+				            if (uri != null) {
+				            	OutputStream out;
+								try {
+									out = getApplicationContext().getContentResolver().openOutputStream(uri);
+									
+									InputStream in = new FileInputStream(zipOutput);
+					        		byte[] buf = new byte[1024];
+					        		int len;
+					        		while ((len = in.read(buf)) > 0) {
+					        			out.write(buf, 0, len);
+					        		}
+					        		in.close();
+					        		out.close();
+					        		
+					        		f.delete(); // remove private file
+								} catch (IOException e) {
+									Log.e(TAG, e.getMessage());
+								}
+				            } */

-											resetNoTouchTimer(true);
-										}
-									})
-									.setNegativeButton("No", new DialogInterface.OnClickListener() {
-										public void onClick(DialogInterface dialog, int which) {
-											resetNoTouchTimer(true);
-										}
-									}).create();
-							d.setCanceledOnTouchOutside(false);
-							d.show();
+							// Send to...
+							Intent shareIntent = new Intent();
+							shareIntent.setAction(Intent.ACTION_SEND);
+							shareIntent.putExtra(Intent.EXTRA_STREAM, FileProvider.getUriForFile(getActivity(), getActivity().getApplicationContext().getPackageName() + ".provider", f));
+							shareIntent.addFlags(Intent.FLAG_GRANT_READ_URI_PERMISSION);
+							shareIntent.setType("application/zip");
+							startActivity(Intent.createChooser(shareIntent, "Sharing..."));
+
+							resetNoTouchTimer(true);
 						}
 					});
 				}
@@ -3471,7 +3703,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 					runOnUiThread(new Runnable() {
 						public void run() {
 							mProgressDialog.dismiss();
-							mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! No files found in tmp directory!? Last export may have failed or have been canceled.", pathHuman), mToast.LENGTH_LONG).show();
+							mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! No files found in tmp directory!? Last export may have failed or have been canceled.", fileName), mToast.LENGTH_LONG).show();
 							resetNoTouchTimer(true);
 						}
 					});
@@ -3498,8 +3730,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Thread openThread = new Thread(new Runnable() {
 			public void run() {

-				final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;				
-				final int status = RTABMapLib.openDatabase2(nativeApplication, mOpenedDatabasePath, tmpDatabase, databaseInMemory, optimize);
+				final int status = RTABMapLib.openDatabase(nativeApplication, mOpenedDatabasePath, databaseInMemory, optimize, false);

 				runOnUiThread(new Runnable() {
 					public void run() {
@@ -3578,20 +3809,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		openThread.start();	
 	}

-	public void copy(File src, File dst) throws IOException {
-		InputStream in = new FileInputStream(src);
-		OutputStream out = new FileOutputStream(dst);
-
-		// Transfer bytes from in to out
-		byte[] buf = new byte[1024];
-		int len;
-		while ((len = in.read(buf)) > 0) {
-			out.write(buf, 0, len);
-		}
-		in.close();
-		out.close();
-	}
-
 	private void shareToSketchfab()
 	{
 		if (!PermissionHelper.hasPermission(this, Manifest.permission.INTERNET)) {
@@ -34,9 +34,10 @@ public class RTABMapLib
    
    public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
    
-    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize);
-    public static native int openDatabase2(long nativeApplication, String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize, boolean clearDatabase);
    
+    public static native boolean recover(long nativeApplication, String from, String to);
+
    public static native boolean isBuiltWith(long nativeApplication, int cameraDriver);
    public static native boolean startCamera(long nativeApplication, IBinder binder, Context context, Activity activity, int driver);
    public static native void stopCamera(long nativeApplication);
@@ -979,9 +979,10 @@
 				ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
 				ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
 				CLANG_CXX_LIBRARY = "libc++";
+				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 7;
+				CURRENT_PROJECT_VERSION = 13;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				EXCLUDED_ARCHS = "";
@@ -1006,7 +1007,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.20.17;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1038,7 +1039,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 7;
+				CURRENT_PROJECT_VERSION = 13;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				FRAMEWORK_SEARCH_PATHS = (
@@ -1063,7 +1064,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.12;
+				MARKETING_VERSION = 0.20.17;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
@@ -39,6 +39,7 @@
      ignoresPersistentStateOnLaunch = "NO"
      debugDocumentVersioning = "YES"
      debugServiceExtension = "internal"
+      enableGPUFrameCaptureMode = "2"
      allowLocationSimulation = "YES">
      <BuildableProductRunnable
         runnableDebuggingMode = "0">
@@ -1,8 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="19455" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
    <device id="retina6_1" orientation="portrait" appearance="light"/>
    <dependencies>
-        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <deployment identifier="iOS"/>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="19454"/>
        <capability name="Safe area layout guides" minToolsVersion="9.0"/>
        <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
    </dependencies>
@@ -1,8 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="19455" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
    <device id="retina3_5" orientation="portrait" appearance="light"/>
    <dependencies>
-        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <deployment identifier="iOS"/>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="19454"/>
        <capability name="Image references" minToolsVersion="12.0"/>
        <capability name="Safe area layout guides" minToolsVersion="9.0"/>
        <capability name="System colors in document resources" minToolsVersion="11.0"/>
@@ -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
@@ -69,24 +69,11 @@ void setScreenRotationNative(const void *object, int displayRotation)
    }
 }

-int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize)
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
    if(object)
    {
-        return native(object)->openDatabase(databasePath, databaseInMemory, optimize);
-    }
-    else
-    {
-        UERROR("object is null!");
-        return -1;
-    }
-}
-
-int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize)
-{
-    if(object)
-    {
-        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, databaseSource);
+        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, clearDatabase);
    }
    else
    {
@@ -107,6 +94,31 @@ void saveNative(const void *object, const char * databasePath)
    }
 }

+bool recoverNative(const void *object, const char * from, const char * to)
+{
+    if(object)
+    {
+        return native(object)->recover(from, to);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+void cancelProcessingNative(const void *object)
+{
+    if(object)
+    {
+        native(object)->cancelProcessing();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
 int postProcessingNative(const void *object, int approach)
 {
    if(object)
@@ -32,9 +32,10 @@ void setupCallbacksNative(const void *object, void * classPtr,
                                                   float, float, float, float, float, float));
 void destroyNativeApplication(const void *object);
 void setScreenRotationNative(const void *object, int displayRotation);
-int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize);
-int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize);
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
 void saveNative(const void *object, const char * databasePath);
+bool recoverNative(const void *object, const char * from, const char * to);
+void cancelProcessingNative(const void * object);
 int postProcessingNative(const void *object, int approach);
 bool exportMeshNative(
            const void *object,
@@ -113,17 +113,9 @@ class RTABMap {
        setupGraphicNative(native_rtabmap, Int32(size.width), Int32(size.height));
    }
    
-    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
+    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool, clearDatabase: Bool) -> Int {
        databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
-            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize))
-        }
-    }
-    
-    func openDatabase(databaseSource:String, databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
-        databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
-            databaseSource.utf8CString.withUnsafeBufferPointer { bufferSource -> Int in
-                return Int(openDatabase2Native(native_rtabmap, bufferSource.baseAddress, buffer.baseAddress, databaseInMemory, optimize))
-            }
+            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize, clearDatabase))
        }
    }
    
@@ -133,6 +125,18 @@ class RTABMap {
        }
    }
    
+    func recover(from: String, to: String) -> Bool {
+        from.utf8CString.withUnsafeBufferPointer { bufferFrom -> Bool in
+            to.utf8CString.withUnsafeBufferPointer { bufferTo -> Bool in
+                return recoverNative(native_rtabmap, bufferFrom.baseAddress, bufferTo.baseAddress)
+            }
+        }
+    }
+    
+    func cancelProcessing() {
+        cancelProcessingNative(native_rtabmap);
+    }
+    
    func postProcessing(approach: Int) -> Int {
        return Int(postProcessingNative(native_rtabmap, Int32(approach)))
    }
@@ -486,7 +490,7 @@ func getPreviewImage(databasePath: String) -> UIImage?
            let bitmap = CIImage(bitmapData: data, bytesPerRow: Int(image.width*image.channels), size: CGSize(width: Int(image.width), height: Int(image.height)), format: CIFormat.BGRA8, colorSpace: nil)
            return UIImage(ciImage: bitmap)
        }
-        return nil
+        return UIImage(named: "RTAB-Map1024")
    }
    return imageOut
 }
@@ -8,6 +8,7 @@
 import GLKit
 import ARKit
 import Zip
+import StoreKit

 extension Array {
    func size() -> Int {
@@ -41,6 +42,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
    private var mMaxFeatures: Int = 0
    private var mLoopThr = 0.11
    
+    private var mReviewRequested = false
+    
    // UI states
    private enum State {
        case STATE_WELCOME,    // Camera/Motion off - showing only buttons open and start new scan
@@ -1340,6 +1343,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")!);

@@ -1413,12 +1417,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        
        present(alertController, animated: true)
        
-        setGLCamera(type: 1);
+        setGLCamera(type: 0);
        startCamera();
    }
    
    func newScan()
    {
+        print("databases.size() = \(databases.size())")
+        if(databases.count >= 5 && !mReviewRequested && self.depthSupported)
+        {
+            SKStoreReviewController.requestReviewInCurrentScene()
+            mReviewRequested = true
+        }
+        
        if(mState == State.STATE_VISUALIZING)
        {
            closeVisualization()
@@ -1427,14 +1438,120 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        mMapNodes = 0;
        self.openedDatabasePath = nil
        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
-        
        let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory")
-        self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false)
-
-        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) &&
+           FileManager.default.fileExists(atPath: tmpDatabase.path) &&
+           tmpDatabase.fileSize > 1024*1024) // > 1MB
        {
-            self.setGLCamera(type: 1);
-            self.startCamera();
+            dismiss(animated: true, completion: {
+                let msg = "The previous session (\(tmpDatabase.fileSizeString)) was not correctly saved, do you want to recover it?"
+                let alert = UIAlertController(title: "Recovery", message: msg, preferredStyle: .alert)
+                let alertActionNo = UIAlertAction(title: "Ignore", style: .destructive) {
+                    (UIAlertAction) -> Void in
+                    do {
+                        try FileManager.default.removeItem(at: tmpDatabase)
+                    }
+                    catch {
+                        print("Could not clear tmp database: \(error)")
+                    }
+                    self.newScan()
+                }
+                alert.addAction(alertActionNo)
+                let alertActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                    (UIAlertAction) -> Void in
+                    // do nothing
+                }
+                alert.addAction(alertActionCancel)
+                let alertActionYes = UIAlertAction(title: "Yes", style: .default) {
+                    (UIAlertAction2) -> Void in
+
+                    let fileName = Date().getFormattedDate(format: "yyMMdd-HHmmss") + ".db"
+                    let outputDbPath = self.getDocumentDirectory().appendingPathComponent(fileName).path
+                    
+                    var indicator: UIActivityIndicatorView?
+                    
+                    let alertView = UIAlertController(title: "Recovering", message: "Please wait while recovering data...", preferredStyle: .alert)
+                    let alertViewActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                        (UIAlertAction) -> Void in
+                        self.dismiss(animated: true, completion: {
+                            self.progressView = nil
+                            
+                            indicator = UIActivityIndicatorView(style: .large)
+                            indicator?.frame = CGRect(x: 0.0, y: 0.0, width: 60.0, height: 60.0)
+                            indicator?.center = self.view.center
+                            self.view.addSubview(indicator!)
+                            indicator?.bringSubviewToFront(self.view)
+                            
+                            indicator?.startAnimating()
+                            self.rtabmap!.cancelProcessing();
+                        })
+                    }
+                    alertView.addAction(alertViewActionCancel)
+                    
+                    let previousState = self.mState
+                    self.updateState(state: .STATE_PROCESSING);
+                    
+                    self.present(alertView, animated: true, completion: {
+                        //  Add your progressbar after alert is shown (and measured)
+                        let margin:CGFloat = 8.0
+                        let rect = CGRect(x: margin, y: 84.0, width: alertView.view.frame.width - margin * 2.0 , height: 2.0)
+                        self.progressView = UIProgressView(frame: rect)
+                        self.progressView!.progress = 0
+                        self.progressView!.tintColor = self.view.tintColor
+                        alertView.view.addSubview(self.progressView!)
+                        
+                        var success : Bool = false
+                        DispatchQueue.background(background: {
+                            
+                            success = self.rtabmap!.recover(from: tmpDatabase.path, to: outputDbPath)
+                            
+                        }, completion:{
+                            if(indicator != nil)
+                            {
+                                indicator!.stopAnimating()
+                                indicator!.removeFromSuperview()
+                            }
+                            if self.progressView != nil
+                            {
+                                self.dismiss(animated: self.openedDatabasePath == nil, completion: {
+                                    if(success)
+                                    {
+                                        let alertSaved = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully recovered!", fileName), preferredStyle: .alert)
+                                        let yes = UIAlertAction(title: "OK", style: .default) {
+                                            (UIAlertAction) -> Void in
+                                            self.openDatabase(fileUrl: URL(fileURLWithPath: outputDbPath))
+                                        }
+                                        alertSaved.addAction(yes)
+                                        self.present(alertSaved, animated: true, completion: nil)
+                                    }
+                                    else
+                                    {
+                                        self.updateState(state: previousState);
+                                        self.showToast(message: "Recovery failed!", seconds: 4)
+                                    }
+                                })
+                            }
+                            else
+                            {
+                                self.showToast(message: "Recovery canceled", seconds: 2)
+                                self.updateState(state: previousState);
+                            }
+                        })
+                    })
+                }
+                alert.addAction(alertActionYes)
+                self.present(alert, animated: true, completion: nil)
+            })
+        }
+        else
+        {
+            self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false, clearDatabase: true)
+
+            if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+            {
+                self.setGLCamera(type: 0);
+                self.startCamera();
+            }
        }
    }
    
@@ -1518,12 +1635,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            
            self.openedDatabasePath = URL(fileURLWithPath: filePath)
            
-            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved on the SD-CARD!", fileName), preferredStyle: .alert)
+            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved!", fileName), preferredStyle: .alert)
            let yes = UIAlertAction(title: "OK", style: .default) {
                (UIAlertAction) -> Void in
            }
            alert.addAction(yes)
            self.present(alert, animated: true, completion: nil)
+            do {
+                let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
+                try FileManager.default.removeItem(at: tmpDatabase)
+            }
+            catch {
+                print("Could not clear tmp database: \(error)")
+            }
            self.updateDatabases()
            self.updateState(state: previousState)
        })
@@ -1559,6 +1683,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                indicator?.bringSubviewToFront(self.view)
                
                indicator?.startAnimating()
+                
+                self.rtabmap!.cancelProcessing()
            })
            
        }))
@@ -1663,6 +1789,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
            self.dismiss(animated: true)
            self.progressView = nil
+            self.rtabmap!.cancelProcessing()
        }))

        let previousState = mState
@@ -1799,7 +1926,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            stopMapping(ignoreSaving: true)
        }
        
-        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
        openedDatabasePath = fileUrl;
        let fileName: String = self.openedDatabasePath!.lastPathComponent
        
@@ -1811,7 +1937,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        updateState(state: .STATE_PROCESSING);
        var status = 0
        DispatchQueue.background(background: {
-            status = self.rtabmap!.openDatabase(databaseSource: self.openedDatabasePath!.path, databasePath: tmpDatabase.path, databaseInMemory: true, optimize: false)
+            status = self.rtabmap!.openDatabase(databasePath: self.openedDatabasePath!.path, databaseInMemory: true, optimize: false, clearDatabase: false)
        }, completion:{
            // main thread
            if(status == -1) {
@@ -1973,6 +2099,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
            self.dismiss(animated: true)
            self.progressView = nil
+            self.rtabmap!.cancelProcessing()
        }))
        
        let previousState = mState;
@@ -2210,6 +2337,14 @@ extension Date {
        dateformat.dateFormat = format
        return dateformat.string(from: self)
    }
+    
+    var millisecondsSince1970:Int64 {
+        Int64((self.timeIntervalSince1970 * 1000.0).rounded())
+    }
+    
+    init(milliseconds:Int64) {
+        self = Date(timeIntervalSince1970: TimeInterval(milliseconds) / 1000)
+    }
 }

 extension DispatchQueue {
@@ -2322,3 +2457,11 @@ extension UserDefaults {
        setDefaultsFromSettingsBundle()
    }
 }
+
+extension SKStoreReviewController {
+    public static func requestReviewInCurrentScene() {
+        if let scene = UIApplication.shared.connectedScenes.first(where: { $0.activationState == .foregroundActive }) as? UIWindowScene {
+            requestReview(in: scene)
+        }
+    }
+}
@@ -13,7 +13,7 @@
 			<string>
 ======= RTAB-Map =======
 RTAB-Map - https://github.com/introlab/rtabmap
-Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
 Copyright (c) XXX, contributors, all rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -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>
@@ -14,7 +14,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<integer>2</integer>
+			<integer>1</integer>
 			<key>Key</key>
 			<string>PointCloudDensity</string>
 			<key>Title</key>
@@ -454,7 +454,7 @@
 		</dict>
 		<dict>
 			<key>FooterText</key>
-			<string>Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
+			<string>Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
 			<key>Title</key>
 			<string>About</string>
 			<key>Type</key>
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.12</string>
+			<string>0.20.17</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
@@ -134,6 +135,24 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
          COMPONENT runtime)
 	ENDIF(WIN32)
  ENDIF(k4a_FOUND)
+  
+  IF(Torch_FOUND)
+	# Install needed cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll
+	# TODO: should be a more general way to include them if version is different
+    IF(WIN32 AND CUDA_FOUND)
+		find_file(CUDNN_OPS_DLL NAMES cudnn_ops_infer64_8.dll)
+		find_file(CUDNN_CNN_DLL NAMES cudnn_cnn_infer64_8.dll)
+		IF(CUDNN_OPS_DLL AND CUDNN_CNN_DLL)
+			MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}")
+			MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}")
+			INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL}
+			  DESTINATION ${plugin_dest_dir} 
+			  COMPONENT runtime)
+		ELSE()
+			MESSAGE(AUTHOR_WARNING "Using Torch with CUDA, but cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll are not found on the PATH, so it won't be added to package.")
+		ENDIF()
+	ENDIF(WIN32 AND CUDA_FOUND)
+  ENDIF(Torch_FOUND)

  # Install needed Qt plugins by copying directories from the qt installation
  # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
@@ -153,17 +172,17 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 		list(GET loc_list 1 plugin_type)
 		IF(NOT plugin_root)
 			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
-		ENDIF(NOT plugin_root)
+		ENDIF(NOT plugin_root)
 		#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
          DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
          COMPONENT runtime)
-	endforeach()
+	endforeach()
 	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
 		IF(WIN32)
-			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
-		ELSEIF(APPLE)
-			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+		ELSEIF(APPLE)
+			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
 		ENDIF()
 		IF(EXISTS ${plugin_loc})
 			get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
@@ -1,5 +0,0 @@
-# Ignore everything in this directory
-*
-# Except this file
-!.gitignore
-!data
@@ -1,4 +1,5 @@
 #Pre-requisites: Look for csparse
+MESSAGE(STATUS "RTAB-Map's cmake g2o find module used for convenience (for older g2o versions)")
 FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse csparse EXTERNAL/suitesparse EXTERNAL/csparse g2o/EXTERNAL/suitesparse g2o/EXTERNAL/csparse
  PATHS "C:\\Program Files\\g2o\\include\\EXTERNAL")
 FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse g2o_ext_csparse 
@@ -21,6 +22,10 @@ FIND_FILE(G2O_CONFIG_FILE g2o/config.h
  PATHS ${G2O_INCLUDE_DIR}
  NO_DEFAULT_PATH)

+FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
 #ifdef G2O_NUMBER_FORMAT_STR
 #define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
 #endif
@@ -83,7 +88,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER

 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_FACTORY_FILE AND G2O_SOLVERS_FOUND)
  SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
  SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -120,8 +125,16 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL
    SET(G2O_CPP11 0)
  ELSE()
    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
-    SET(G2O_CPP11 1)
+    FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+    STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+    IF(${matchres} EQUAL -1)
+      MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 2)
+    ELSE()
+      MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 1)
+    ENDIF()
  ENDIF()

  SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_FACTORY_FILE AND G2O_SOLVERS_FOUND)
@@ -1 +0,0 @@
-/Version.h
@@ -125,7 +125,8 @@ void RTABMAP_EXP computeMaxGraphErrors(
 		float & maxLinearError,
 		float & maxAngularError,
 		const Link ** maxLinearErrorLink = 0,
-		const Link ** maxAngularErrorLink = 0);
+		const Link ** maxAngularErrorLink = 0,
+		bool for3DoF = false);

 std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links);

@@ -133,7 +134,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,
@@ -265,54 +267,58 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float angularVelocity = 0.0f); // rad/sec

 /**
- * Get the nearest node of the target pose
+ * Find the nearest node of the target pose
 * @param nodes the nodes to search for
 * @param targetPose the target pose to search around
 * @param distance squared distance of the nearest node found (optional)
 * @return the node id.
 */
 int RTABMAP_EXP findNearestNode(
-		const std::map<int, rtabmap::Transform> & nodes,
+		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance = 0);

 /**
- * Get K nearest nodes of the target pose
- * @param nodes the nodes to search for
- * @param targetPose the target pose to search around
- * @param k number of nearest neighbors to search for
- * @return the node ids with squared distance to target pose.
- */
-std::map<int, float> RTABMAP_EXP findNearestNodes(
-		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
-		int k);
-
-/**
- * Get nodes near the query
+ * Find the nearest nodes of the query pose or node
 * @param nodeId the query id
 * @param nodes the nodes to search for
- * @param radius radius to search for (m)
+ * @param radius radius to search for (m), if 0, k should be > 0.
+ * @param k max nearest neighbors (0=all inside the radius)
 * @return the nodes with squared distance to query node.
 */
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius);
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius);
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
+		float angle = 0.0f,
+		int k=0);
+
+// typedef hack to avoid error with RTABMAP_DEPRECATED
+typedef std::map<int, float> _mapIntFloat;
+typedef std::map<int, Transform> _mapIntTransform;
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");

 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
@@ -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;
@@ -312,6 +320,7 @@ private:
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
 	float _laserScanNormalRadius;
+	float _laserScanGroundNormalsUp;
 	bool _reextractLoopClosureFeatures;
 	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
@@ -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,10 +118,11 @@ private:
 	int scanDecimation_;
 	float cellSize_;
 	bool preVoxelFiltering_;
-	bool occupancyFromDepth_;
+	int occupancySensor_;
 	bool projMapFrame_;
 	float maxObstacleHeight_;
 	int normalKSearch_;
+	float groundNormalsUp_;
 	float maxGroundAngle_;
 	float clusterRadius_;
 	int minClusterSize_;
@@ -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:
@@ -75,7 +75,8 @@ public:
 			const std::map<int, Transform> & posesIn,
 			const std::multimap<int, Link> & linksIn,
 			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut) const;
+			std::multimap<int, Link> & linksOut,
+			bool adjustPosesWithConstraints = true) const;

 public:
 	virtual ~Optimizer() {}
@@ -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.");
@@ -385,7 +385,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
-    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str(), kRGBDProximityPathRawPosesUsed().c_str()));
+    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str()));
+    RTABMAP_PARAM(RGBD, ProximityMergedScanCovFactor, double, 100.0, uFormat("Covariance factor for one-to-many proximity detection (when %s>0 and scans are used).", kRGBDProximityPathMaxNeighbors().c_str()));

    // Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -432,7 +433,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 +536,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 +573,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");
@@ -664,7 +670,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
 #endif
    RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 0.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
    RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
@@ -722,15 +728,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 +750,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 +835,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);
@@ -196,8 +196,8 @@ public:
 			bool withGrid = false,
 			bool withWords = true,
 			bool withGlobalDescriptors = true) const;
-	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
-	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
 			float clusterRadiusMax = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -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;
@@ -294,6 +307,7 @@ private:
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
 	bool _proximityOdomGuess;
+	double _proximityMergedScanCovFactor;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;
@@ -348,7 +362,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
@@ -73,7 +73,8 @@ public:
 			kCmdResume,
 			kCmdGoal,             // params: [string] label or [int] location ID
 			kCmdCancelGoal,
-			kCmdLabel             // params: [string] label, [int] location ID
+			kCmdLabel,            // params: [string] label, [int] location ID
+			kCmdRemoveLabel       // params: [string] label
 	};
 public:
 	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :
@@ -68,7 +68,8 @@ public:
 		kStateTriggeringMap,
 		kStateSettingGoal,
 		kStateCancellingGoal,
-		kStateLabelling
+		kStateLabelling,
+		kStateRemovingLabel
 	};

 public:
@@ -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}
@@ -104,6 +104,8 @@ public:
 	Transform translation() const;
 	Transform to3DoF() const;
 	Transform to4DoF() const;
+	bool is3DoF() const;
+	bool is4DoF() const;

 	cv::Mat rotationMatrix() const;
 	cv::Mat translationMatrix() const;
@@ -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_;
@@ -126,6 +126,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			UDEBUG("Cluster radius=%f", clusterRadius_);
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
 			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
+			UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -138,7 +139,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 					flatObstaclesDetected_,
 					maxGroundHeight_,
 					flatObstacles,
-					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
+					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
+					groundNormalsUp_);
 			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
@@ -166,7 +168,11 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
 		{
-			UDEBUG("");
+			UDEBUG("Radius filtering (%ld ground %ld obstacles, radius=%f k=%d)",
+					groundIndices->size(),
+					obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0),
+					noiseFilteringRadius_,
+					noiseFilteringMinNeighbors_);
 			if(groundIndices->size())
 			{
 				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
@@ -179,6 +185,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			{
 				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
 			}
+			UDEBUG("Radius filtering end (%ld ground %ld obstacles)",
+					groundIndices->size(),
+					obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0));

 			if(groundIndices->empty() && obstaclesIndices->empty())
 			{
@@ -63,7 +63,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
-		const Eigen::Vector4f & viewPoint)
+		const Eigen::Vector4f & viewPoint,
+		float groundNormalsUp)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -81,7 +82,8 @@ void segmentObstaclesFromGround(
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
 				normalKSearch,
-				viewPoint);
+				viewPoint,
+				groundNormalsUp);

 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
@@ -205,7 +207,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
-		const Eigen::Vector4f & viewPoint)
+		const Eigen::Vector4f & viewPoint,
+		float groundNormalsUp)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -220,7 +223,8 @@ void segmentObstaclesFromGround(
 			segmentFlatObstacles,
 			maxGroundHeight,
 			flatObstacles,
-			viewPoint);
+			viewPoint,
+			groundNormalsUp);
 }

 template<typename PointT>
@@ -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_ */
@@ -42,16 +42,7 @@ public:
 	static bool isCholmodAvailable();

 public:
-	OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
-		Optimizer(parameters),
-		solver_(Parameters::defaultg2oSolver()),
-		optimizer_(Parameters::defaultg2oOptimizer()),
-		pixelVariance_(Parameters::defaultg2oPixelVariance()),
-		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
-		baseline_(Parameters::defaultg2oBaseline())
-	{
-		parseParameters(parameters);
-	}
+	OptimizerG2O(const ParametersMap & parameters = ParametersMap());
 	virtual ~OptimizerG2O() {}

 	virtual Type type() const {return kTypeG2O;}
@@ -313,6 +313,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectC
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
+		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,
 		const ProgressState * state = 0);
 /**
@@ -326,6 +327,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectC
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
+		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,
 		const ProgressState * state = 0);

@@ -160,9 +160,21 @@ inline pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr uniformSampling(
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		int samples);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		int samples);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int samples);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		int samples);


 pcl::IndicesPtr RTABMAP_EXP passThrough(
@@ -449,6 +461,99 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius);

+/* for convenience */
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+
+/**
+ * @brief Filter points based on distance from their viewpoint.
+ *
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param viewpointIndices should be same size than the input cloud, it tells the viewpoint index in viewpoints for each point.
+ * @param viewpoints the viewpoints.
+ * @param factor will determine the search radius based on the distance from a point and its viewpoint. Setting it higher will filter points farther from accurate points (but processing time will be also higher).
+ * @param neighborScale will scale the search radius of neighbors found around a point. Setting it higher will accept more noisy points close to accurate points (but processing time will be also higher).
+ * @return the indices of the points satisfying the parameters.
+ */
+
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+
 /**
 * For convenience.
 */
@@ -592,13 +697,15 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);

 /**
 * @brief Given a normal and a maximum angle error, keep all points of the cloud
@@ -622,42 +729,48 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);

 /**
 * For convenience.
@@ -152,7 +152,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
-		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
+		float groundNormalsUp = 0);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -165,7 +166,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
-		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
+		float groundNormalsUp = 0);

 template<typename PointT>
 void occupancy2DFromGroundObstacles(
@@ -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,
@@ -430,18 +481,27 @@ void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);

 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & viewpoints,
 		const LaserScan & rawScan,
 		const std::vector<int> & viewpointIds,
-		LaserScan & scan);
+		LaserScan & scan,
+		float groundNormalsUp = 0.0f);

 pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);

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

@@ -370,18 +373,22 @@ ENDIF(WITH_TORO)

 IF(G2O_FOUND)
 	IF(g2o_FOUND)
-	  SET(LIBRARIES
-             ${LIBRARIES}
+	  SET(LIBRARIES ${LIBRARIES}
 	     g2o::core
-	     g2o::solver_cholmod
 	     g2o::solver_eigen
 	     g2o::solver_pcg
-	     g2o::solver_csparse
-	     g2o::csparse_extension
 	     g2o::types_slam2d
 	     g2o::types_slam3d
-	     g2o::types_sba
-	  )
+	     g2o::types_sba)
+	  IF(TARGET g2o::solver_csparse)
+	    SET(LIBRARIES ${LIBRARIES}
+          g2o::solver_csparse
+          g2o::csparse_extension)
+      ENDIF(TARGET g2o::solver_csparse)
+      IF(TARGET g2o::solver_cholmod)
+        SET(LIBRARIES ${LIBRARIES} 
+          g2o::solver_cholmod)
+      ENDIF(TARGET g2o::solver_cholmod)
 	ELSE()
 	  SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
@@ -392,13 +399,11 @@ IF(G2O_FOUND)
 		${G2O_LIBRARIES}
 	  )
 	ENDIF()
-	SET(SRC_FILES
-    		${SRC_FILES}
+	SET(SRC_FILES ${SRC_FILES}
 		optimizer/g2o/edge_se3_xyzprior.cpp
 	)
 	IF(WITH_VERTIGO)
-		SET(SRC_FILES
-    			${SRC_FILES}
+		SET(SRC_FILES ${SRC_FILES}
 			optimizer/vertigo/g2o/edge_se2Switchable.cpp
 			optimizer/vertigo/g2o/edge_se3Switchable.cpp
 			optimizer/vertigo/g2o/edge_switchPrior.cpp
@@ -419,7 +424,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 +433,7 @@ IF(CERES_FOUND)
 		${LIBRARIES}
 		${CERES_LIBRARIES}
 	)
-ENDIF(CERES_FOUND)
+ENDIF(WITH_CERES AND CERES_FOUND)

 IF(libpointmatcher_FOUND)
 	SET(INCLUDE_DIRS 
@@ -448,6 +453,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 +501,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 +720,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)
@@ -753,4 +776,11 @@ install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/
 		COMPONENT devel 
 		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!");
@@ -898,7 +898,8 @@ void computeMaxGraphErrors(
 		float & maxLinearError,
 		float & maxAngularError,
 		const Link ** maxLinearErrorLink,
-		const Link ** maxAngularErrorLink)
+		const Link ** maxAngularErrorLink,
+		bool for3DoF)
 {
 	maxLinearErrorRatio = -1;
 	maxAngularErrorRatio = -1;
@@ -918,7 +919,7 @@ void computeMaxGraphErrors(
 			float linearError = uMax3(
 					fabs(iter->second.transform().x() - t.x()),
 					fabs(iter->second.transform().y() - t.y()),
-					fabs(iter->second.transform().z() - t.z()));
+					for3DoF?0:fabs(iter->second.transform().z() - t.z()));
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
@@ -937,8 +938,8 @@ void computeMaxGraphErrors(
 			t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
 			iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
 			float angularError = uMax3(
-					fabs(opt_roll - link_roll),
-					fabs(opt_pitch - link_pitch),
+					for3DoF?0:fabs(opt_roll - link_roll),
+					for3DoF?0:fabs(opt_pitch - link_pitch),
 					fabs(opt_yaw - link_yaw));
 			angularError = angularError>M_PI?2*M_PI-angularError:angularError;
 			UASSERT(iter->second.rotVariance(false)>0.0);
@@ -989,12 +990,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 +1009,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;
 			}
@@ -1335,7 +1337,7 @@ std::map<int, Transform> radiusPosesFiltering(

 		//pcl::IndicesPtr indicesOut(new std::vector<int>);
 		//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
-		UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
+		UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
 		//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
 		//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);

@@ -2078,12 +2080,12 @@ std::list<std::pair<int, Transform> > computePath(
 }

 int findNearestNode(
-		const std::map<int, rtabmap::Transform> & nodes,
+		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance)
 {
 	int id = 0;
-	std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, 0, 0, 1);
 	if(!nearestNodes.empty())
 	{
 		id = nearestNodes.begin()->first;
@@ -2095,70 +2097,44 @@ int findNearestNode(
 	return id;
 }

+// return <id, sqrd distance>, excluding query
 std::map<int, float> findNearestNodes(
-		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
+		int nodeId,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
 		int k)
 {
-	std::map<int, float> nearestIds;
-	if(nodes.size() && !targetPose.isNull())
-	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		cloud->resize(nodes.size());
-		std::vector<int> ids(nodes.size());
-		int oi = 0;
-		for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
-		{
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			ids[oi++] = iter->first;
-		}
+	UASSERT(uContains(poses, nodeId));

-		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-		kdTree->setInputCloud(cloud);
-		std::vector<int> ind;
-		std::vector<float> dist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->nearestKSearch(pt, k, ind, dist);
-
-		for(unsigned int i=0; i<ind.size(); ++i)
-		{
-			nearestIds.insert(std::make_pair(ids[ind[i]], dist[i]));
-		}
-	}
-	return nearestIds;
-}
-
-// return <id, sqrd distance>, excluding query
-std::map<int, float> getNodesInRadius(
-		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius)
-{
-	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
+	std::map<int, Transform> nodesMinusTarget = poses;
+	Transform targetPose = poses.at(nodeId);
 	nodesMinusTarget.erase(nodeId);
-	return getNodesInRadius(targetPose, nodesMinusTarget, radius);
+	return findNearestNodes(targetPose, nodesMinusTarget, radius, angle, k);
 }

-// return <id, sqrd distance>, excluding query
-std::map<int, float> getNodesInRadius(
+// return <id, sqrd distance>
+std::map<int, float> findNearestNodes(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius)
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
 {
+	UASSERT(radius>=0.0f);
+	UASSERT(k>=0);
+	UASSERT(radius > 0.0f || k>0);
 	std::map<int, float> foundNodes;
-	if(nodes.empty())
+	if(poses.empty())
 	{
 		return foundNodes;
 	}

 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(nodes.size());
-	std::vector<int> ids(nodes.size());
+	cloud->resize(poses.size());
+	std::vector<int> ids(poses.size());
 	int oi = 0;
-	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
 		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
@@ -2175,89 +2151,33 @@ std::map<int, float> getNodesInRadius(
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
-		for(unsigned int i=0; i<ind.size(); ++i)
+		if(radius>0.0f)
 		{
-			if(ind[i] >=0)
-			{
-				foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
-			}
+			kdTree->radiusSearch(pt, radius, ind, sqrdDist, k);
+		}
+		else
+		{
+			kdTree->nearestKSearch(pt, k, ind, sqrdDist);
 		}
-	}
-	UDEBUG("found nodes=%d", (int)foundNodes.size());
-	return foundNodes;
-}
-
-// return <id, Transform>, excluding query
-std::map<int, Transform> getPosesInRadius(
-		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle)
-{
-	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
-	nodesMinusTarget.erase(nodeId);
-	return getPosesInRadius(targetPose, nodesMinusTarget, radius, angle);
-}
-// return <id, Transform>, excluding query
-std::map<int, Transform> getPosesInRadius(
-		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle)
-{
-	std::map<int, Transform> foundNodes;
-	if(nodes.empty())
-	{
-		return foundNodes;
-	}
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(nodes.size());
-	std::vector<int> ids(nodes.size());
-	int oi = 0;
-	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
-	{
-		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-		ids[oi] = iter->first;
-		++oi;
-	}
-	cloud->resize(oi);
-	ids.resize(oi);
-
-	if(cloud->size())
-	{
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-		kdTree->setInputCloud(cloud);
-		std::vector<int> ind;
-		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
-
 		Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
-
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
 			if(ind[i] >=0)
 			{
 				if(angle > 0.0f)
 				{
-					const Transform & checkT = nodes.at(ids[ind[i]]);
+					const Transform & checkT = poses.at(ids[ind[i]]);
 					// same orientation?
 					Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 					if(a <= angle)
 					{
-						foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+						foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
 					}
 				}
 				else
 				{
-					foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+					foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
 				}
 			}
 		}
@@ -2266,6 +2186,62 @@ std::map<int, Transform> getPosesInRadius(
 	return foundNodes;
 }

+// return <id, Transform>, excluding query
+std::map<int, Transform> findNearestPoses(
+		int nodeId,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
+{
+	UASSERT(uContains(poses, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = poses;
+	Transform targetPose = poses.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return findNearestPoses(targetPose, nodesMinusTarget, radius, angle, k);
+}
+// return <id, Transform>
+std::map<int, Transform> findNearestPoses(
+		const Transform & targetPose,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
+{
+	std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, radius, angle, k);
+	std::map<int, Transform> foundPoses;
+	for(std::map<int, float>::iterator iter=nearestNodes.begin(); iter!=nearestNodes.end(); ++iter)
+	{
+		foundPoses.insert(*poses.find(iter->first));
+	}
+	UDEBUG("found nodes=%d", (int)foundPoses.size());
+	return foundPoses;
+}
+
+// deprecated stuff
+std::map<int, float> findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k)
+{
+	return findNearestNodes(targetPose, nodes, 0, 0, k);
+}
+std::map<int, float> getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius)
+{
+	return findNearestNodes(nodeId, nodes, radius);
+}
+std::map<int, float> getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius)
+{
+	return findNearestNodes(targetPose, nodes, radius);
+}
+std::map<int, Transform> getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle)
+{
+	return findNearestPoses(nodeId, nodes, radius, angle);
+}
+std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle)
+{
+	return findNearestPoses(targetPose, nodes, radius, angle);
+}
+
+
 float computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
 		unsigned int fromIndex,
@@ -368,8 +368,8 @@ LaserScan LaserScan::clone() const

 float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
 {
-	UASSERT(pointIndex < data_.cols);
-	UASSERT(channelOffset < data_.channels());
+	UASSERT(pointIndex < (unsigned int)data_.cols);
+	UASSERT(channelOffset < (unsigned int)data_.channels());
 	return data_.ptr<float>(0, pointIndex)[channelOffset];
 }

@@ -387,7 +387,7 @@ LaserScan LaserScan::operator+(const LaserScan & scan)
 	{
 		if(this->empty())
 		{
-			dest = LaserScan(scan.data().clone(), 0, 0, this->format());
+			dest = scan.clone();
 		}
 		else
 		{
@@ -98,6 +98,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
 	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
+	_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
@@ -565,6 +566,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
 	Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
 	Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
+	Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
 	Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
 	Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
@@ -2066,10 +2068,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 +2083,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>();
 		}
@@ -2522,6 +2525,14 @@ int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase
 		if(id == 0 && _dbDriver && lookInDatabase)
 		{
 			_dbDriver->getNodeIdByLabel(label, id);
+			if(_signatures.find(id) != _signatures.end())
+			{
+				// The signature is already in WM, but label was not
+				// found above. It means the label has been cleared in
+				// current session (not yet saved to database), so return
+				// not found.
+				id = 0;
+			}
 		}
 	}
 	return id;
@@ -2531,15 +2542,35 @@ bool Memory::labelSignature(int id, const std::string & label)
 {
 	// verify that this label is not used
 	int idFound=getSignatureIdByLabel(label);
+	if(idFound == 0 && label.empty() && _labels.find(id)==_labels.end())
+	{
+		UWARN("Trying to remove label from node %d but it has already no label", id);
+		return false;
+	}
 	if(idFound == 0 || idFound == id)
 	{
 		Signature * s  = this->_getSignature(id);
 		if(s)
 		{
-			uInsert(_labels, std::make_pair(s->id(), label));
+			if(label.empty())
+			{
+				UWARN("Label \"%s\" removed from node %d", _labels.at(id).c_str(), id);
+				_labels.erase(id);
+			}
+			else
+			{
+				if(_labels.find(id)!=_labels.end())
+				{
+					UWARN("Label \"%s\" set to node %d (previously labeled \"%s\")", label.c_str(), id, _labels.at(id).c_str());
+				}
+				else
+				{
+					UWARN("Label \"%s\" set to node %d", label.c_str(), id);
+				}
+				uInsert(_labels, std::make_pair(s->id(), label));
+			}
 			s->setLabel(label);
 			_linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
-			UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 			return true;
 		}
 		else if(_dbDriver)
@@ -2550,9 +2581,25 @@ bool Memory::labelSignature(int id, const std::string & label)
 			_dbDriver->loadSignatures(ids,signatures);
 			if(signatures.size())
 			{
-				uInsert(_labels, std::make_pair(signatures.front()->id(), label));
+				if(label.empty())
+				{
+					UWARN("Label \"%s\" removed from node %d", _labels.at(id).c_str(), id);
+					_labels.erase(id);
+				}
+				else
+				{
+					if(_labels.find(id)!=_labels.end())
+					{
+						UWARN("Label \"%s\" set to node %d (previously labeled \"%s\")", label.c_str(), id, _labels.at(id).c_str());
+					}
+					else
+					{
+						UWARN("Label \"%s\" set to node %d", label.c_str(), id);
+					}
+					uInsert(_labels, std::make_pair(id, label));
+				}
+
 				signatures.front()->setLabel(label);
-				UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 				_dbDriver->asyncSave(signatures.front()); // move it again to trash
 				return true;
 			}
@@ -2564,7 +2611,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 	}
 	else if(idFound)
 	{
-		UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
+		UWARN("Another node %d has already label \"%s\", cannot set it to node %d", idFound, label.c_str(), id);
 	}
 	return false;
 }
@@ -2731,13 +2778,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 +2794,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 +4097,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 +4712,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 +4906,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);
@@ -5081,7 +5482,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				0,
 				_laserScanVoxelSize,
 				_laserScanNormalK,
-				_laserScanNormalRadius);
+				_laserScanNormalRadius,
+				_laserScanGroundNormalsUp);
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
 		UDEBUG("time normals scan = %fs", t);
@@ -5285,7 +5687,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 +5834,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,10 +52,11 @@ 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()),
+	groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
 	clusterRadius_(Parameters::defaultGridClusterRadius()),
 	minClusterSize_(Parameters::defaultGridMinClusterSize()),
@@ -91,7 +92,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)
 	{
@@ -115,6 +116,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
 	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
 	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_);
 	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
 	{
 		maxGroundAngle_ *= M_PI/180.0f;
@@ -284,12 +286,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 +330,7 @@ void OccupancyGrid::createLocalMap(
 	else
 	{
 		// 3D
-		if(!occupancyFromDepth_)
+		if(occupancySensor_ == 0 || occupancySensor_ == 2)
 		{
 			if(!node.sensorData().laserScanRaw().isEmpty())
 			{
@@ -336,10 +338,11 @@ void OccupancyGrid::createLocalMap(
 				const Transform & t = node.sensorData().laserScanRaw().localTransform();
 				LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
 #ifdef RTABMAP_OCTOMAP
-				// clipping will be done in OctoMap
-				float maxRange = grid3D_&&rayTracing_?0.0f:cloudMaxDepth_;
+				// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
+				float maxRange = rayTracing_?0.0f:cloudMaxDepth_;
 #else
-				float maxRange = cloudMaxDepth_;
+				// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
+				float maxRange = !grid3D_ && rayTracing_?0.0f:cloudMaxDepth_;
 #endif
 				if(cloudMinDepth_ > 0.0f || maxRange > 0.0f)
 				{
@@ -350,14 +353,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;
@@ -369,10 +393,11 @@ void OccupancyGrid::createLocalMap(
 					node.sensorData(),
 					cloudDecimation_,
 #ifdef RTABMAP_OCTOMAP
-					// clipping will be done in OctoMap
-					grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
+					// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
+					rayTracing_?0.0f:cloudMaxDepth_,
 #else
-					cloudMaxDepth_,
+					// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
+					!grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
 #endif
 					cloudMinDepth_,
 					indices.get(),
@@ -380,6 +405,7 @@ void OccupancyGrid::createLocalMap(
 					roiRatios_);

 			// update viewpoint
+			viewPoint = cv::Point3f(0,0,0);
 			if(node.sensorData().cameraModels().size())
 			{
 				// average of all local transforms
@@ -407,7 +433,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
 }
@@ -872,7 +891,7 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 						}
 					}

-					RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, orderedPoses.size() == 1);
+					RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false,  true);
 					if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
 					{
 						n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
@@ -938,6 +957,10 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 							}
 						}
 					}
+				}
+
+				if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty())
+				{
 					octree_->updateInnerOccupancy();
 				}

@@ -1209,21 +1232,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)
 			{
@@ -190,7 +190,8 @@ void Optimizer::getConnectedGraph(
 		const std::map<int, Transform> & posesIn,
 		const std::multimap<int, Link> & linksIn,
 		std::map<int, Transform> & posesOut,
-		std::multimap<int, Link> & linksOut) const
+		std::multimap<int, Link> & linksOut,
+		bool adjustPosesWithConstraints) const
 {
 	UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
 	UASSERT(fromId>0);
@@ -216,15 +217,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,33 +234,40 @@ 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))
 					{
-						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
+						if(adjustPosesWithConstraints)
 						{
-							Transform t;
-							if(kter->second.from()==fromId)
+							if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
 							{
-								t = kter->second.transform();
+								Transform t;
+								if(kter->second.from()==currentId)
+								{
+									t = kter->second.transform();
+								}
+								else
+								{
+									t = kter->second.transform().inverse();
+								}
+								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
 							}
 							else
 							{
-								t = 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));
 							}
-							posesOut.insert(std::make_pair(toId, (posesOut.at(fromId) * t).to3DoF()));
 						}
 						else
 						{
-							Transform t = posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse());
-							posesOut.insert(std::make_pair(toId, t));
+							posesOut.insert(*posesIn.find(toId));
 						}
 						// add prior links
 						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
@@ -274,7 +282,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;
@@ -136,7 +136,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
 	{
-		_workingDir = iter->second;
+		_workingDir = uReplaceChar(iter->second, '~', UDirectory::homeDir());
 	}

 	bool pointToPlane = _pointToPlane;
@@ -277,8 +277,12 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 #ifndef RTABMAP_CCCORELIB
 	if(_strategy==2)
 	{
-		UWARN("Parameter %s is set to true but RTAB-Map has not been built with CCCoreLib support. Setting to 0.", Parameters::kIcpStrategy().c_str());
+#ifdef RTABMAP_POINTMATCHER
+		_strategy = 1;
+#else
 		_strategy = 0;
+#endif
+		UWARN("Parameter %s is set to 2 but RTAB-Map has not been built with CCCoreLib support. Setting to %d.", Parameters::kIcpStrategy().c_str(), _strategy);
 	}
 #else
 	if(_strategy==2 && _pointToPlane)
@@ -329,6 +333,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Max translation=%f", _maxTranslation);
 	UDEBUG("Max rotation=%f", _maxRotation);
 	UDEBUG("Downsampling step=%d", _downsamplingStep);
+	UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false");
 	UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false");
 	UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
 	UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);
@@ -292,9 +292,16 @@ void RtabmapThread::mainLoop()
 		_rtabmap->clearPath(0);
 		break;
 	case kStateLabelling:
-		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()))
+		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label")))
 		{
-			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()));
+			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label")));
+		}
+		break;
+	case kStateRemovingLabel:
+		id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"), true);
+		if(!_rtabmap->labelLocation(id, ""))
+		{
+			this->post(new RtabmapLabelErrorEvent(id, parameters.at("label")));
 		}
 		break;
 	default:
@@ -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());
@@ -221,6 +221,20 @@ Transform Transform::to4DoF() const
 	return Transform(x,y,z, 0,0,yaw);
 }

+bool Transform::is3DoF() const
+{
+	return is4DoF() && z() == 0.0;
+}
+
+bool Transform::is4DoF() const
+{
+	return r13() == 0.0 &&
+		   r23() == 0.0 &&
+		   r31() == 0.0 &&
+		   r32() == 0.0 &&
+		   r33() == 0.0;
+}
+
 cv::Mat Transform::rotationMatrix() const
 {
 	return data_.colRange(0, 3).clone();
@@ -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;
 		  }
 		}
@@ -137,15 +137,14 @@ rtabmap::Transform icpCC(
 		icpTransformation.setNull();
 		return icpTransformation;
 	}
-	else if(finalPointCount < 50)
+	else if(!transform.R.isValid())
 	{
-		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 50 ", finalPointCount);
+		std::string msg = uFormat("CCCoreLib has failed: Rotation matrix is invalid");
 		UDEBUG(msg.c_str());
 		if(errorMsg)
 		{
 			*errorMsg = msg;
 		}
-
 		icpTransformation.setNull();
 		return icpTransformation;
 	}
@@ -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());
 				}
 			}
 		}
@@ -205,7 +205,14 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 	{
 		cv::Mat newFrame;
 		cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
-		data.setImageRaw(newFrame);
+		if(data.stereoCameraModel().isValidForProjection())
+		{
+			data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModel());
+		}
+		else
+		{
+			data.setRGBDImage(newFrame, data.depthRaw(), data.cameraModels());
+		}
 	}

 	if(!localMap_.empty())
@@ -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);
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	4d8c1d8b90	bump 0.20.18	2022-01-28 17:12:24 -05:00
matlabbe	315350dbfc	CCCorelib: updated wrong output check. Updated RGBD/MaxOdomCacheSize approach to work properly with Rtabmap/CreateIntermediateNodes=true.	2022-01-28 14:48:32 -05:00
matlabbe	02140f92db	fixed build	2022-01-27 22:55:59 -05:00
matlabbe	402afc07ed	ExportDialog: added ground normals up option, added camera projection mask and decimation options. Export CLI: added --ground_normals_up and --cam_projection_mask options, changed --bin option by --ascii option (now binary by default). DBViewer: warn when scan from depth option is enabled but there is no depth.	2022-01-27 17:57:56 -05:00
matlabbe	a8e5bbf415	export: added --texture_roi_ratios option	2022-01-26 20:12:04 -05:00
matlabbe	5b3a7d5674	Localization: fixed always rejected localization when proximity and global loop closures are both detected at the same time at two different locations (clearing proximity detections from odometry cache if global loop closures are good).	2022-01-26 15:39:09 -05:00
matlabbe	d56692640e	DBViewer: fixed grid cell size in 3D View. Memory/Grid, using Icp/PointToPlaneGroundNormalsUp parameter when normals are computed. Grid: fixed 2D noHit ray tracing.	2022-01-25 18:10:43 -05:00
Ben	2dc7b59b05	allow negative min disparity in StereoBM dialog (#817 )	2022-01-24 17:02:47 -05:00
Ben	bbe10bca39	fix missing curl install in focal dockerfile (#816 ) * add missing curl install to focal dockerfile * add missing apt-get update	2022-01-24 16:44:39 -05:00
matlabbe	e2dc3ac67e	OctoMap: fixed empty occupancy not correctly updated when ray tracing is done on more than one local map	2022-01-23 16:29:11 -05:00
matlabbe	83d1e27b81	Refactored graph::getPosesInRadius -> graph::findNearestPoses. Labels can be removed: added Remove label option in MainWindow. In localization mode, label set with id=0 is set to nearest node of current pose.	2022-01-20 18:56:38 -05:00
matlabbe	3c215b9b4d	android: fixed driver preference for arcore	2022-01-20 10:57:39 -05:00
matlabbe	bdaed2f362	Tango: fixed google tango driver not selected automatically on first use	2022-01-19 20:24:23 -05:00
matlabbe	e048d8ae72	Fixed compilation error without octomap	2022-01-19 19:53:49 -05:00
matlabbe	5b868d665f	Grid/Sensor=2: fixed viewpoint offset bug of the camera.	2022-01-19 19:27:32 -05:00
matlabbe	a56373f66d	GUI/ExportClouds: fixed color of scans having RGB instead of intensity.	2022-01-19 16:59:43 -05:00
matlabbe	5ec73bee8d	ios: improved significantly rendering performance	2022-01-19 02:11:38 -05:00
matlabbe	b1dd44526d	ios: fixed background camera not shown in landscape mode (iPhone)	2022-01-17 22:06:35 -05:00
matlabbe	de62bbb9b7	Grid: don't filter max range before doing ray tracing (to ray trace empty space).	2022-01-17 14:22:11 -05:00
matlabbe	7a5c50cb0e	Fixed compilation warnings	2022-01-16 15:27:52 -05:00
matlabbe	9622fe8393	Parameters: set default Icp/PointToPlaneRadius to 0 to avoid 'Both radius and K defined' error when using 3d lidar.	2022-01-16 12:56:18 -05:00
matlabbe	5f55f63443	report: ignoring landmarks after optimization to avoid errors when exporting poses	2022-01-15 22:02:45 -05:00
matlabbe	aa0e6ef692	GUI:Export Poses: fixed poses!=stamps issue when there are landmarks (landmarks now only exported with g2o format)	2022-01-15 21:49:40 -05:00
matlabbe	03a106ea36	Fixed build without g2o	2022-01-15 13:00:12 -05:00
matlabbe	29a2b64a0c	RGBD/NeighborLinkRefining: do one proximity detection by time if intermediate nodes are added. Optimized radiusFiltering(). Updated default parameters of proportionalRadiusFiltering().	2022-01-14 18:47:57 -05:00
matlabbe	51f6b32f47	Fixed windows build (https://ci.appveyor.com/project/matlabbe/rtabmap/builds/42207812 )	2022-01-14 14:33:32 -05:00
matlabbe	9e0173f4cb	Export: added random sample filter, added proportional radius filter, refactored when normals are computed (now after the clouds are assembled and voxelized), moved ceiling and floor filtering inside assembling loop.	2022-01-14 13:36:01 -05:00
matlabbe	12c2dd707c	Localization with Reg/Force3DoF=true, fixed how graph optimization errors are computed (use only x,y,yaw).	2022-01-12 16:59:03 -05:00
matlabbe	fcec98105d	Added RGBD/ProximityMergedScanCovFactor parameter. Localization: fixed output height when Reg/Force3DoF=true but input poses are 6DoF. Transform: added is3DoF() and is4DoF() functions. GTSAM: when Reg/Force3DoF=true, copy input roll,pitch,z values for output poses. RegIcp: fixed working memory dir '~' conversion. RGBD/ProximityGlobalScanMap: fixed map::at error when some nodes don't have scans.	2022-01-12 16:14:42 -05:00
matlabbe	a4ec95963e	Fixed a debug information	2022-01-11 15:59:53 -05:00
matlabbe	49353b134c	LaserScan: fixed destination format when source is empty with operator+	2022-01-11 15:32:30 -05:00
Tim Clephas	f69085304e	List dependencies only once (#812 )	2022-01-11 10:40:48 -05:00
Marvin Stüde	3f797e02e3	Adds Boost serialization package for libpointmatcher case (#810 )	2022-01-07 13:11:12 -05:00
matlabbe	ab50ba0b6c	Fixed android30	2022-01-05 21:39:27 -05:00
matlabbe	528b640a18	workflows: added android30 docker image	2022-01-04 20:54:53 -05:00
matlabbe	4d8a7b4659	CameraTango: fixed opencv4 errors	2022-01-04 20:49:03 +00:00
matlabbe	ba6575fcb9	Added android30 build	2022-01-04 14:55:32 -05:00
matlabbe	1173f06bc8	android: fixed opencv4 deprecated errors	2022-01-04 19:46:20 +00:00
matlabbe	a08fce2068	ios: added requestReview()	2022-01-03 20:57:41 -05:00
matlabbe	d8324a11d5	android: fixed sharing file exiting bug, fixed camera not correctly set back to device default on reset settings.	2022-01-03 17:51:23 -05:00
matlabbe	a6c9b3babe	💄 changed some cmake warnings in info	2022-01-03 16:33:21 -05:00
matlabbe	145430cafd	Fixed noetic build (g2o error)	2022-01-03 16:05:11 -05:00
matlabbe	5194f40794	Fixed g2o build error with melodic/noetic versions	2022-01-03 15:37:06 -05:00
matlabbe	129bbf8f98	ios: added placeholder R in Library view for db not having preview (like recovered database)	2022-01-03 13:44:29 -05:00
matlabbe	a9d4a16ba4	bump ios version 0.20.17	2022-01-02 23:40:46 -05:00
matlabbe	b2f13c2772	android: fixed g2o seg fault (CACHE_SE3_OFFSET not registered)	2022-01-02 23:30:55 -05:00
matlabbe	f3ace6c86a	Added recovery feature to android	2022-01-02 18:09:57 -05:00
matlabbe	3147b4ab56	ios: added recovery feature	2022-01-02 17:08:44 -05:00
matlabbe	fcde52c6bf	ios: set First-P. View by default	2021-12-30 15:37:46 -05:00
matlabbe	5b31a4725d	Update README.md	2021-12-29 19:05:35 -05:00
matlabbe	e4955ee386	GUI: fixed mynteye raw images support error	2021-12-29 16:12:02 -05:00
matlabbe	e5b4973ddc	GUI: Added Orbbec Astra icon	2021-12-29 15:53:40 -05:00
matlabbe	f3095a2e0a	Calibration dialog: fixed right panel width size. Win32: packaging missing cudnn dlls (when torch is used)	2021-12-29 15:13:13 -05:00
matlabbe	044145926f	ios: incremented build number	2021-12-28 18:35:55 -05:00
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