Updated package.xml version to 0.20.14

OdometryFLOAM: fixed published local scan map local transform
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.
2026-09-13 15:00:21 +08:00 · 2021-09-30 10:14:24 -04:00 · 2021-09-30 10:05:56 -04:00 · 2021-09-30 09:53:50 -04:00 · 2021-09-28 17:12:08 -04:00 · 2021-09-27 20:20:11 -04:00
58 changed files with 2633 additions and 679 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}}/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,70 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      matrix:
+        docker_tag: [xenial, bionic, focal, android23, android24, android26]
+        include:
+        - docker_tag: xenial
+          docker_tags: |
+            introlab3it/rtabmap:xenial
+            introlab3it/rtabmap:16.04
+          docker_path: 'xenial'
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_path: 'bionic'
+        - docker_tag: focal
+          docker_tags: |
+            introlab3it/rtabmap:focal
+            introlab3it/rtabmap:20.04
+            introlab3it/rtabmap:latest
+          docker_path: 'focal'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_path: 'bionic/android/rtabmap_api23'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_path: 'bionic/android/rtabmap_api24'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_path: 'bionic/android/rtabmap_api26'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      -
+        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
+          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 14)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -187,6 +187,7 @@ option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
 option(WITH_CCCORELIB     "Include CCCoreLib support"            ON)
 option(WITH_LOAM          "Include LOAM support"                 ON)
+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)
@@ -209,6 +210,7 @@ option(WITH_VINS          "Include VINS-Fusion support"          ON)
 option(WITH_OPENVINS      "Include OpenVINS support"             ON)
 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 +255,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 +264,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()

@@ -493,6 +496,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 +605,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 +647,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 +695,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)
 	 #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 +705,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)

 	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
 	       G2O_FOUND OR
@@ -813,6 +830,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 +895,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()
@@ -1267,11 +1291,11 @@ MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()

 IF(CERES_FOUND)
-MESSAGE(STATUS " *With Ceres                = YES (License: BSD)")
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = YES (License: BSD)")
 ELSEIF(NOT WITH_CERES)
-MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (WITH_CERES=OFF)")
 ELSE()
-MESSAGE(STATUS " *With Ceres                = NO (Ceres not found)")
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (Ceres not found)")
 ENDIF()

 IF(G2O_FOUND OR GTSAM_FOUND)
@@ -1302,7 +1326,7 @@ 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)")
@@ -1465,6 +1489,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)
@@ -1,13 +1,13 @@
-rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap ![.](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
 =======

 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)

 [![Release][release-image]][releases]
 [![License][license-image]][license]
-Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
+Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)

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

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

 #backward compatibilities
@@ -55,6 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@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
@@ -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;
@@ -54,7 +54,8 @@ public:
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
 		kTypeVINS = 9,
-		kTypeOpenVINS = 10
+		kTypeOpenVINS = 10,
+		kTypeFLOAM = 11
 	};

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

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

@@ -368,7 +368,7 @@ 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.");
@@ -432,7 +432,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");

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

@@ -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_ */
@@ -253,7 +253,7 @@ cv::Mat RTABMAP_EXP mergeTextures(

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

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

 cv::Mat RTABMAP_EXP computeNormals(
 		const cv::Mat & laserScan,
@@ -89,6 +89,7 @@ SET(SRC_FILES
    odometry/OdometryOkvis.cpp
    odometry/OdometryORBSLAM.cpp
    odometry/OdometryLOAM.cpp
+    odometry/OdometryFLOAM.cpp
    odometry/OdometryMSCKF.cpp
    odometry/OdometryVINS.cpp
    odometry/OdometryOpenVINS.cpp
@@ -344,8 +345,8 @@ ENDIF(mynteye_FOUND)
 IF(depthai_FOUND)
 	SET(LIBRARIES
 		${LIBRARIES}
-		depthai::depthai-core
-		depthai::depthai-opencv
+		depthai::core
+		depthai::opencv
 	)
 ENDIF(depthai_FOUND)

@@ -489,6 +490,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}
@@ -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)
 	{
@@ -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";
 		}

@@ -2066,10 +2066,11 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 		bool ok = true;
 		std::map<int, Transform> poses = _dbDriver->loadOptimizedPoses(lastlocalizationPose);
 		// Make sure optimized poses match the working directory! Otherwise return nothing.
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && ok; ++iter)
+		for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end() && ok; ++iter)
 		{
 			if(_workingMem.find(iter->first)==_workingMem.end())
 			{
+				UWARN("Node %d not found in working memory", iter->first);
 				ok = false;
 			}
 		}
@@ -2080,7 +2081,7 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 				  "poses to force re-update. If you want to use the "
 				  "saved optimized poses, set %s to true",
 				  (int)poses.size(),
-				  (int)_workingMem.size(),
+				  (int)_workingMem.size()-1, // less virtual place
 				  Parameters::kMemInitWMWithAllNodes().c_str());
 			return std::map<int, Transform>();
 		}
@@ -4050,6 +4051,221 @@ void Memory::generateGraph(const std::string & fileName, const std::set<int> & i
 	_dbDriver->generateGraph(fileName, ids, _signatures);
 }

+int Memory::cleanupLocalGrids(
+		const std::map<int, Transform> & poses,
+		const cv::Mat & map,
+		float xMin,
+		float yMin,
+		float cellSize,
+		int cropRadius,
+		bool filterScans)
+{
+	if(!_dbDriver)
+	{
+		UERROR("A database must be loaded first...");
+		return -1;
+	}
+
+	if(poses.empty() || poses.lower_bound(1) == poses.end())
+	{
+		UERROR("Empty poses?!");
+		return -1;
+	}
+	if(map.empty())
+	{
+		UERROR("Map is empty!");
+		return -1;
+	}
+	UASSERT(cropRadius>=0);
+	UASSERT(cellSize>0.0f);
+
+	int maxPoses = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
+	{
+		++maxPoses;
+	}
+
+	UINFO("Processing %d grids...", maxPoses);
+	int processedGrids = 1;
+	int gridsScansModified = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter, ++processedGrids)
+	{
+		// local grid
+		cv::Mat gridGround;
+		cv::Mat gridObstacles;
+		cv::Mat gridEmpty;
+
+		// scan
+		SensorData data = this->getNodeData(iter->first, false, true, false, true);
+		LaserScan scan;
+		data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
+
+		if(!gridObstacles.empty())
+		{
+			UASSERT(data.gridCellSize() == cellSize);
+			cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
+			int oi = 0;
+			for(int i=0; i<gridObstacles.cols; ++i)
+			{
+				const float * ptr = gridObstacles.ptr<float>(0, i);
+				cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
+				pt = util3d::transformPoint(pt, iter->second);
+
+				int x = int((pt.x - xMin) / cellSize + 0.5f);
+				int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+				if(x>=0 && x<map.cols &&
+				   y>=0 && y<map.rows)
+				{
+					bool obstacleDetected = false;
+
+					for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+					{
+						for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+						{
+							if(x+j>=0 && x+j<map.cols &&
+							   y+k>=0 && y+k<map.rows &&
+							   map.at<unsigned char>(y+k,x+j) == 100)
+							{
+								obstacleDetected = true;
+							}
+						}
+					}
+
+					if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
+					{
+						// Verify that we don't have an obstacle on neighbor cells
+						cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+						++oi;
+					}
+				}
+			}
+
+			if(oi != gridObstacles.cols)
+			{
+				UINFO("Grid id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, gridObstacles.cols, oi);
+				gridsScansModified += 1;
+
+				// update
+				Signature * s = this->_getSignature(iter->first);
+				cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
+				bool modifyDb = true;
+				if(s)
+				{
+					s->sensorData().setOccupancyGrid(gridGround, newObstacles, gridEmpty, cellSize, data.gridViewPoint());
+					if(!s->isSaved())
+					{
+						// not saved in database yet
+						modifyDb = false;
+					}
+				}
+				if(modifyDb)
+				{
+					_dbDriver->updateOccupancyGrid(iter->first,
+							gridGround,
+							newObstacles,
+							gridEmpty,
+							cellSize,
+							data.gridViewPoint());
+				}
+			}
+		}
+
+		if(filterScans && !scan.isEmpty())
+		{
+			Transform mapToScan = iter->second * scan.localTransform();
+
+			cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
+			int oi = 0;
+			for(int i=0; i<scan.size(); ++i)
+			{
+				const float * ptr = scan.data().ptr<float>(0, i);
+				cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
+				pt = util3d::transformPoint(pt, mapToScan);
+
+				int x = int((pt.x - xMin) / cellSize + 0.5f);
+				int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+				if(x>=0 && x<map.cols &&
+				   y>=0 && y<map.rows)
+				{
+					bool obstacleDetected = false;
+
+					for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+					{
+						for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+						{
+							if(x+j>=0 && x+j<map.cols &&
+							   y+k>=0 && y+k<map.rows &&
+							   map.at<unsigned char>(y+k,x+j) == 100)
+							{
+								obstacleDetected = true;
+							}
+						}
+					}
+
+					if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
+					{
+						// Verify that we don't have an obstacle on neighbor cells
+						cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+						++oi;
+					}
+				}
+			}
+
+			if(oi != scan.size())
+			{
+				UINFO("Scan id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, (int)scan.size(), oi);
+				gridsScansModified += 1;
+
+				// update
+				if(scan.angleIncrement()!=0)
+				{
+					// copy meta data
+					scan = LaserScan(
+							cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+							scan.format(),
+							scan.rangeMin(),
+							scan.rangeMax(),
+							scan.angleMin(),
+							scan.angleMax(),
+							scan.angleIncrement(),
+							scan.localTransform());
+				}
+				else
+				{
+					// copy meta data
+					scan = LaserScan(
+							cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+							scan.maxPoints(),
+							scan.rangeMax(),
+							scan.format(),
+							scan.localTransform());
+				}
+
+				// update
+				Signature * s = this->_getSignature(iter->first);
+				bool modifyDb = true;
+				if(s)
+				{
+					s->sensorData().setLaserScan(scan, true);
+					if(!s->isSaved())
+					{
+						// not saved in database yet
+						modifyDb = false;
+					}
+				}
+				if(modifyDb)
+				{
+					_dbDriver->updateLaserScan(iter->first, scan);
+				}
+
+			}
+		}
+	}
+	return gridsScansModified;
+}
+
 int Memory::getNi(int signatureId) const
 {
 	int ni = 0;
@@ -5285,7 +5501,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())
@@ -34,6 +34,7 @@ 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"
@@ -90,6 +91,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;
@@ -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);

@@ -824,6 +824,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 +1059,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;
@@ -348,9 +348,9 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	this->parseParameters(allParameters);

 	Transform lastPose;
+	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
 	if(!_memory->isIncremental())
 	{
-		_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
 		if(_optimizedPoses.empty() &&
 			_memory->getWorkingMem().size()>1 &&
 			_memory->getWorkingMem().lower_bound(1)!=_memory->getWorkingMem().end())
@@ -404,6 +404,16 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 			UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
 		}
 	}
+	else
+	{
+		_lastLocalizationPose = lastPose;
+		if(!_optimizedPoses.empty())
+		{
+			std::map<int, Transform> tmp;
+			// Get just the links
+			_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
+		}
+	}

 	if(_databasePath.empty())
 	{
@@ -4706,6 +4716,12 @@ void Rtabmap::getGraph(
 				poses = _optimizedPoses; // guess
 				cv::Mat covariance;
 				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
+				if(!global && !_optimizedPoses.empty())
+				{
+					// We send directly the already optimized poses if they are set
+					UDEBUG("_optimizedPoses=%ld poses=%ld", _optimizedPoses.size(), poses.size());
+					poses = _optimizedPoses;
+				}
 			}
 			else
 			{
@@ -5080,6 +5096,88 @@ int Rtabmap::detectMoreLoopClosures(
 	return (int)loopClosuresAdded.size();
 }

+bool Rtabmap::globalBundleAdjustment(
+		int optimizerType,
+		bool rematchFeatures,
+		int iterations,
+		float pixelVariance)
+{
+	if(!_optimizedPoses.empty() && !_constraints.empty())
+	{
+		int iterations = Parameters::defaultOptimizerIterations();
+		float pixelVariance = Parameters::defaultg2oPixelVariance();
+		ParametersMap params = _parameters;
+		Parameters::parse(params, Parameters::kOptimizerIterations(), iterations);
+		Parameters::parse(params, Parameters::kg2oPixelVariance(), pixelVariance);
+		if(iterations > 0)
+		{
+			uInsert(params, ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
+		}
+		if(pixelVariance > 0.0f)
+		{
+			uInsert(params, ParametersPair(Parameters::kg2oPixelVariance(), uNumber2Str(pixelVariance)));
+		}
+
+		std::map<int, Signature> signatures;
+		for(std::map<int, Transform>::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter)
+		{
+			if(_memory->getSignature(iter->first))
+			{
+				signatures.insert(std::make_pair(iter->first, *_memory->getSignature(iter->first)));
+			}
+		}
+
+		Optimizer * optimizer = Optimizer::create((Optimizer::Type)optimizerType, params);
+		std::map<int, Transform> poses = optimizer->optimizeBA(
+				_optimizeFromGraphEnd?_optimizedPoses.lower_bound(1)->first:_optimizedPoses.rbegin()->first,
+				_optimizedPoses,
+				_constraints,
+				signatures,
+				rematchFeatures);
+		delete optimizer;
+
+		if(poses.empty())
+		{
+			UERROR("Optimization failed!");
+		}
+		else
+		{
+			_optimizedPoses = poses;
+			// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+			_memory->save2DMap(cv::Mat(), 0, 0, 0);
+			return true;
+		}
+	}
+	else
+	{
+		UERROR("Optimized poses (%ld) or constraints (%ld) are empty!", _optimizedPoses.size(), _constraints.size());
+	}
+	return false;
+}
+
+int Rtabmap::cleanupLocalGrids(
+		const std::map<int, Transform> & poses,
+		const cv::Mat & map,
+		float xMin,
+		float yMin,
+		float cellSize,
+		int cropRadius,
+		bool filterScans)
+{
+	if(_memory)
+	{
+		return _memory->cleanupLocalGrids(
+				poses,
+				map,
+				xMin,
+				yMin,
+				cellSize,
+				cropRadius,
+				filterScans);
+	}
+	return -1;
+}
+
 int Rtabmap::refineLinks()
 {
 	if(!_rgbdSlamMode)
@@ -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
@@ -74,7 +74,8 @@ CameraOpenNI2::CameraOpenNI2(
 	_deviceId(deviceId),
 	_openNI2StampsAndIDsUsed(false),
 	_depthHShift(0),
-	_depthVShift(0)
+	_depthVShift(0),
+	_depthDecimation(1)
 #endif
 {
 }
@@ -184,6 +185,14 @@ void CameraOpenNI2::setIRDepthShift(int horizontal, int 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
@@ -544,8 +553,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
@@ -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
@@ -275,19 +275,21 @@ namespace clams

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

-float SCAN_PERIOD = 0.1f;
-
 namespace rtabmap {

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

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

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

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

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

@@ -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);
@@ -2863,7 +2863,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	int cameraProcessed = 0;
 	for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
 	{
-		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.begin();
+		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.find(pter->first);
 		if(iter!=cameraModels.end() && !iter->second.empty())
 		{
 			for(size_t i=0; i<iter->second.size(); ++i)
@@ -601,71 +601,72 @@ pcl::texture_mapping::CameraVector createTextureCameras(
 		const std::map<int, cv::Mat> & cameraDepths,
 		const std::vector<float> & roiRatios)
 {
-	UASSERT_MSG(poses.size() == cameraModels.size(), uFormat("%d vs %d", (int)poses.size(), (int)cameraModels.size()).c_str());
 	UASSERT(roiRatios.empty() || roiRatios.size() == 4);
 	pcl::texture_mapping::CameraVector cameras;
-	std::map<int, Transform>::const_iterator poseIter=poses.begin();
-	std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.begin();
-	for(; poseIter!=poses.end(); ++poseIter, ++modelIter)
+
+	for(std::map<int, Transform>::const_iterator poseIter=poses.begin(); poseIter!=poses.end(); ++poseIter)
 	{
-		UASSERT(poseIter->first == modelIter->first);
+		std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.find(poseIter->first);

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

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

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

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

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

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

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

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

 	mvsUtils::MultiViewParams mp(sfmData);

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

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

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

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

 	return true;
@@ -38,7 +38,7 @@ RUN cd libpointmatcher && \
    cd && \
    rm -r libpointmatcher

-# AliceVision
+# AliceVision v2.4.0 modified (Sept 13 2021)
 RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
      libsuitesparse-dev \
      libceres-dev \
@@ -50,38 +50,44 @@ RUN cd oiio && \
    git checkout Release-2.0.12 && \
    mkdir build && \
    cd build && \
-    cmake .. && \
+    cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \
    make -j$(nproc) && \
    make install && \
    cd && \
    rm -r oiio
-RUN git clone https://github.com/alembic/alembic.git
-RUN cd alembic && \
-    git checkout 1.7.12 && \
+RUN git clone https://github.com/assimp/assimp.git
+RUN cd assimp && \
+    git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \
    mkdir build && \
    cd build && \
    cmake .. && \
    make -j$(nproc) && \
    make install && \
    cd && \
-    rm -r alembic
+    rm -r assimp
 RUN git clone https://github.com/alicevision/geogram.git
 RUN cd geogram && \
-    git checkout v1.7.1 && \
+    git checkout v1.7.6 && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \
+    git apply geogram_8b2ae61.patch && \
    ./configure.sh && \
    cd build/Linux64-gcc-dynamic-Release && \
    make -j$(nproc) && \
    make install && \
    cd && \
    rm -r geogram
+# cmake >=3.11 required
+RUN wget -nv https://github.com/Kitware/CMake/releases/download/v3.17.0/cmake-3.17.0-Linux-x86_64.tar.gz && \
+    tar -xzf cmake-3.17.0-Linux-x86_64.tar.gz && \
+    rm cmake-3.17.0-Linux-x86_64.tar.gz
 RUN git clone https://github.com/alicevision/AliceVision.git --recursive
 RUN cd AliceVision && \
-    git checkout v2.2.0 && \
-    wget https://gist.githubusercontent.com/matlabbe/469bba5e7733ad6f2e3d7857b84f1f9e/raw/edaa88ed38344219af1cc919a5597f5a74445336/alice_vision_eigen.patch && \
-    git apply alice_vision_eigen.patch && \
+    git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
+    git apply alicevision_0f6115b.patch && \
    mkdir build && \
    cd build && \
-    cmake -DALICEVISION_USE_CUDA=OFF .. && \
+    ../../cmake-3.17.0-Linux-x86_64/bin/cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \
    make -j$(nproc) && \
    make install && \
    cd && \
@@ -96,7 +102,7 @@ RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
    cd rtabmap/build && \
-    cmake -DWITH_ALICE_VISION=ON .. && \
+    ../../cmake-3.17.0-Linux-x86_64/bin/cmake -DWITH_ALICE_VISION=ON .. && \
    make -j2 && \
    make install && \
    cd ../.. && \
@@ -4,6 +4,7 @@ FROM introlab3it/rtabmap:android-deps

 WORKDIR /root/

+ARG CACHE_DATE=2016-01-01
 ADD rtabmap.bash /root/rtabmap.bash
 RUN chmod +x rtabmap.bash
 RUN /bin/bash -c "./rtabmap.bash /opt/android 23"
@@ -4,6 +4,7 @@ FROM introlab3it/rtabmap:android-deps

 WORKDIR /root/

+ARG CACHE_DATE=2016-01-01
 ADD rtabmap.bash /root/rtabmap.bash
 RUN chmod +x rtabmap.bash
 RUN /bin/bash -c "./rtabmap.bash /opt/android 24"
@@ -4,6 +4,7 @@ FROM introlab3it/rtabmap:android-deps

 WORKDIR /root/

+ARG CACHE_DATE=2016-01-01
 ADD rtabmap.bash /root/rtabmap.bash
 RUN chmod +x rtabmap.bash
 RUN /bin/bash -c "./rtabmap.bash /opt/android 26"
@@ -84,50 +84,52 @@ RUN cd zed-open-capture && \
    cd && \
    rm -r zed-open-capture

-# AliceVision
-# Issue: It could be possible to use version >2.2, but there is a seg fault after texturing the mesh (see #564).
-RUN apt-get update && apt-get install -y \
+# AliceVision v2.4.0 modified (Sept 13 2021)
+RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
      libsuitesparse-dev \
      libceres-dev \
      xorg-dev \
      libglu1-mesa-dev \
-      wget \
-      python-is-python3
+      wget
 RUN git clone https://github.com/OpenImageIO/oiio.git
 RUN cd oiio && \
    git checkout Release-2.0.12 && \
    mkdir build && \
    cd build && \
-    cmake .. && \
+    cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \
    make -j$(nproc) && \
    make install && \
    cd && \
    rm -r oiio
-RUN git clone https://github.com/alembic/alembic.git
-RUN cd alembic && \
-    git checkout 1.7.12 && \
+RUN git clone https://github.com/assimp/assimp.git
+RUN cd assimp && \
+    git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \
    mkdir build && \
    cd build && \
    cmake .. && \
    make -j$(nproc) && \
    make install && \
    cd && \
-    rm -r alembic
-RUN git clone -b v1.7.1 https://github.com/alicevision/geogram.git
+    rm -r assimp
+RUN git clone https://github.com/alicevision/geogram.git
 RUN cd geogram && \
+    git checkout v1.7.6 && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \
+    git apply geogram_8b2ae61.patch && \
    ./configure.sh && \
    cd build/Linux64-gcc-dynamic-Release && \
    make -j$(nproc) && \
    make install && \
    cd && \
    rm -r geogram
-RUN git clone -b v2.2.0 https://github.com/alicevision/AliceVision.git --recursive
+RUN git clone https://github.com/alicevision/AliceVision.git --recursive
 RUN cd AliceVision && \
-    wget https://gist.githubusercontent.com/matlabbe/469bba5e7733ad6f2e3d7857b84f1f9e/raw/f0545b36028a1156e857ed433547fdade0cbf53f/alice_vision_eigen.patch && \
-    git apply alice_vision_eigen.patch && \
+    git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
+    git apply alicevision_0f6115b.patch && \
    mkdir build && \
    cd build && \
-    cmake -DALICEVISION_USE_CUDA=OFF .. && \
+    cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \
    make -j$(nproc) && \
    make install && \
    cd && \
@@ -339,6 +339,7 @@ private Q_SLOTS:
 	void updateKpROI();
 	void updateStereoDisparityVisibility();
 	void updateFeatureMatchingVisibility();
+	void updateOdometryStackedIndex(int index);
 	void useOdomFeatures();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
@@ -314,7 +314,7 @@ void CloudViewer::createMenu()
 	_aSetNormalsScale = new QAction("Set normals scale...", this);
 	_aSetIntensityRedColormap = new QAction("Red/Yellow Colormap", this);
 	_aSetIntensityRedColormap->setCheckable(true);
-	_aSetIntensityRedColormap->setChecked(false);
+	_aSetIntensityRedColormap->setChecked(true);
 	_aSetIntensityRainbowColormap = new QAction("Rainbow Colormap", this);
 	_aSetIntensityRainbowColormap->setCheckable(true);
 	_aSetIntensityRainbowColormap->setChecked(false);
@@ -1757,6 +1757,7 @@ void DatabaseViewer::updateIds()
 	uSleep(100);
 	QApplication::processEvents();

+	int lastValidNodeId = 0;
 	for(int i=0; i<ids_.size(); ++i)
 	{
 		idToIndex_.insert(ids_[i], i);
@@ -1772,6 +1773,17 @@ void DatabaseViewer::updateIds()
 		dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps, sensors);
 		mapIds_.insert(std::make_pair(ids_[i], mapId));
 		weights_.insert(std::make_pair(ids_[i], w));
+		if(w>=0)
+		{
+			for(std::multimap<int, Link>::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
+			{
+				// Make compatible with old databases, when "weight=-1" was not yet introduced to identify ignored nodes
+				if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
+				{
+					lastValidNodeId = ids_[i];
+				}
+			}
+		}
 		if(wmStates.find(ids_[i]) != wmStates.end())
 		{
 			wmStates_.insert(std::make_pair(ids_[i], wmStates.at(ids_[i])));
@@ -1983,6 +1995,38 @@ void DatabaseViewer::updateIds()
 				ui_->label_optimizeFrom->setText(tr("Root [%1, %2]").arg(odomPoses_.begin()->first).arg(odomPoses_.rbegin()->first));
 			}
 		}
+
+		if(lastValidNodeId>0)
+		{
+			// find full connected graph from last node in working memory
+			Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
+
+			std::map<int, rtabmap::Transform> posesOut;
+			std::multimap<int, rtabmap::Link> linksOut;
+			UINFO("Get connected graph from %d (%d poses, %d links)", lastValidNodeId, (int)odomPoses_.size(), (int)links_.size());
+			optimizer->getConnectedGraph(
+					lastValidNodeId,
+					odomPoses_,
+					links_,
+					posesOut,
+					linksOut);
+
+			if(!posesOut.empty())
+			{
+				bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
+				Parameters::parse(dbDriver_->getLastParameters(), Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
+				if(optimizeFromGraphEnd)
+				{
+					ui_->spinBox_optimizationsFrom->setValue(posesOut.rbegin()->first);
+				}
+				else
+				{
+					ui_->spinBox_optimizationsFrom->setValue(posesOut.lower_bound(1)->first);
+				}
+			}
+
+			delete optimizer;
+		}
 	}

 	ui_->menuExport_poses->setEnabled(!odomPoses_.empty());
@@ -7511,8 +7555,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			reextractVisualFeatures ||
 			!silent)
 		{
-			dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
-			dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
+			dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
+			dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
 		
 			if(!silent)
 			{
@@ -7555,10 +7599,27 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 				fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
 				toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));

-				if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
+				if(!fromS->sensorData().laserScanCompressed().isEmpty() && !toS->sensorData().laserScanCompressed().isEmpty())
 				{
 					UWARN("There are laser scans in data, but generate laser scan from "
-						  "depth image option is activated. Ignoring saved laser scans...");
+						  "depth image option is activated (GUI Parameters->Refine). "
+						  "Ignoring saved laser scans...");
+				}
+				else
+				{
+					QString msg = tr("Generating laser scan from depth image is checked "
+							"(GUI Parameters->Refine), but selected nodes don't contain "
+							"depth data. Empty laser scans are generated, so transform "
+							"estimation will likely fail. Uncheck to use laser scans instead "
+							"(if there are some).");
+					if(!silent)
+					{
+
+						QMessageBox::warning(this,
+								tr("Refine a link"),
+								msg);
+					}
+					UWARN(msg.toStdString().c_str());
 				}
 			}
 			else
@@ -7785,9 +7846,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			!silent)
 		{
 			// Add sensor data to generate features
-			dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
+			dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
 			fromS->sensorData().uncompressData();
-			dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
+			dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
 			toS->sensorData().uncompressData();
 			if(reextractVisualFeatures)
 			{
@@ -7796,6 +7857,33 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				toS->removeAllWords();
 				toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			}
+			if(reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked())
+			{
+				// generate laser scans from depth image
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
+						fromS->sensorData(),
+						ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
+						ui_->doubleSpinBox_icp_maxDepth->value(),
+						ui_->doubleSpinBox_icp_minDepth->value(),
+						0,
+						ui_->parameters_toolbox->getParameters());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
+						toS->sensorData(),
+						ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
+						ui_->doubleSpinBox_icp_maxDepth->value(),
+						ui_->doubleSpinBox_icp_minDepth->value(),
+						0,
+						ui_->parameters_toolbox->getParameters());
+				int maxLaserScans = cloudFrom->size();
+				fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
+				toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
+
+				if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
+				{
+					UWARN("There are laser scans in data, but generate laser scan from "
+						  "depth image option is activated. Ignoring saved laser scans...");
+				}
+			}
 		}
 		else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
 		{
@@ -230,6 +230,16 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->doubleSpinBox_cameraFilterVel, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_cameraFilterVelRad, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_multiband, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_multiband, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
+	connect(_ui->spinBox_multiband_downscale, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->lineEdit_multiband_nbcontrib, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_multiband_unwrap, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_multiband_fillholes, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_multiband_padding, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_multiband_bestscore, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_multiband_angle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_multiband_forcevisible, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));

 	connect(_ui->checkBox_poisson_outputPolygons, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_poisson_manifold, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -440,6 +450,14 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mesh_textureBlending", _ui->checkBox_blending->isChecked());
 	settings.setValue("mesh_textureBlendingDecimation", _ui->comboBox_blendingDecimation->currentIndex());
 	settings.setValue("mesh_textureMultiband", _ui->checkBox_multiband->isChecked());
+	settings.setValue("mesh_textureMultibandDownScale", _ui->spinBox_multiband_downscale->value());
+	settings.setValue("mesh_textureMultibandNbContrib", _ui->lineEdit_multiband_nbcontrib->text());
+	settings.setValue("mesh_textureMultibandUnwrap", _ui->comboBox_multiband_unwrap->currentIndex());
+	settings.setValue("mesh_textureMultibandFillHoles", _ui->checkBox_multiband_fillholes->isChecked());
+	settings.setValue("mesh_textureMultibandPadding", _ui->spinBox_multiband_padding->value());
+	settings.setValue("mesh_textureMultibandBestScoreThr", _ui->doubleSpinBox_multiband_bestscore->value());
+	settings.setValue("mesh_textureMultibandAngleHardThr", _ui->doubleSpinBox_multiband_angle->value());
+	settings.setValue("mesh_textureMultibandForceVisible", _ui->checkBox_multiband_forcevisible->isChecked());


 	settings.setValue("mesh_angle_tolerance", _ui->doubleSpinBox_mesh_angleTolerance->value());
@@ -607,6 +625,14 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->checkBox_blending->setChecked(settings.value("mesh_textureBlending", _ui->checkBox_blending->isChecked()).toBool());
 	_ui->comboBox_blendingDecimation->setCurrentIndex(settings.value("mesh_textureBlendingDecimation", _ui->comboBox_blendingDecimation->currentIndex()).toInt());
 	_ui->checkBox_multiband->setChecked(settings.value("mesh_textureMultiband", _ui->checkBox_multiband->isChecked()).toBool());
+	_ui->spinBox_multiband_downscale->setValue(settings.value("mesh_textureMultibandDownScale", _ui->spinBox_multiband_downscale->value()).toInt());
+	_ui->lineEdit_multiband_nbcontrib->setText(settings.value("mesh_textureMultibandNbContrib", _ui->lineEdit_multiband_nbcontrib->text()).toString());
+	_ui->comboBox_multiband_unwrap->setCurrentIndex(settings.value("mesh_textureMultibandUnwrap", _ui->comboBox_multiband_unwrap->currentIndex()).toInt());
+	_ui->checkBox_multiband_fillholes->setChecked(settings.value("mesh_textureMultibandFillHoles", _ui->checkBox_multiband_fillholes->isChecked()).toBool());
+	_ui->spinBox_multiband_padding->setValue(settings.value("mesh_textureMultibandPadding", _ui->spinBox_multiband_padding->value()).toInt());
+	_ui->doubleSpinBox_multiband_bestscore->setValue(settings.value("mesh_textureMultibandBestScoreThr", _ui->doubleSpinBox_multiband_bestscore->value()).toDouble());
+	_ui->doubleSpinBox_multiband_angle->setValue(settings.value("mesh_textureMultibandAngleHardThr", _ui->doubleSpinBox_multiband_angle->value()).toDouble());
+	_ui->checkBox_multiband_forcevisible->setChecked(settings.value("mesh_textureMultibandForceVisible", _ui->checkBox_multiband_forcevisible->isChecked()).toBool());

 	_ui->doubleSpinBox_mesh_angleTolerance->setValue(settings.value("mesh_angle_tolerance", _ui->doubleSpinBox_mesh_angleTolerance->value()).toDouble());
 	_ui->checkBox_mesh_quad->setChecked(settings.value("mesh_quad", _ui->checkBox_mesh_quad->isChecked()).toBool());
@@ -745,7 +771,7 @@ void ExportCloudsDialog::restoreDefaults()

 	_ui->checkBox_textureMapping->setChecked(false);
 	_ui->comboBox_meshingTextureFormat->setCurrentIndex(0);
-	_ui->comboBox_meshingTextureSize->setCurrentIndex(5); // 4096
+	_ui->comboBox_meshingTextureSize->setCurrentIndex(6); // 8192
 	_ui->spinBox_mesh_maxTextures->setValue(1);
 	_ui->doubleSpinBox_meshingTextureMaxDistance->setValue(3.0);
 	_ui->doubleSpinBox_meshingTextureMaxDepthError->setValue(0.0);
@@ -765,6 +791,15 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->checkBox_blending->setChecked(true);
 	_ui->comboBox_blendingDecimation->setCurrentIndex(0);
 	_ui->checkBox_multiband->setChecked(false);
+	_ui->spinBox_multiband_downscale->setValue(2);
+	_ui->lineEdit_multiband_nbcontrib->setText("1 5 10 0");
+	_ui->comboBox_multiband_unwrap->setCurrentIndex(0);
+	_ui->checkBox_multiband_fillholes->setChecked(false);
+	_ui->spinBox_multiband_padding->setValue(5);
+	_ui->doubleSpinBox_multiband_bestscore->setValue(0.1);
+	_ui->doubleSpinBox_multiband_angle->setValue(90.0);
+	_ui->checkBox_multiband_forcevisible->setChecked(false);
+

 	_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
 	_ui->checkBox_mesh_quad->setChecked(false);
@@ -891,6 +926,7 @@ void ExportCloudsDialog::updateReconstructionFlavor()
 	_ui->groupBox_subtraction->setVisible(_ui->checkBox_subtraction->isChecked());
 	_ui->groupBox_textureMapping->setVisible(_ui->checkBox_textureMapping->isChecked());
 	_ui->groupBox_cameraFilter->setVisible(_ui->checkBox_cameraFilter->isChecked());
+	_ui->groupBox_multiband->setVisible(_ui->checkBox_multiband->isChecked());

 	// dense texturing options
 	if(_ui->checkBox_meshing->isChecked())
@@ -3713,7 +3749,6 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 					float h = _ui->doubleSpinBox_footprintHeight->value();
 					float w = _ui->doubleSpinBox_footprintWidth->value();
 					float l = _ui->doubleSpinBox_footprintLength->value();
-					int before=  indices->size();
 					indices = util3d::cropBox(
 							cloud,
 							indices,
@@ -4444,10 +4479,19 @@ void ExportCloudsDialog::saveTextureMeshes(
 								0,
 								_dbDriver,
 								textureSize,
+								_ui->spinBox_multiband_downscale->value(),
+								_ui->lineEdit_multiband_nbcontrib->text().toStdString(),
 								_ui->comboBox_meshingTextureFormat->currentText().toStdString(),
 								gains,
 								blendingGains,
-								contrastValues);
+								contrastValues,
+								true,
+								_ui->comboBox_multiband_unwrap->currentIndex(),
+								_ui->checkBox_multiband_fillholes->isChecked(),
+								_ui->spinBox_multiband_padding->value(),
+								_ui->doubleSpinBox_multiband_bestscore->value(),
+								_ui->doubleSpinBox_multiband_angle->value(),
+								_ui->checkBox_multiband_forcevisible->isChecked());
 						if(success)
 						{
 							_progressDialog->incrementStep();
@@ -6931,7 +6931,7 @@ void MainWindow::downloadAllClouds()
 	items.append("Global map not optimized");

 	bool ok;
-	QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 2, false, &ok);
+	QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 0, false, &ok);
 	if(ok)
 	{
 		bool optimized=false, global=false;
@@ -6975,7 +6975,7 @@ void MainWindow::downloadPoseGraph()
 	items.append("Global map not optimized");

 	bool ok;
-	QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 2, false, &ok);
+	QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 0, false, &ok);
 	if(ok)
 	{
 		bool optimized=false, global=false;
@@ -196,6 +196,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_OPENVINS
 	_ui->odom_strategy->setItemData(10, 0, Qt::UserRole - 1);
 #endif
+#ifndef RTABMAP_FLOAM
+	_ui->odom_strategy->setItemData(11, 0, Qt::UserRole - 1);
+#endif

 #if CV_MAJOR_VERSION < 3
 	_ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1);
@@ -689,6 +692,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->openni2_stampsIdsUsed, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_hshift, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_vshift, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->openni2_depth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_freenect2MinDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_freenect2MaxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1246,9 +1250,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :

 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
-	connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryType, SLOT(setCurrentIndex(int)));
+	connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOdometryStackedIndex(int)));
 	_ui->odom_strategy->setCurrentIndex(Parameters::defaultOdomStrategy());
-	_ui->stackedWidget_odometryType->setCurrentIndex(Parameters::defaultOdomStrategy());
+	updateOdometryStackedIndex(Parameters::defaultOdomStrategy());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
 	_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
@@ -1360,6 +1364,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// Odometry LOAM
 	_ui->odom_loam_sensor->setObjectName(Parameters::kOdomLOAMSensor().c_str());
 	_ui->odom_loam_scan_period->setObjectName(Parameters::kOdomLOAMScanPeriod().c_str());
+	_ui->odom_loam_resolution->setObjectName(Parameters::kOdomLOAMResolution().c_str());
 	_ui->odom_loam_linvar->setObjectName(Parameters::kOdomLOAMLinVar().c_str());
 	_ui->odom_loam_angvar->setObjectName(Parameters::kOdomLOAMAngVar().c_str());
 	_ui->odom_loam_localMapping->setObjectName(Parameters::kOdomLOAMLocalMapping().c_str());
@@ -1949,6 +1954,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->openni2_stampsIdsUsed->setChecked(false);
 		_ui->openni2_hshift->setValue(0);
 		_ui->openni2_vshift->setValue(0);
+		_ui->openni2_depth_decimation->setValue(1);
 		_ui->comboBox_freenect2Format->setCurrentIndex(1);
 		_ui->doubleSpinBox_freenect2MinDepth->setValue(0.3);
 		_ui->doubleSpinBox_freenect2MaxDepth->setValue(12.0);
@@ -2404,6 +2410,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->lineEdit_openni2OniPath->setText(settings.value("oniPath", _ui->lineEdit_openni2OniPath->text()).toString());
 	_ui->openni2_hshift->setValue(settings.value("hshift", _ui->openni2_hshift->value()).toInt());
 	_ui->openni2_vshift->setValue(settings.value("vshift", _ui->openni2_vshift->value()).toInt());
+	_ui->openni2_depth_decimation->setValue(settings.value("depthDecimation", _ui->openni2_depth_decimation->value()).toInt());
 	settings.endGroup(); // Openni2

 	settings.beginGroup("Freenect2");
@@ -2917,6 +2924,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("oniPath", 		  _ui->lineEdit_openni2OniPath->text());
 	settings.setValue("hshift",           _ui->openni2_hshift->value());
 	settings.setValue("vshift",           _ui->openni2_vshift->value());
+	settings.setValue("depthDecimation",  _ui->openni2_depth_decimation->value());
 	settings.endGroup(); // Openni2

 	settings.beginGroup("Freenect2");
@@ -4913,6 +4921,18 @@ void PreferencesDialog::updateFeatureMatchingVisibility()
 	_ui->groupBox_gms->setVisible(_ui->reextract_nn->currentIndex() == 7);
 }

+void PreferencesDialog::updateOdometryStackedIndex(int index)
+{
+	if(index == 11) // FLOAM -> LOAM
+	{
+		_ui->stackedWidget_odometryType->setCurrentIndex(7);
+	}
+	else
+	{
+		_ui->stackedWidget_odometryType->setCurrentIndex(index);
+	}
+}
+
 void PreferencesDialog::useOdomFeatures()
 {
 	if(this->isVisible() && _ui->checkBox_useOdomFeatures->isChecked())
@@ -5172,7 +5192,8 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo) || //T265
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo) || // ZEDm, ZED2
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo) || // MYNT EYE S
-			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo));
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo) ||
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo));
 	_ui->stackedWidget_imuFilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() > 0);
 	_ui->groupBox_madgwickfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 1);
 	_ui->groupBox_complementaryfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 2);
@@ -6332,6 +6353,7 @@ Camera * PreferencesDialog::createCamera(
 				}
 				((CameraOpenNI2*)camera)->setIRDepthShift(_ui->openni2_hshift->value(), _ui->openni2_vshift->value());
 				((CameraOpenNI2*)camera)->setMirroring(_ui->openni2_mirroring->isChecked());
+				((CameraOpenNI2*)camera)->setDepthDecimation(_ui->openni2_depth_decimation->value());
 			}
 		}
 	}
@@ -6,8 +6,8 @@
   <rect>
    <x>0</x>
    <y>0</y>
-    <width>814</width>
-    <height>680</height>
+    <width>1032</width>
+    <height>869</height>
   </rect>
  </property>
  <property name="windowTitle">
@@ -23,9 +23,9 @@
      <property name="geometry">
       <rect>
        <x>0</x>
-        <y>-1225</y>
-        <width>780</width>
-        <height>5739</height>
+        <y>-3537</y>
+        <width>998</width>
+        <height>5673</height>
       </rect>
      </property>
      <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -2751,6 +2751,202 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
               </layout>
              </widget>
             </item>
+             <item>
+              <widget class="QGroupBox" name="groupBox_multiband">
+               <property name="title">
+                <string>MultiBand Texturing</string>
+               </property>
+               <layout class="QGridLayout" name="gridLayout_22" columnstretch="0,1">
+                <item row="0" column="1">
+                 <widget class="QLabel" name="label_49">
+                  <property name="text">
+                   <string>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.</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="4" column="1">
+                 <widget class="QLabel" name="label_52">
+                  <property name="text">
+                   <string>Texture edge padding size in pixel.</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="2" column="1">
+                 <widget class="QLabel" name="label_50">
+                  <property name="text">
+                   <string>Unwrap method: 0=basic (default, &gt;600k faces, fast), 1=ABF (&lt;=300k faces, generate 1 atlas), 2=LSCM (&lt;=600k faces, optimize space).</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="7" column="1">
+                 <widget class="QLabel" name="label_55">
+                  <property name="text">
+                   <string>Force visible by all vertices. Triangle visibility is based on the union of vertices visibility.</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="0" column="0">
+                 <widget class="QSpinBox" name="spinBox_multiband_downscale">
+                  <property name="maximum">
+                   <number>16</number>
+                  </property>
+                  <property name="value">
+                   <number>2</number>
+                  </property>
+                 </widget>
+                </item>
+                <item row="2" column="0">
+                 <widget class="QComboBox" name="comboBox_multiband_unwrap">
+                  <item>
+                   <property name="text">
+                    <string>Basic</string>
+                   </property>
+                  </item>
+                  <item>
+                   <property name="text">
+                    <string>ABF</string>
+                   </property>
+                  </item>
+                  <item>
+                   <property name="text">
+                    <string>LSCM</string>
+                   </property>
+                  </item>
+                 </widget>
+                </item>
+                <item row="6" column="1">
+                 <widget class="QLabel" name="label_54">
+                  <property name="text">
+                   <string>Angle hard threshold. 0 to disable angle hard threshold filtering.</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="5" column="1">
+                 <widget class="QLabel" name="label_53">
+                  <property name="text">
+                   <string>Best score threshold. 0 to disable filtering based on threshold to relative best score.</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="7" column="0">
+                 <widget class="QCheckBox" name="checkBox_multiband_forcevisible">
+                  <property name="text">
+                   <string/>
+                  </property>
+                 </widget>
+                </item>
+                <item row="5" column="0">
+                 <widget class="QDoubleSpinBox" name="doubleSpinBox_multiband_bestscore">
+                  <property name="maximum">
+                   <double>1.000000000000000</double>
+                  </property>
+                  <property name="value">
+                   <double>0.100000000000000</double>
+                  </property>
+                 </widget>
+                </item>
+                <item row="3" column="0">
+                 <widget class="QCheckBox" name="checkBox_multiband_fillholes">
+                  <property name="text">
+                   <string/>
+                  </property>
+                 </widget>
+                </item>
+                <item row="6" column="0">
+                 <widget class="QDoubleSpinBox" name="doubleSpinBox_multiband_angle">
+                  <property name="decimals">
+                   <number>1</number>
+                  </property>
+                  <property name="maximum">
+                   <double>180.000000000000000</double>
+                  </property>
+                  <property name="value">
+                   <double>90.000000000000000</double>
+                  </property>
+                 </widget>
+                </item>
+                <item row="3" column="1">
+                 <widget class="QLabel" name="label_51">
+                  <property name="text">
+                   <string>Fill Texture holes with plausible values.</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="4" column="0">
+                 <widget class="QSpinBox" name="spinBox_multiband_padding">
+                  <property name="maximum">
+                   <number>100</number>
+                  </property>
+                  <property name="value">
+                   <number>5</number>
+                  </property>
+                 </widget>
+                </item>
+                <item row="1" column="1">
+                 <widget class="QLabel" name="label_56">
+                  <property name="text">
+                   <string>Number of contributions per frequency band for the multi-band blending. Should be 4 values.</string>
+                  </property>
+                  <property name="wordWrap">
+                   <bool>true</bool>
+                  </property>
+                  <property name="textInteractionFlags">
+                   <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                  </property>
+                 </widget>
+                </item>
+                <item row="1" column="0">
+                 <widget class="QLineEdit" name="lineEdit_multiband_nbcontrib">
+                  <property name="text">
+                   <string>1 5 10 0</string>
+                  </property>
+                 </widget>
+                </item>
+               </layout>
+              </widget>
+             </item>
            </layout>
           </widget>
          </item>
@@ -63,9 +63,9 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>-496</y>
-            <width>686</width>
-            <height>3905</height>
+            <y>-466</y>
+            <width>675</width>
+            <height>3499</height>
           </rect>
          </property>
          <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                    <item>
                     <widget class="QStackedWidget" name="stackedWidget_src">
                      <property name="currentIndex">
-                       <number>3</number>
+                       <number>0</number>
                      </property>
                      <widget class="QWidget" name="page_41">
                       <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3346,28 +3346,35 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>OpenNI 2</string>
                                 </property>
                                 <layout class="QGridLayout" name="gridLayout_54" columnstretch="0,1,0">
-                                  <item row="7" column="1">
-                                   <widget class="QLabel" name="label_435">
+                                  <item row="1" column="0">
+                                   <widget class="QCheckBox" name="openni2_autoWhiteBalance">
                                    <property name="text">
-                                     <string>IR-Depth horizontal shift.</string>
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="1">
+                                   <widget class="QLabel" name="label_218">
+                                    <property name="text">
+                                     <string>Auto exposure.</string>
                                    </property>
                                    <property name="wordWrap">
                                     <bool>true</bool>
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="7" column="0">
-                                   <widget class="QSpinBox" name="openni2_hshift">
-                                    <property name="suffix">
-                                     <string> pix</string>
-                                    </property>
-                                    <property name="maximum">
-                                     <number>65535</number>
+                                  <item row="0" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_openni2OniPath">
+                                    <property name="text">
+                                     <string/>
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="1" column="0">
-                                   <widget class="QCheckBox" name="openni2_autoWhiteBalance">
+                                  <item row="2" column="0">
+                                   <widget class="QCheckBox" name="openni2_autoExposure">
                                    <property name="text">
                                     <string/>
                                    </property>
@@ -3393,50 +3400,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="5" column="0">
-                                   <widget class="QCheckBox" name="openni2_stampsIdsUsed">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                    <property name="checked">
-                                     <bool>false</bool>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="0" column="2">
-                                   <widget class="QLabel" name="label_231">
-                                    <property name="text">
-                                     <string>Path to a *.ONI file.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="4" column="1">
-                                   <widget class="QLabel" name="label_220">
-                                    <property name="text">
-                                     <string>Gain.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="0" column="1">
-                                   <widget class="QLineEdit" name="lineEdit_openni2OniPath">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="0" column="0">
-                                   <widget class="QToolButton" name="toolButton_openni2OniPath">
-                                    <property name="text">
-                                     <string>...</string>
-                                    </property>
-                                   </widget>
-                                  </item>
                                  <item row="1" column="1">
                                   <widget class="QLabel" name="label_217">
                                    <property name="text">
@@ -3447,6 +3410,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
+                                  <item row="8" column="1">
+                                   <widget class="QLabel" name="label_436">
+                                    <property name="text">
+                                     <string>IR-Depth vertical shift.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  <item row="6" column="1">
                                   <widget class="QLabel" name="label_223">
                                    <property name="text">
@@ -3467,46 +3440,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="4" column="0">
-                                   <widget class="QSpinBox" name="openni2_gain">
+                                  <item row="8" column="0">
+                                   <widget class="QSpinBox" name="openni2_vshift">
+                                    <property name="suffix">
+                                     <string> pix</string>
+                                    </property>
                                    <property name="maximum">
-                                     <number>1000</number>
-                                    </property>
-                                    <property name="value">
-                                     <number>100</number>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="9" column="0">
-                                   <spacer name="verticalSpacer_33">
-                                    <property name="orientation">
-                                     <enum>Qt::Vertical</enum>
-                                    </property>
-                                    <property name="sizeHint" stdset="0">
-                                     <size>
-                                      <width>20</width>
-                                      <height>0</height>
-                                     </size>
-                                    </property>
-                                   </spacer>
-                                  </item>
-                                  <item row="2" column="0">
-                                   <widget class="QCheckBox" name="openni2_autoExposure">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                    <property name="checked">
-                                     <bool>true</bool>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="2" column="1">
-                                   <widget class="QLabel" name="label_218">
-                                    <property name="text">
-                                     <string>Auto exposure.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
+                                     <number>65535</number>
                                    </property>
                                   </widget>
                                  </item>
@@ -3520,18 +3460,78 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="8" column="1">
-                                   <widget class="QLabel" name="label_436">
+                                  <item row="4" column="0">
+                                   <widget class="QSpinBox" name="openni2_gain">
+                                    <property name="maximum">
+                                     <number>1000</number>
+                                    </property>
+                                    <property name="value">
+                                     <number>100</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLabel" name="label_220">
                                    <property name="text">
-                                     <string>IR-Depth vertical shift.</string>
+                                     <string>Gain.</string>
                                    </property>
                                    <property name="wordWrap">
                                     <bool>true</bool>
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="8" column="0">
-                                   <widget class="QSpinBox" name="openni2_vshift">
+                                  <item row="0" column="2">
+                                   <widget class="QLabel" name="label_231">
+                                    <property name="text">
+                                     <string>Path to a *.ONI file.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="0" column="0">
+                                   <widget class="QToolButton" name="toolButton_openni2OniPath">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="0">
+                                   <widget class="QCheckBox" name="openni2_stampsIdsUsed">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="7" column="1">
+                                   <widget class="QLabel" name="label_435">
+                                    <property name="text">
+                                     <string>IR-Depth horizontal shift.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="10" column="0">
+                                   <spacer name="verticalSpacer_33">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                  <item row="7" column="0">
+                                   <widget class="QSpinBox" name="openni2_hshift">
                                    <property name="suffix">
                                     <string> pix</string>
                                    </property>
@@ -3540,6 +3540,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
+                                  <item row="9" column="1">
+                                   <widget class="QLabel" name="label_641">
+                                    <property name="text">
+                                     <string>Depth decimation.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="9" column="0">
+                                   <widget class="QSpinBox" name="openni2_depth_decimation">
+                                    <property name="suffix">
+                                     <string/>
+                                    </property>
+                                    <property name="minimum">
+                                     <number>1</number>
+                                    </property>
+                                    <property name="maximum">
+                                     <number>65535</number>
+                                    </property>
+                                   </widget>
+                                  </item>
                                 </layout>
                                </widget>
                               </item>
@@ -10951,7 +10974,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="9" column="1">
                        <widget class="QLabel" name="label_scanMatching_9">
                         <property name="text">
-                          <string>Re-extract visual features when computing loop closure transformations.</string>
+                          <string>Re-extract visual features when computing loop closure transformations. Raw features are not saved in database.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -13790,6 +13813,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <string>OpenVINS</string>
                         </property>
                        </item>
+                        <item>
+                         <property name="text">
+                          <string>FLOAM</string>
+                         </property>
+                        </item>
                       </widget>
                      </item>
                      <item row="2" column="1">
@@ -14093,7 +14121,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <item>
                     <widget class="QStackedWidget" name="stackedWidget_odometryType">
                      <property name="currentIndex">
-                       <number>10</number>
+                       <number>7</number>
                      </property>
                      <widget class="QWidget" name="page_52">
                       <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -16267,13 +16295,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item>
                         <widget class="QGroupBox" name="groupBox_odomLOAM">
                          <property name="title">
-                           <string>LOAM</string>
+                           <string>LOAM - FLOAM</string>
                          </property>
                          <layout class="QVBoxLayout" name="verticalLayout_129" stretch="0,1">
                           <item>
                            <widget class="QLabel" name="label_472">
                             <property name="text">
-                              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;LOAM: &lt;a href=&quot;https://github.com/laboshinl/loam_velodyne&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/laboshinl/loam_velodyne&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Velodyne input required. Currently tested only with KITTI data set and with pull request #66.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;LOAM: &lt;a href=&quot;https://github.com/laboshinl/loam_velodyne&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/laboshinl/loam_velodyne&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Velodyne input required. Currently tested only with KITTI data set.&lt;/p&gt;&lt;p&gt;FLOAM: &lt;a href=&quot;https://github.com/wh200720041/floam&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/wh200720041/floam&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Other remapped parameters for FLOAM:&lt;/p&gt;&lt;ul style=&quot;margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;&quot;&gt;&lt;li style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Icp/RangeMax (Max distance: set to 200 if 0)&lt;/li&gt;&lt;li style=&quot; margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Icp/RangeMin (Min distance)&lt;/li&gt;&lt;/ul&gt;&lt;/body&gt;&lt;/html&gt;</string>
                             </property>
                             <property name="wordWrap">
                              <bool>true</bool>
@@ -16288,7 +16316,78 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           </item>
                           <item>
                            <layout class="QGridLayout" name="gridLayout_101" columnstretch="0,1">
-                             <item row="2" column="1">
+                             <item row="4" column="1">
+                              <widget class="QLabel" name="label_476">
+                               <property name="text">
+                                <string>Angular output variance.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="3" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_loam_linvar">
+                               <property name="decimals">
+                                <number>4</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.500000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_loam_angvar">
+                               <property name="decimals">
+                                <number>4</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.500000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="1" column="1">
+                              <widget class="QLabel" name="label_474">
+                               <property name="text">
+                                <string>Scan period (s).</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="5" column="0">
+                              <widget class="QCheckBox" name="odom_loam_localMapping">
+                               <property name="text">
+                                <string/>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="3" column="1">
                              <widget class="QLabel" name="label_475">
                               <property name="text">
                                <string>Linear output variance.</string>
@@ -16314,19 +16413,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </property>
                              </widget>
                             </item>
-                             <item row="1" column="1">
-                              <widget class="QLabel" name="label_474">
-                               <property name="text">
-                                <string>Scan period (s).</string>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                               </property>
-                              </widget>
-                             </item>
                             <item row="1" column="0">
                              <widget class="QDoubleSpinBox" name="odom_loam_scan_period">
                               <property name="minimum">
@@ -16343,6 +16429,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </property>
                              </widget>
                             </item>
+                             <item row="5" column="1">
+                              <widget class="QLabel" name="label_477">
+                               <property name="text">
+                                <string>Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
                             <item row="0" column="0">
                              <widget class="QComboBox" name="odom_loam_sensor">
                               <property name="sizeAdjustPolicy">
@@ -16365,10 +16464,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </item>
                              </widget>
                             </item>
-                             <item row="3" column="1">
-                              <widget class="QLabel" name="label_476">
+                             <item row="2" column="1">
+                              <widget class="QLabel" name="label_566">
                               <property name="text">
-                                <string>Angular output variance.</string>
+                                <string>Map resolution.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -16379,60 +16478,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </widget>
                             </item>
                             <item row="2" column="0">
-                              <widget class="QDoubleSpinBox" name="odom_loam_linvar">
+                              <widget class="QDoubleSpinBox" name="odom_loam_resolution">
                               <property name="decimals">
-                                <number>4</number>
+                                <number>2</number>
                               </property>
                               <property name="minimum">
-                                <double>0.000100000000000</double>
+                                <double>0.010000000000000</double>
                               </property>
                               <property name="maximum">
-                                <double>1.000000000000000</double>
+                                <double>9.000000000000000</double>
                               </property>
                               <property name="singleStep">
                                <double>0.000100000000000</double>
                               </property>
                               <property name="value">
-                                <double>0.500000000000000</double>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="3" column="0">
-                              <widget class="QDoubleSpinBox" name="odom_loam_angvar">
-                               <property name="decimals">
-                                <number>4</number>
-                               </property>
-                               <property name="minimum">
-                                <double>0.000100000000000</double>
-                               </property>
-                               <property name="maximum">
-                                <double>1.000000000000000</double>
-                               </property>
-                               <property name="singleStep">
-                                <double>0.000100000000000</double>
-                               </property>
-                               <property name="value">
-                                <double>0.500000000000000</double>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="4" column="0">
-                              <widget class="QCheckBox" name="odom_loam_localMapping">
-                               <property name="text">
-                                <string/>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="4" column="1">
-                              <widget class="QLabel" name="label_477">
-                               <property name="text">
-                                <string>Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.</string>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                <double>0.200000000000000</double>
                               </property>
                              </widget>
                             </item>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
  <name>rtabmap</name>
-  <version>0.20.13</version>
+  <version>0.20.14</version>
  <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -13,6 +13,8 @@ ADD_SUBDIRECTORY( DetectMoreLoopClosures )
 ADD_SUBDIRECTORY( Export )
 ADD_SUBDIRECTORY( Report )
 ADD_SUBDIRECTORY( Info )
+ADD_SUBDIRECTORY( CleanupLocalGrids )
+ADD_SUBDIRECTORY( GlobalBundleAdjustment )

 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )
@@ -0,0 +1,36 @@
+
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(cleanupLocalGrids main.cpp)
+  
+TARGET_LINK_LIBRARIES(cleanupLocalGrids ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( cleanupLocalGrids 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-cleanupLocalGrids)
+
+INSTALL(TARGETS cleanupLocalGrids
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+
@@ -0,0 +1,138 @@
+/*
+Copyright (c) 2010-2021, 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/Rtabmap.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/util3d_transforms.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\n"
+			"Clear empty space from local occupancy grids and laser scans based on the saved optimized global 2d grid map.\n"
+			"Advantages:\n"
+			"   * If the map needs to be regenerated in the future (e.g., when \n"
+			"     we re-use the map in SLAM mode), removed obstacles won't reappear.\n"
+			"   * [--scan] The cropped laser scans will be also used for localization,\n"
+			"     so if dynamic obstacles have been removed, localization won't try to\n"
+			"     match them anymore.\n\n"
+			"Disadvantage:\n"
+			"   * [--scan] Cropping the laser scans cannot be reverted, but grids can.\n"
+			"\nUsage:\n"
+			"rtabmap-cleanupLocalGrids [options] database.db\n"
+			"Options:\n"
+			"    --radius #   Radius in cells around empty cell without obstacles to clear\n"
+			"                  underlying obstacles. Default is 1.\n"
+			"    --scan       Filter also scans, otherwise only local grids are filtered.\n"
+			"\n");
+	;
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	int cropRadius = 1;
+	bool filterScans = false;
+
+	for(int i=1; i<argc; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+		else if(std::strcmp(argv[i], "--scan") == 0)
+		{
+			filterScans = true;
+		}
+		else if(std::strcmp(argv[i], "--radius") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				cropRadius = uStr2Int(argv[i]);
+				UASSERT(cropRadius>=0);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+	}
+
+	std::string dbPath = argv[argc-1];
+
+	if(!UFile::exists(dbPath))
+	{
+		UERROR("File \"%s\" doesn't exist!", dbPath.c_str());
+		return -1;
+	}
+
+	// Get parameters
+	ParametersMap parameters;
+	Rtabmap rtabmap;
+	rtabmap.init(ParametersMap(), dbPath, true);
+
+	float xMin, yMin, cellSize;
+	cv::Mat map = rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize);
+	if(map.empty())
+	{
+		UERROR("Database %s doesn't have optimized 2d map saved in it!", dbPath.c_str());
+		return -1;
+	}
+
+	printf("Options:\n");
+	printf("  --radius:  %d cell(s) (cell size=%.3fm)\n", cropRadius, cellSize);
+	printf("  --scan: %s\n", filterScans?"true":"false");
+
+	std::map<int, Transform> poses = rtabmap.getLocalOptimizedPoses();
+	if(poses.empty() || poses.lower_bound(1) == poses.end())
+	{
+		UERROR("Database %s doesn't have optimized poses saved in it!", dbPath.c_str());
+		return -1;
+	}
+
+	UTimer timer;
+	printf("Cleaning grids...\n");
+	int modifiedCells = rtabmap.cleanupLocalGrids(poses, map, xMin, yMin, cellSize, cropRadius, filterScans);
+	printf("Cleanup %d cells! (%fs)\n", modifiedCells, timer.ticks());
+
+	rtabmap.close();
+
+	printf("Done!\n");
+	return 0;
+}
@@ -62,12 +62,13 @@ void showUsage()
 			"    --las                 Export cloud in LAS instead of PLY (PDAL dependency required).\n"
 			"    --mesh                Create a mesh.\n"
 			"    --texture             Create a mesh with texture.\n"
-			"    --texture_size  #     Texture size (default 4096).\n"
-			"    --texture_count #     Maximum textures generated (default 1).\n"
+			"    --texture_size  #     Texture size 1024, 2048, 4096, 8192, 16384 (default 8192).\n"
+			"    --texture_count #     Maximum textures generated (default 1). Ignored by --multiband option (adjust --multiband_contrib instead).\n"
 			"    --texture_range #     Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
 			"    --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used, default=0).\n"
 			"    --texture_d2c         Distance to camera policy.\n"
 			"    --cam_projection      Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
+			"    --cam_projection_keep_all  Keep not colored points from cameras (node ID will be 0 and color will be red).\n"
 			"    --poses               Export optimized poses of the robot frame (e.g., base_link).\n"
 			"    --poses_camera        Export optimized poses of the camera frame (e.g., optical frame).\n"
 			"    --poses_scan          Export optimized poses of the scan frame.\n"
@@ -88,15 +89,23 @@ void showUsage()
 			"    --no_clean            Disable cleaning colorless polygons.\n"
 			"    --low_gain      #     Low brightness gain 0-100 (default 0).\n"
 			"    --high_gain     #     High brightness gain 0-100 (default 10).\n"
-			"    --multiband           Enable multiband texturing (AliceVision dependency required).\n"
+			"    --multiband               Enable multiband texturing (AliceVision dependency required).\n"
+			"    --multiband_downscale #   Downscaling reduce the texture quality but speed up the computation time (default 2).\n"
+			"    --multiband_contrib \"# # # # \"  Number of contributions per frequency band for the multi-band blending, should be 4 values! (default \"1 5 10 0\").\n"
+			"    --multiband_unwrap #      Method to unwrap input mesh: 0=basic (default, >600k faces, fast), 1=ABF (<=300k faces, generate 1 atlas), 2=LSCM (<=600k faces, optimize space).\n"
+			"    --multiband_fillholes     Fill Texture holes with plausible values.\n"
+			"    --multiband_padding #     Texture edge padding size in pixel (0-100) (default 5).\n"
+			"    --multiband_scorethr #    0 to disable filtering based on threshold to relative best score (0.0-1.0). (default 0.1).\n"
+			"    --multiband_anglethr #    0 to disable angle hard threshold filtering (0.0, 180.0) (default 90.0).\n"
+			"    --multiband_forcevisible  Triangle visibility is based on the union of vertices visibility.\n"
 			"    --poisson_depth #     Set Poisson depth for mesh reconstruction.\n"
 			"    --poisson_size  #     Set target polygon size when computing Poisson's depth for mesh reconstruction (default 0.03 m).\n"
-			"    --max_polygons  #     Maximum polygons when creating a mesh (default 500000, set 0 for no limit).\n"
+			"    --max_polygons  #     Maximum polygons when creating a mesh (default 300000, set 0 for no limit).\n"
 			"    --max_range     #     Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
 			"    --decimation    #     Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
 			"    --voxel         #     Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
 			"    --noise_radius  #     Noise filtering search radius (default 0, 0=disabled).\n"
-			"    --noise_k       #     Noise filtering minimum neighbors in search radius (default 5, 0=disabled)."
+			"    --noise_k       #     Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
 			"    --scan                Use laser scan for the point cloud.\n"
 			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
@@ -132,24 +141,33 @@ int main(int argc, char * argv[])
 	bool doClean = true;
 	int poissonDepth = 0;
 	float poissonSize = 0.03;
-	int maxPolygons = 500000;
+	int maxPolygons = 300000;
 	int decimation = -1;
 	float maxRange = -1.0f;
 	float voxelSize = -1.0f;
 	float noiseRadius = 0.0f;
 	int noiseMinNeighbors = 5;
-	int textureSize = 4096;
+	int textureSize = 8192;
 	int textureCount = 1;
 	int textureRange = 0;
 	float textureDepthError = 0;
 	bool distanceToCamPolicy = false;
 	bool multiband = false;
+	int multibandDownScale = 2;
+	std::string multibandNbContrib = "1 5 10 0";
+	int multibandUnwrap = 0;
+	bool multibandFillHoles = false;
+	int multibandPadding = 5;
+	double multibandBestScoreThr = 0.1;
+	double multibandAngleHardthr = 90;
+	bool multibandForceVisible = false;
 	float colorRadius = -1.0f;
 	bool cloudFromScan = false;
 	bool saveInDb = false;
 	int lowBrightnessGain = 0;
 	int highBrightnessGain = 10;
 	bool camProjection = false;
+	bool camProjectionKeepAll = false;
 	bool exportPoses = false;
 	bool exportPosesCamera = false;
 	bool exportPosesScan = false;
@@ -267,6 +285,10 @@ int main(int argc, char * argv[])
 		{
 			camProjection = true;
 		}
+		else if(std::strcmp(argv[i], "--cam_projection_keep_all") == 0)
+		{
+			camProjectionKeepAll = true;
+		}
 		else if(std::strcmp(argv[i], "--poses") == 0)
 		{
 			exportPoses = true;
@@ -314,7 +336,7 @@ int main(int argc, char * argv[])
 			if(i<argc-1)
 			{
 				gainValue = uStr2Float(argv[i]);
-				UASSERT(gainValue>0.0f);
+				UASSERT(gainValue>=0.0f);
 			}
 			else
 			{
@@ -337,6 +359,91 @@ int main(int argc, char * argv[])
 			printf("\"--multiband\" option cannot be used because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n");
 #endif
 		}
+		else if(std::strcmp(argv[i], "--multiband_fillholes") == 0)
+		{
+			multibandFillHoles = true;
+		}
+		else if(std::strcmp(argv[i], "--multiband_downscale") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				multibandDownScale = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--multiband_contrib") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				if(uSplit(argv[i], ' ').size() != 4)
+				{
+					printf("--multiband_contrib has wrong format! value=\"%s\"\n", argv[i]);
+					showUsage();
+				}
+				multibandNbContrib = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--multiband_unwrap") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				multibandUnwrap = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--multiband_padding") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				multibandPadding = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--multiband_forcevisible") == 0)
+		{
+			multibandForceVisible = true;
+		}
+		else if(std::strcmp(argv[i], "--multiband_scorethr") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				multibandBestScoreThr = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--multiband_anglethr") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				multibandAngleHardthr = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--poisson_depth") == 0)
 		{
 			++i;
@@ -683,17 +790,7 @@ int main(int argc, char * argv[])
 	{
 		printf("Global bundle adjustment...\n");
 		OptimizerG2O g2o(parameters);
-		std::map<int, cv::Point3f> points3DMap;
-		std::map<int, std::map<int, FeatureBA> > wordReferences;
-		g2o.computeBACorrespondences(optimizedPoses, links, nodes, points3DMap, wordReferences, true);
-		std::map<int, rtabmap::CameraModel> cameraSingleModels;
-		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
-		{
-			Signature node = nodes.find(iter->first)->second;
-			UASSERT(node.sensorData().cameraModels().size()==1);
-			cameraSingleModels.insert(std::make_pair(iter->first, node.sensorData().cameraModels().front()));
-		}
-		optimizedPoses = g2o.optimizeBA(optimizedPoses.begin()->first, optimizedPoses, links, cameraSingleModels, points3DMap, wordReferences);
+		optimizedPoses = ((Optimizer*)&g2o)->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
 		printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
 	}

@@ -969,6 +1066,7 @@ int main(int argc, char * argv[])
 		}

 		std::vector<int> pointToCamId;
+		std::vector<float> pointToCamIntensity;
 		if(camProjection && !robotPoses.empty())
 		{
 			printf("Camera projection...\n");
@@ -995,6 +1093,7 @@ int main(int argc, char * argv[])
 						0,
 						std::vector<float>(),
 						distanceToCamPolicy);
+				pointToCamIntensity.resize(pointToPixel.size());
 			}

 			// color the cloud
@@ -1007,6 +1106,7 @@ int main(int argc, char * argv[])
 			for(size_t i=0; i<pointToPixel.size(); ++i)
 			{
 				pcl::PointXYZRGBNormal pt;
+				float intensity = 0;
 				if(!cloudToExport->empty())
 				{
 					pt = cloudToExport->at(i);
@@ -1019,6 +1119,7 @@ int main(int argc, char * argv[])
 					pt.normal_x = cloudIToExport->at(i).normal_x;
 					pt.normal_y = cloudIToExport->at(i).normal_y;
 					pt.normal_z = cloudIToExport->at(i).normal_z;
+					intensity = cloudIToExport->at(i).intensity;
 				}
 				int nodeID = pointToPixel[i].first.first;
 				int cameraIndex = pointToPixel[i].first.second;
@@ -1062,14 +1163,34 @@ int main(int argc, char * argv[])

 					int exportedId = nodeID;
 					pointToCamId[oi] = exportedId;
+					if(!pointToCamIntensity.empty())
+					{
+						pointToCamIntensity[oi] = intensity;
+					}
 					assembledCloudValidPoints->at(oi++) = pt;
 				}
+				else if(camProjectionKeepAll)
+				{
+					pointToCamId[oi] = 0; // invalid
+					pt.b = 0;
+					pt.g = 0;
+					pt.r = 255;
+					if(!pointToCamIntensity.empty())
+					{
+						pointToCamIntensity[oi] = intensity;
+					}
+					assembledCloudValidPoints->at(oi++) = pt; // red
+				}
 			}

 			assembledCloudValidPoints->resize(oi);
 			cloudToExport = assembledCloudValidPoints;
 			cloudIToExport->clear();
 			pointToCamId.resize(oi);
+			if(!pointToCamIntensity.empty())
+			{
+				pointToCamIntensity.resize(oi);
+			}

 			printf("Camera projection... done! (%fs)\n", timer.ticks());
 		}
@@ -1091,10 +1212,19 @@ int main(int argc, char * argv[])
 				std::string outputPath=outputDirectory+"/"+baseName+"_cloud."+ext;
 				printf("Saving %s... (%d points)\n", outputPath.c_str(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
 #ifdef RTABMAP_PDAL
-				if(las || !pointToCamId.empty())
+				if(las || !pointToCamId.empty() || !pointToCamIntensity.empty())
 				{
 					if(!cloudToExport->empty())
-						savePDALFile(outputPath, *cloudToExport, pointToCamId, binary);
+					{
+						if(!pointToCamIntensity.empty())
+						{
+							savePDALFile(outputPath, *cloudToExport, pointToCamId, binary, pointToCamIntensity);
+						}
+						else
+						{
+							savePDALFile(outputPath, *cloudToExport, pointToCamId, binary);
+						}
+					}
 					else if(!cloudIToExport->empty())
 						savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary);
 				}
@@ -1103,8 +1233,16 @@ int main(int argc, char * argv[])
 				{
 					if(!pointToCamId.empty())
 					{
-						printf("Option --cam_projection is enabled but rtabmap is not built "
-								"with PDAL support, so camera IDs won't be exported in the output cloud.\n");
+						if(!pointToCamIntensity.empty())
+						{
+							printf("Option --cam_projection is enabled but rtabmap is not built "
+									"with PDAL support, so camera IDs and lidar intensities won't be exported in the output cloud.\n");
+						}
+						else
+						{
+							printf("Option --cam_projection is enabled but rtabmap is not built "
+									"with PDAL support, so camera IDs won't be exported in the output cloud.\n");
+						}
 					}
 					if(!cloudToExport->empty())
 						pcl::io::savePLYFile(outputPath, *cloudToExport, binary);
@@ -1152,7 +1290,10 @@ int main(int argc, char * argv[])

 			if(mesh->polygons.size())
 			{
-				printf("Mesh color transfer...\n");
+				printf("Mesh color transfer (max polygons=%d, color radius=%f, clean=%s)...\n",
+						maxPolygons,
+						colorRadius,
+						doClean?"true":"false");
 				rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
 						mesh,
 						0.0f,
@@ -1299,7 +1440,16 @@ int main(int argc, char * argv[])
 						{
 							timer.restart();
 							std::string outputPath=outputDirectory+"/"+baseName+"_mesh_multiband.obj";
-							printf("MultiBand texturing... \"%s\"\n", outputPath.c_str());
+							printf("MultiBand texturing (size=%d, downscale=%d, unwrap method=%s, fill holes=%s, padding=%d, best score thr=%f, angle thr=%f, force visible=%s)... \"%s\"\n",
+									textureSize,
+									multibandDownScale,
+									multibandUnwrap==1?"ABF":multibandUnwrap==2?"LSCM":"Basic",
+									multibandFillHoles?"true":"false",
+									multibandPadding,
+									multibandBestScoreThr,
+									multibandAngleHardthr,
+									multibandForceVisible?"false":"true",
+									outputPath.c_str());
 							if(util3d::multiBandTexturing(outputPath,
 									textureMesh->cloud,
 									textureMesh->tex_polygons[0],
@@ -1310,11 +1460,19 @@ int main(int argc, char * argv[])
 									rtabmap.getMemory(),
 									0,
 									textureSize,
+									multibandDownScale,
+									multibandNbContrib,
 									"jpg",
 									gains,
 									blendingGains,
 									contrastValues,
-									doGainCompensationRGB))
+									doGainCompensationRGB,
+									multibandUnwrap,
+									multibandFillHoles,
+									multibandPadding,
+									multibandBestScoreThr,
+									multibandAngleHardthr,
+									multibandForceVisible))
 							{
 								printf("MultiBand texturing...done (%fs).\n", timer.ticks());
 							}
@@ -0,0 +1,40 @@
+
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
+
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 NEW)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(globalBundleAdjustment main.cpp)
+  
+TARGET_LINK_LIBRARIES(globalBundleAdjustment ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( globalBundleAdjustment 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-globalBundleAdjustment)
+
+INSTALL(TARGETS globalBundleAdjustment
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+
@@ -0,0 +1,138 @@
+/*
+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/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-globalBundleAdjustment database.db\n"
+			"\n%s", Parameters::showUsage());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+	}
+	ParametersMap inputParams = Parameters::parseArguments(argc,  argv);
+
+	std::string dbPath = argv[argc-1];
+	if(!UFile::exists(dbPath))
+	{
+		printf("Database %s doesn't exist!\n", dbPath.c_str());
+	}
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		if(uStrNumCmp(driver->getDatabaseVersion(), "0.17.0")<0)
+		{
+			printf("Database is too old (%s), we cannot save back optimized poses. "
+					"Consider upgrading the database with:\n"
+					"rtabmap-reprocess --Db/TargetVersion \"\" \"%s\" \"output.db\"\n",
+					driver->getDatabaseVersion().c_str(),
+					dbPath.c_str());
+			driver->closeConnection(false);
+			delete driver;
+			return -1;
+		}
+
+		parameters = driver->getLastParameters();
+		// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+		driver->save2DMap(cv::Mat(), 0, 0, 0);
+		driver->saveOptimizedMesh(cv::Mat());
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	for(ParametersMap::iterator iter=inputParams.begin(); iter!=inputParams.end(); ++iter)
+	{
+		printf("Added custom parameter %s=%s\n",iter->first.c_str(), iter->second.c_str());
+	}
+
+	UTimer timer;
+
+	printf("Loading database \"%s\"...\n", dbPath.c_str());
+	// Get the global optimized map
+	Rtabmap rtabmap;
+	uInsert(parameters, inputParams);
+	rtabmap.init(parameters, dbPath);
+	printf("Loading database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks());
+
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	printf("Optimizing the map...\n");
+	rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
+	printf("Optimizing the map... done (%fs, poses=%d).\n", timer.ticks(), (int)optimizedPoses.size());
+
+	printf("Global bundle adjustment...\n");
+	Optimizer * optimizer = Optimizer::create(Optimizer::kTypeG2O, parameters);
+	optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
+	delete optimizer;
+	printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
+
+	if(!optimizedPoses.empty())
+	{
+		rtabmap.setOptimizedPoses(optimizedPoses, links);
+	}
+	else
+	{
+		UERROR("Returned empty poses!");
+	}
+
+	rtabmap.close();
+
+	return 0;
+}
@@ -141,7 +141,7 @@ int main(int argc, char * argv[])
 	HANDLE H = GetStdHandle(STD_OUTPUT_HANDLE);
 #endif
 	int padding = 35;
-	std::cout << ("Parameters (Yellow=modified, Red=old parameter not used anymore):\n");
+	std::cout << ("Parameters (Yellow=modified, Red=old parameter not used anymore, NA=not in database):\n");
 	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
 		ParametersMap::const_iterator jter = defaultParameters.find(iter->first);
@@ -197,7 +197,7 @@ int main(int argc, char * argv[])
 				std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
 			}
 		}
-		else if(!defaultValueSet && otherDatabasePath.empty())
+		else if(!defaultValueSet)
 		{
 			//red
 #ifdef _WIN32
@@ -205,7 +205,7 @@ int main(int argc, char * argv[])
 #else
 			printf("%s", COLOR_RED);
 #endif
-			std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+			std::cout << (uFormat("%s%s  (%s=NA)\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str()));
 		}
 		else if(!diff)
 		{
@@ -224,6 +224,27 @@ int main(int argc, char * argv[])
 #endif
 	}

+	for(ParametersMap::iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
+	{
+		ParametersMap::const_iterator jter = parameters.find(iter->first);
+		if(jter == parameters.end())
+		{
+			//red
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_RED);
+#else
+			printf("%s", COLOR_RED);
+#endif
+			std::cout << (uFormat("%sNA  (%s=\"%s\")\n", pad(iter->first + "=", padding).c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), iter->second.c_str()));
+		
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+			printf("%s", COLOR_NORMAL);
+#endif
+		}
+	}
+
 	if(otherDatabasePath.empty())
 	{
 		printf("\nInfo:\n\n");
@@ -68,10 +68,11 @@ void showUsage()
 			"                       arguments, they overwrite those in config file and the database.\n"
 			"     -start #    Start from this node ID.\n"
 			"     -stop #     Last node to process.\n"
-			"     -g2         Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\".\n"
+			"     -g2         Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\". Use with -db to save in database.\n"
 			"     -g3         Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
-			"     -o2         Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n"
+			"     -o2         Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\". Use with -db to save in database.\n"
 			"     -o3         Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
+			"     -db         Save assembled 2D occupancy grid in database instead of a file.\n"
 			"     -p          Save odometry and localization poses (*.g2o).\n"
 			"     -scan_from_depth    Generate scans from depth images (overwrite previous\n"
 			"                         scans if they exist).\n"
@@ -211,6 +212,7 @@ int main(int argc, char * argv[])
 		showUsage();
 	}

+	bool save2DMap = false;
 	bool assemble2dMap = false;
 	bool assemble3dMap = false;
 	bool assemble2dOctoMap = false;
@@ -300,6 +302,11 @@ int main(int argc, char * argv[])
 			exportPoses = true;
 			printf("Odometry trajectory and localization poses will be exported in g2o format (-p option).\n");
 		}
+		else if(strcmp(argv[i], "-db") == 0 || strcmp(argv[i], "--db") == 0)
+		{
+			save2DMap = true;
+			printf("2D occupancy grid will be saved in database (-db option).\n");
+		}
 		else if(strcmp(argv[i], "-g2") == 0 || strcmp(argv[i], "--g2") == 0)
 		{
 			assemble2dMap = true;
@@ -856,36 +863,50 @@ int main(int argc, char * argv[])
 		cv::Mat map = grid.getMap(xMin, yMin);
 		if(!map.empty())
 		{
-			cv::Mat map8U(map.rows, map.cols, CV_8U);
-			//convert to gray scaled map
-			for (int i = 0; i < map.rows; ++i)
+			if(save2DMap)
 			{
-				for (int j = 0; j < map.cols; ++j)
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(outputDatabasePath))
 				{
-					char v = map.at<char>(i, j);
-					unsigned char gray;
-					if(v == 0)
-					{
-						gray = 178;
-					}
-					else if(v == 100)
-					{
-						gray = 0;
-					}
-					else // -1
-					{
-						gray = 89;
-					}
-					map8U.at<unsigned char>(i, j) = gray;
+					driver->save2DMap(map, xMin, yMin, grid.getCellSize());
+					printf("Saving occupancy grid to database... done!\n");
 				}
-			}
-			if(cv::imwrite(outputPath, map8U))
-			{
-				printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
+				delete driver;
 			}
 			else
 			{
-				printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
+				cv::Mat map8U(map.rows, map.cols, CV_8U);
+				//convert to gray scaled map
+				for (int i = 0; i < map.rows; ++i)
+				{
+					for (int j = 0; j < map.cols; ++j)
+					{
+						char v = map.at<char>(i, j);
+						unsigned char gray;
+						if(v == 0)
+						{
+							gray = 178;
+						}
+						else if(v == 100)
+						{
+							gray = 0;
+						}
+						else // -1
+						{
+							gray = 89;
+						}
+						map8U.at<unsigned char>(i, j) = gray;
+					}
+				}
+
+				if(cv::imwrite(outputPath, map8U))
+				{
+					printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
+				}
+				else
+				{
+					printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
+				}
 			}
 		}
 		else
@@ -937,36 +958,49 @@ int main(int argc, char * argv[])
 		cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
 		if(!map.empty())
 		{
-			cv::Mat map8U(map.rows, map.cols, CV_8U);
-			//convert to gray scaled map
-			for (int i = 0; i < map.rows; ++i)
+			if(save2DMap)
 			{
-				for (int j = 0; j < map.cols; ++j)
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(outputDatabasePath))
 				{
-					char v = map.at<char>(i, j);
-					unsigned char gray;
-					if(v == 0)
-					{
-						gray = 178;
-					}
-					else if(v == 100)
-					{
-						gray = 0;
-					}
-					else // -1
-					{
-						gray = 89;
-					}
-					map8U.at<unsigned char>(i, j) = gray;
+					driver->save2DMap(map, xMin, yMin, cellSize);
+					printf("Saving occupancy grid to database... done!\n");
 				}
-			}
-			if(cv::imwrite(outputPath, map8U))
-			{
-				printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
+				delete driver;
 			}
 			else
 			{
-				printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
+				cv::Mat map8U(map.rows, map.cols, CV_8U);
+				//convert to gray scaled map
+				for (int i = 0; i < map.rows; ++i)
+				{
+					for (int j = 0; j < map.cols; ++j)
+					{
+						char v = map.at<char>(i, j);
+						unsigned char gray;
+						if(v == 0)
+						{
+							gray = 178;
+						}
+						else if(v == 100)
+						{
+							gray = 0;
+						}
+						else // -1
+						{
+							gray = 89;
+						}
+						map8U.at<unsigned char>(i, j) = gray;
+					}
+				}
+				if(cv::imwrite(outputPath, map8U))
+				{
+					printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
+				}
+				else
+				{
+					printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
+				}
 			}
 		}
 		else
Author	SHA1	Message	Date
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