CameraRealSense2: Odom extrinsics against another sensor can be calibrated with having to calibrate stereo first. DBViewer: fixed local grid wrongly using global grid parameters.

Setting WITH_FLOAM to OFF by default because floam binaries (this [version](https://github.com/flynneva/floam)) in ros is not compatible.

.github/workflows/cmake.yml

@@ -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}}
+      

.github/workflows/docker.yml

@@ -0,0 +1,90 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      matrix:
+        docker_tag: [xenial, bionic, focal, android23, android24, android26]
+        include:
+        - docker_tag: xenial
+          docker_tags: |
+            introlab3it/rtabmap:xenial
+            introlab3it/rtabmap:16.04
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'xenial'
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'bionic'
+        - docker_tag: focal
+          docker_tags: |
+            introlab3it/rtabmap:focal
+            introlab3it/rtabmap:20.04
+            introlab3it/rtabmap:latest
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api23'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api24'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api26'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v1
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v2
+        with:
+          context: ./docker/${{ matrix.docker_path }}
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          build-args: |
+            CACHE_DATE=${{ github.head_ref }}.${{ github.sha }}
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+

.travis.yml

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

CMakeLists.txt

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

README.md

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

RTABMapConfig.cmake.in

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

Version.h.in

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

corelib/include/rtabmap/core/Memory.h

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

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -58,7 +58,7 @@ public:
 	float getCellSize() const {return cellSize_;}
 	void setCloudAssembling(bool enabled);
 	float getMinMapSize() const {return minMapSize_;}
-	bool isGridFromDepth() const {return occupancyFromDepth_;}
+	bool isGridFromDepth() const {return occupancySensor_;}
 	bool isFullUpdate() const {return fullUpdate_;}
 	float getUpdateError() const {return updateError_;}
 	bool isMapFrameProjection() const {return projMapFrame_;}
@@ -81,7 +81,7 @@ public:
 			cv::Mat & groundCells,
 			cv::Mat & obstacleCells,
 			cv::Mat & emptyCells,
-			cv::Point3f & viewPoint) const;
+			cv::Point3f & viewPoint);
 
 	void createLocalMap(
 			const LaserScan & cloud,
@@ -118,7 +118,7 @@ private:
 	int scanDecimation_;
 	float cellSize_;
 	bool preVoxelFiltering_;
-	bool occupancyFromDepth_;
+	int occupancySensor_;
 	bool projMapFrame_;
 	float maxObstacleHeight_;
 	int normalKSearch_;

corelib/include/rtabmap/core/Odometry.h

@@ -54,7 +54,8 @@ public:
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
 		kTypeVINS = 9,
-		kTypeOpenVINS = 10
+		kTypeOpenVINS = 10,
+		kTypeFLOAM = 11
 	};
 
 public:

corelib/include/rtabmap/core/PDALWriter.h

@@ -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::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);
+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);
 

corelib/include/rtabmap/core/Parameters.h

@@ -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.");
@@ -722,15 +723,15 @@ class RTABMAP_EXP Parameters
 #endif
 
     // Occupancy Grid
-    RTABMAP_PARAM(Grid, FromDepth,               bool,   true,    "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
+    RTABMAP_PARAM(Grid, Sensor,                  int,    1,       "Create occupancy grid from selected sensor: 0=laser scan, 1=depth image(s) or 2=both laser scan and depth image(s).");
     RTABMAP_PARAM(Grid, DepthDecimation,         unsigned int,  4, uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
     RTABMAP_PARAM(Grid, RangeMin,                float,  0.0,     "Minimum range from sensor.");
     RTABMAP_PARAM(Grid, RangeMax,                float,  5.0,     "Maximum range from sensor. 0=inf.");
-    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));
+    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s>=1] Region of interest ratios [left, right, top, bottom].", kGridSensor().c_str()));
     RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
     RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
     RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
-    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=0 or 2] Decimation of the laser scan before creating cloud.", kGridSensor().c_str()));
     RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
     RTABMAP_PARAM(Grid, PreVoxelFiltering,       bool,   true,    uFormat("Input cloud is downsampled by voxel filter (voxel size is \"%s\") before doing segmentation of obstacles and ground.", kGridCellSize().c_str()));
     RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
@@ -744,9 +745,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
 #ifdef RTABMAP_OCTOMAP
-    RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #else
-    RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #endif
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
@@ -829,7 +830,11 @@ public:
     static bool isFeatureParameter(const std::string & param);
     static ParametersMap getDefaultOdometryParameters(bool stereo = false, bool vis = true, bool icp = false);
     static ParametersMap getDefaultParameters(const std::string & group);
-    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);
+    /**
+     * If remove=false: keep only parameters of the specified group.
+     * If remove=true: remove parameters of the specified group.
+     */
+    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);
 
     static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
     static void writeINI(const std::string & configFile, const ParametersMap & parameters);

corelib/include/rtabmap/core/Rtabmap.h

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

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

@@ -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
 };
 

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

@@ -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
 };
 

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

@@ -89,7 +89,7 @@ public:
 	void setJsonConfig(const std::string & json);
 	// T265 related parameters
 	void setImagesRectified(bool enabled);
-	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false);
+	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false, bool onlyLeftStream = false);
 
 #ifdef RTABMAP_REALSENSE2
 private:
@@ -116,8 +116,6 @@ private:
 	std::string deviceId_;
 	rs2::syncer syncer_;
 	float depth_scale_meters_;
-	rs2_intrinsics depthIntrinsics_;
-	rs2_intrinsics rgbIntrinsics_;
 	cv::Mat depthBuffer_;
 	cv::Mat rgbBuffer_;
 	CameraModel model_;
@@ -138,6 +136,7 @@ private:
 	bool rectifyImages_;
 	bool odometryProvided_;
 	bool odometryImagesDisabled_;
+	bool odometryOnlyLeftStream_;
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;

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

@@ -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_ */

corelib/include/rtabmap/core/util3d_surface.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,

corelib/src/CMakeLists.txt

@@ -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::core
-		depthai::depthai-opencv
+		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}

corelib/src/CameraModel.cpp

@@ -350,7 +350,7 @@ bool CameraModel::load(const std::string & filePath)
 		}
 		catch(const cv::Exception & e)
 		{
-			UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
+			UERROR("Error reading calibration file \"%s\": %s (Make sure the first line of the yaml file is \"%YAML:1.0\")", filePath.c_str(), e.what());
 		}
 	}
 	else

corelib/src/CameraThread.cpp

@@ -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 &&
+		if(_distortionModel->getWidth() >= data.depthRaw().cols &&
-		   _distortionModel->getHeight() == data.depthRaw().rows	)
+		   _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);
 		}

corelib/src/DBDriverSqlite3.cpp

@@ -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";
 		}
 

corelib/src/Memory.cpp

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

corelib/src/OccupancyGrid.cpp

@@ -52,7 +52,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	scanDecimation_(Parameters::defaultGridScanDecimation()),
 	cellSize_(Parameters::defaultGridCellSize()),
 	preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
-	occupancyFromDepth_(Parameters::defaultGridFromDepth()),
+	occupancySensor_(Parameters::defaultGridSensor()),
 	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
 	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
 	normalKSearch_(Parameters::defaultGridNormalK()),
@@ -91,7 +91,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 
 void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromDepth_);
+	Parameters::parse(parameters, Parameters::kGridSensor(), occupancySensor_);
 	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
 	if(cloudDecimation_ == 0)
 	{
@@ -284,12 +284,12 @@ void OccupancyGrid::createLocalMap(
 		cv::Mat & groundCells,
 		cv::Mat & obstacleCells,
 		cv::Mat & emptyCells,
-		cv::Point3f & viewPoint) const
+		cv::Point3f & viewPoint)
 {
-	UDEBUG("scan format=%s, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
+	UDEBUG("scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d",
-			node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
+			node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancySensor_, normalsSegmentation_?1:0, grid3D_?1:0);
 
-	if((node.sensorData().laserScanRaw().is2d()) && !occupancyFromDepth_)
+	if((node.sensorData().laserScanRaw().is2d()) && occupancySensor_ == 0)
 	{
 		UDEBUG("2D laser scan");
 		//2D
@@ -328,7 +328,7 @@ void OccupancyGrid::createLocalMap(
 	else
 	{
 		// 3D
-		if(!occupancyFromDepth_)
+		if(occupancySensor_ == 0 || occupancySensor_ == 2)
 		{
 			if(!node.sensorData().laserScanRaw().isEmpty())
 			{
@@ -350,14 +350,35 @@ void OccupancyGrid::createLocalMap(
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 
 				UDEBUG("scan format=%d", scan.format());
+
+				bool normalSegmentationTmp = normalsSegmentation_;
+				float minGroundHeightTmp = minGroundHeight_;
+				float maxGroundHeightTmp = maxGroundHeight_;
+				if(scan.is2d())
+				{
+					// if 2D, assume the whole scan is obstacle
+					normalsSegmentation_ = false;
+					minGroundHeight_ = std::numeric_limits<int>::min();
+					maxGroundHeight_ = std::numeric_limits<int>::min()+100;
+				}
+
 				createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+
+				if(scan.is2d())
+				{
+					// restore
+					normalsSegmentation_ = normalSegmentationTmp;
+					minGroundHeight_ = minGroundHeightTmp;
+					maxGroundHeight_ = maxGroundHeightTmp;
+				}
 			}
 			else
 			{
-				UWARN("Cannot create local map, scan is empty (node=%d, %s=false).", node.id(), Parameters::kGridFromDepth().c_str());
+				UWARN("Cannot create local map, scan is empty (node=%d, %s=0).", node.id(), Parameters::kGridSensor().c_str());
 			}
 		}
-		else
+
+		if(occupancySensor_ >= 1)
 		{
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
@@ -407,7 +428,49 @@ void OccupancyGrid::createLocalMap(
 				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 			}
+
+			cv::Mat scanGroundCells;
+			cv::Mat scanObstacleCells;
+			cv::Mat scanEmptyCells;
+			if(occupancySensor_ == 2)
+			{
+				// backup
+				scanGroundCells = groundCells.clone();
+				scanObstacleCells = obstacleCells.clone();
+				scanEmptyCells = emptyCells.clone();
+			}
+
 			createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+
+			if(occupancySensor_ == 2)
+			{
+				if(grid3D_)
+				{
+					// We should convert scans to 4 channels (XYZRGB) to be compatible
+					scanGroundCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanGroundCells), Transform::getIdentity(), 255, 255, 255)).data();
+					scanObstacleCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanObstacleCells), Transform::getIdentity(), 255, 255, 255)).data();
+					scanEmptyCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanEmptyCells), Transform::getIdentity(), 255, 255, 255)).data();
+				}
+
+				UDEBUG("groundCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", groundCells.cols, groundCells.channels(), scanGroundCells.cols, scanGroundCells.channels());
+				UDEBUG("obstacleCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", obstacleCells.cols, obstacleCells.channels(), scanObstacleCells.cols, scanObstacleCells.channels());
+				UDEBUG("emptyCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", emptyCells.cols, emptyCells.channels(), scanEmptyCells.cols, scanEmptyCells.channels());
+
+				if(!groundCells.empty() && !scanGroundCells.empty())
+					cv::hconcat(groundCells, scanGroundCells, groundCells);
+				else if(!scanGroundCells.empty())
+					groundCells = scanGroundCells;
+
+				if(!obstacleCells.empty() && !scanObstacleCells.empty())
+					cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells);
+				else if(!scanObstacleCells.empty())
+					obstacleCells = scanObstacleCells;
+
+				if(!emptyCells.empty() && !scanEmptyCells.empty())
+					cv::hconcat(emptyCells, scanEmptyCells, emptyCells);
+				else if(!scanEmptyCells.empty())
+					emptyCells = scanEmptyCells;
+			}
 		}
 	}
 }

corelib/src/Odometry.cpp

@@ -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;
@@ -299,9 +303,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation*
 					data.imu().localTransform().rotation().inverse();
 
-			IMU imu2 = data.imu();
-			imu2.convertToBaseFrame();
-
 			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
 				this->framesProcessed() == 0)
 			{

corelib/src/PDALWriter.cpp

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

corelib/src/Parameters.cpp

@@ -213,14 +213,15 @@ ParametersMap Parameters::getDefaultParameters(const std::string & groupIn)
 	return parameters;
 }
 
-ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & groupIn)
+ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & group, bool remove)
 {
 	ParametersMap output;
 	for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
 		UASSERT(uSplit(iter->first, '/').size()  == 2);
 		std::string group = uSplit(iter->first, '/').front();
-		if(group.compare(groupIn) == 0)
+		bool sameGroup = group.compare(group) == 0;
+		if((!remove && sameGroup) || (remove && !sameGroup))
 		{
 			output.insert(*iter);
 		}
@@ -234,6 +235,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.20.15
+		removedParameters_.insert(std::make_pair("Grid/FromDepth",           std::make_pair(true, Parameters::kGridSensor())));
+
 		// 0.20.9
 		removedParameters_.insert(std::make_pair("OdomORBSLAM2/VocPath",     std::make_pair(true, Parameters::kOdomORBSLAMVocPath())));
 		removedParameters_.insert(std::make_pair("OdomORBSLAM2/Bf",          std::make_pair(true, Parameters::kOdomORBSLAMBf())));
@@ -662,6 +666,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With PDAL:";
+#ifdef RTABMAP_PDAL
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With TORO:";
 #ifdef RTABMAP_TORO
@@ -824,6 +834,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With FLOAM:";
+#ifdef RTABMAP_FLOAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With FOVIS:";
 #ifdef RTABMAP_FOVIS
@@ -1053,7 +1069,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 						ignore = true;
 					}
 #endif
-#ifndef RTABMAP_LOAM
+#if not defined(RTABMAP_LOAM) and not defined(RTABMAP_FLOAM)
 					if(group.compare("OdomLOAM") == 0)
 					{
 						ignore = true;

corelib/src/Rtabmap.cpp

@@ -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())
 	{
@@ -4408,9 +4418,9 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	{
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
-			// Apply guess poses (if some)
+			// Apply guess poses (if some), ignore for rootid to avoid origin drifting
 			std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
-			if(foundGuess!=guessPoses.end())
+			if(foundGuess!=guessPoses.end() && iter->first != fromId)
 			{
 				iter->second = foundGuess->second;
 			}
@@ -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)

corelib/src/camera/CameraDepthAI.cpp

@@ -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())
+	cv::Size targetSize(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);
-	{
-		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();
-	}
 
 	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");
+	xoutLeft->setStreamName("rectified_left");
-	xoutDepthOrRight->setStreamName(outputDepth_?"depth"/*:stereoModel_.isValidForRectification()?"rectified_right"*/:"right");
+	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->initialConfig.setConfidenceThreshold(depthConfidence_);
-	stereo->setOutputRectified(stereoModel_.isValidForRectification());
-	stereo->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("");
+	UINFO("Loading eeprom calibration data");
-	if(outputDepth_)
+	dai::CalibrationHandler calibHandler = device_->readCalibration();
-	{
+	std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height));
-		leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
+	double fx = matrix[0][0];
-		rightOrDepthQueue_ = device_->getOutputQueue("depth", 8, false);
+	double fy = matrix[1][1];
-	}
+	double cx = matrix[0][2];
-	else
+	double cy = matrix[1][2];
-	{
+	matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT);
-		UDEBUG("");
+	double baseline = matrix[0][3]/100.0;
-		leftQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_left":*/"left", 8, false);
+	UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
-		UDEBUG("");
+	stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);
-		rightOrDepthQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_right":*/"right", 8, false);
-		UDEBUG("");
-	}
 
-	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

corelib/src/camera/CameraImages.cpp

@@ -862,7 +862,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 						cv::cvtColor(img, out, CV_BGRA2BGR);
 						img = out;
 					}
-					else if(_bayerMode >= 0 && _bayerMode <=3)
+					else if(!img.empty() && _bayerMode >= 0 && _bayerMode <=3)
 					{
 						cv::Mat debayeredImg;
 						try

corelib/src/camera/CameraOpenNI2.cpp

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

corelib/src/camera/CameraRealSense2.cpp

@@ -70,6 +70,7 @@ CameraRealSense2::CameraRealSense2(
 	rectifyImages_(true),
 	odometryProvided_(false),
 	odometryImagesDisabled_(false),
+	odometryOnlyLeftStream_(false),
 	cameraWidth_(640),
 	cameraHeight_(480),
 	cameraFps_(30),
@@ -193,7 +194,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
 
 void CameraRealSense2::frame_callback(rs2::frame frame)
 {
-	//UDEBUG("Frame callback! %f", frame.get_timestamp());
+	UDEBUG("Frame callback! %f", frame.get_timestamp());
 	syncer_(frame);
 }
 void CameraRealSense2::multiple_message_callback(rs2::frame frame)
@@ -694,8 +695,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	UDEBUG("");
 
 	model_ = CameraModel();
-	rs2::stream_profile depthStreamProfile;
-	rs2::stream_profile rgbStreamProfile;
 	std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(sensors.size());
 	 for (unsigned int i=0; i<sensors.size(); ++i)
 	 {
@@ -759,8 +758,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 								intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
 								intrinsic.model,
 								intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]);
-						rgbStreamProfile = profile;
-						rgbIntrinsics_ = intrinsic;
 						added = true;
 						if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2)
 						{
@@ -773,8 +770,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					{
 						profilesPerSensor[i].push_back(profile);
 						depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						depthIntrinsics_ = intrinsic;
 						added = true;
 						if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2)
 						{
@@ -828,20 +823,44 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 				{
 					UASSERT(i<2);
 					profilesPerSensor[i].push_back(profile);
-					auto intrinsic = video_profile.get_intrinsics();
 					if(pi==0)
 					{
 						// LEFT FISHEYE
 						rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						rgbStreamProfile = profile;
+						if(odometryOnlyLeftStream_)
-						rgbIntrinsics_ = intrinsic;
+						{
+							auto intrinsic = video_profile.get_intrinsics();
+							UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f",
+									intrinsic.width, intrinsic.height,
+									intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
+									intrinsic.model,
+									intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3]);
+							cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
+							K.at<double>(0,0) = intrinsic.fx;
+							K.at<double>(1,1) = intrinsic.fy;
+							K.at<double>(0,2) = intrinsic.ppx;
+							K.at<double>(1,2) = intrinsic.ppy;
+							UASSERT(intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4); // we expect fisheye 4 values
+							cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+							D.at<double>(0,0) = intrinsic.coeffs[0];
+							D.at<double>(0,1) = intrinsic.coeffs[1];
+							D.at<double>(0,4) = intrinsic.coeffs[2];
+							D.at<double>(0,5) = intrinsic.coeffs[3];
+							cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+							P.at<double>(0,0) = intrinsic.fx;
+							P.at<double>(1,1) = intrinsic.fy;
+							P.at<double>(0,2) = intrinsic.ppx;
+							P.at<double>(1,2) = intrinsic.ppy;
+							cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+							model_ = CameraModel(camera_name, cv::Size(intrinsic.width, intrinsic.height), K, D, R, P, this->getLocalTransform());
+							if(rectifyImages_)
+								model_.initRectificationMap();
 						}
-					else
+					}
+					else if(!odometryOnlyLeftStream_)
 					{
 						// RIGHT FISHEYE
 						depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						depthIntrinsics_ = intrinsic;
 					}
 					added = true;
 				}
@@ -961,7 +980,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 {
 			 serial = cameraName;
 		 }
-		if(!calibrationFolder.empty() && !serial.empty())
+		if(!odometryImagesDisabled_ &&
+		   !odometryOnlyLeftStream_ &&
+		   !calibrationFolder.empty() && !serial.empty())
 		{
 			if(!stereoModel_.load(calibrationFolder, serial, false))
 			{
@@ -1005,6 +1026,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 
 			Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 			this->setLocalTransform(this->getLocalTransform() * opticalTransform.inverse());
+			if(odometryOnlyLeftStream_)
+				model_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			else
 				stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
 			imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
 
@@ -1027,9 +1051,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
 			 imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
 			 UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+			 if(odometryOnlyLeftStream_)
+				 model_.setLocalTransform(this->getLocalTransform());
+			 else
 				 stereoModel_.setLocalTransform(this->getLocalTransform());
 		}
-		if(rectifyImages_ && !stereoModel_.isValidForRectification())
+		if(!odometryImagesDisabled_ && rectifyImages_ && !model_.isValidForRectification() && !stereoModel_.isValidForRectification())
 		{
 			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 			return false;
@@ -1192,6 +1219,7 @@ void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
 	{
 		odometryProvided_ = true;
 		odometryImagesDisabled_ = false;
+		odometryOnlyLeftStream_ = false;
 	}
 #endif
 }
@@ -1210,7 +1238,7 @@ void CameraRealSense2::setImagesRectified(bool enabled)
 #endif
 }
 
-void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
+void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, bool onlyLeftStream)
 {
 #ifdef RTABMAP_REALSENSE2
 	if(dualMode_ && !enabled)
@@ -1220,6 +1248,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
 	}
 	odometryProvided_ = enabled;
 	odometryImagesDisabled_ = enabled && imageStreamsDisabled;
+	odometryOnlyLeftStream_ = enabled && !imageStreamsDisabled && onlyLeftStream;
 #endif
 }
 
@@ -1374,9 +1403,29 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
 				}
 			}
-			else if(is_left_fisheye_arrived && is_right_fisheye_arrived)
+			else if(is_left_fisheye_arrived)
+			{
+				if(odometryOnlyLeftStream_)
+				{
+					cv::Mat left;
+					if(rectifyImages_ && model_.isValidForRectification())
+					{
+						left = model_.rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+					}
+					else
+					{
+						left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+					}
+
+					if(model_.imageHeight() == 0 || model_.imageWidth() == 0)
+					{
+						model_.setImageSize(left.size());
+					}
+
+					data = SensorData(left, cv::Mat(), model_, this->getNextSeqID(), stamp);
+				}
+				else if(is_right_fisheye_arrived)
 				{
-				auto from_image_frame = depth_frame.as<rs2::video_frame>();
 					cv::Mat left,right;
 					if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
 					{
@@ -1396,6 +1445,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 
 					data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
 				}
+			}
 			else
 			{
 				UERROR("Not received depth and rgb");
@@ -1471,6 +1521,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				lastImuStamp_ = imuStamp;
 			}
 		}
+		else if(frameset.size()==1 && frameset[0].get_profile().stream_type() == RS2_STREAM_FISHEYE)
+		{
+			UERROR("Missing frames (received %d, needed=%d). For T265 camera, "
+					"either use realsense sdk v2.42.0, or apply "
+					"this patch (https://github.com/IntelRealSense/librealsense/issues/9030#issuecomment-962223017) "
+					"to fix this problem.", (int)frameset.size(), desiredFramesetSize);
+		}
 		else
 		{
 			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);

corelib/src/camera/CameraStereoImages.cpp

@@ -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.");
+		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.");
-		return false;
 	}
 
 	//desactivate before init as we will do it in this class instead for convenience
@@ -165,6 +166,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
 	{
 		if(camera2_)
 		{
+			camera2_->setBayerMode(this->getBayerMode());
 			right = camera2_->takeImage(info);
 		}
 		else

corelib/src/clams/discrete_depth_distortion_model.cpp

@@ -275,19 +275,21 @@ namespace clams
 
   void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
   {
-    UASSERT(width_ == depth.cols);
+    UASSERT(width_ >= depth.cols && width_ % depth.cols == 0);
-    UASSERT(height_ ==depth.rows);
+    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 v = 0; v < depth.rows; ++v) {
-		  for(int u = 0; u < width_; ++u) {
+		  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 v = 0; v < depth.rows; ++v) {
-		  for(int u = 0; u < width_; ++u) {
+		  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;
 		  }
 		}

corelib/src/odometry/OdometryF2F.cpp

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

corelib/src/odometry/OdometryFLOAM.cpp

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

corelib/src/odometry/OdometryLOAM.cpp

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

corelib/src/odometry/OdometryORBSLAM.cpp

@@ -813,6 +813,10 @@ public:
 			Verbose::SetTh(Verbose::VERBOSITY_QUIET);
 #endif
 
+		// Reset all static variables
+		Frame::mbInitialComputations = true;
+		mpTracker->Reset(true);
+
 		return true;
 	}
 

corelib/src/odometry/OdometryOpenVINS.cpp

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

corelib/src/util3d.cpp

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

corelib/src/util3d_surface.cpp

@@ -601,15 +601,15 @@ 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)
-	{
-		UASSERT(poseIter->first == modelIter->first);
 
+	for(std::map<int, Transform>::const_iterator poseIter=poses.begin(); poseIter!=poses.end(); ++poseIter)
+	{
+		std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.find(poseIter->first);
+
+		if(modelIter!=cameraModels.end())
+		{
 			std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);
 
 			// for each sub camera
@@ -668,6 +668,7 @@ pcl::texture_mapping::CameraVector createTextureCameras(
 				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.textureSide = textureSize;
-	texturing.texParams.downscale = 8192/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();
+			Transform t = (iter->second * model.localTransform()).inverse();
-			Eigen::Matrix<double, 3, 4> m = (t.inverse()).toEigen3d().matrix().block<3,4>(0, 0);
+			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...");
+	UINFO("Unwrapping (method=%d=%s)...", unwrapMethod, mesh::EUnwrapMethod_enumToString((mesh::EUnwrapMethod)unwrapMethod).c_str());
-	texturing.unwrap(mp, mesh::EUnwrapMethod::Basic);
+	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;

docker/bionic/Dockerfile

@@ -38,66 +38,81 @@ RUN cd libpointmatcher && \
     cd && \
     rm -r libpointmatcher
 
-# AliceVision
+ARG TARGETPLATFORM
-RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
+ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
+RUN echo "I am building for $TARGETPLATFORM"
+
+# arm64
+RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then ln -s /usr/bin/cmake ~/cmake; fi
+
+# cmake >=3.11 required for amd64 dependencies
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt install -y wget && \
+    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 &&\
+    ln -s ~/cmake-3.17.0-Linux-x86_64/bin/cmake ~/cmake; fi
+
+# AliceVision v2.4.0 modified (Sept 13 2021)
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
       libsuitesparse-dev \
       libceres-dev \
       xorg-dev \
-      libglu1-mesa-dev \
+      libglu1-mesa-dev; fi
-      wget
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \
-RUN git clone https://github.com/OpenImageIO/oiio.git
+    cd oiio && \
-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
+    rm -r oiio; fi
-RUN git clone https://github.com/alembic/alembic.git
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \
-RUN cd alembic && \
+    cd assimp && \
-    git checkout 1.7.12 && \
+    git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \
     mkdir build && \
     cd build && \
     cmake .. && \
     make -j$(nproc) && \
     make install && \
     cd && \
-    rm -r alembic
+    rm -r assimp; fi
-RUN git clone https://github.com/alicevision/geogram.git
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \
-RUN cd geogram && \
+    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
+    rm -r geogram; fi
-RUN git clone https://github.com/alicevision/AliceVision.git --recursive
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \
-RUN cd AliceVision && \
+    cd AliceVision && \
-    git checkout v2.2.0 && \
+    git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
-    wget https://gist.githubusercontent.com/matlabbe/469bba5e7733ad6f2e3d7857b84f1f9e/raw/edaa88ed38344219af1cc919a5597f5a74445336/alice_vision_eigen.patch && \
+    wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \
-    git apply alice_vision_eigen.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 && \
-    rm -r AliceVision
+    rm -r AliceVision; fi
-
-# Clone source code
-ARG CACHE_DATE=2016-01-01
-RUN git clone https://github.com/introlab/rtabmap.git
 
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
+# Clone source code
+ARG CACHE_DATE=2016-01-01
+
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
+    git clone https://github.com/introlab/rtabmap.git && \
     cd rtabmap/build && \
-    cmake -DWITH_ALICE_VISION=ON .. && \
+    ~/cmake -DWITH_ALICE_VISION=ON .. && \
-    make -j2 && \
+    make -j$(nproc) && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/bionic/android/rtabmap_api23/Dockerfile

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

docker/bionic/android/rtabmap_api24/Dockerfile

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

docker/bionic/android/rtabmap_api26/Dockerfile

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

docker/bionic/nvidia/Dockerfile

@@ -5,3 +5,9 @@ ENV NVIDIA_VISIBLE_DEVICES \
     ${NVIDIA_VISIBLE_DEVICES:-all}
 ENV NVIDIA_DRIVER_CAPABILITIES \
     ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
+
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map
+
+# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776):
+# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra

docker/focal/Dockerfile

@@ -38,115 +38,132 @@ RUN cd libpointmatcher && \
     cd && \
     rm -r libpointmatcher
     
-# libfreenect2 
+# PDAL
-ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get install -y libpdal-dev
-RUN apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev
-RUN git clone https://github.com/OpenKinect/libfreenect2
-RUN cd libfreenect2 && \
-    mkdir build && \
-    cd build && \
-    cmake .. && \
-    make -j$(nproc) && \
-    make install && \
-    cd && \
-    rm -r libfreenect2
 
 # RealSense2
-RUN apt-get install -y ros-noetic-realsense2-camera
+RUN apt-get install -y ros-noetic-librealsense2
+
+ARG TARGETPLATFORM
+ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64}
+RUN echo "I am building for $TARGETPLATFORM"
+
+ENV DEBIAN_FRONTEND=noninteractive
 
 # Azure Kinect DK
 # Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494
 # K4A binaries on 20.04 not released yet, we should take those from 18.04
-RUN curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \
-RUN curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb
+    echo "Download libk4a1.3_1.3.0_amd64.deb..." && \
-RUN curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb
+    curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \
-RUN curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb
+    echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \
-RUN curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb
+    curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \
-RUN echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections
+    echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \
-RUN echo 'libk4abt1.0	libk4abt1.0/accepted-eula-hash	string	03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections
+    curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \
-RUN dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb
+    echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \
-RUN dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb
+    curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \
-RUN dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb
+    echo "Download k4a-tools_1.3.0_amd64.deb..." && \
-RUN dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb
+    curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \
-RUN apt-get install -y libsoundio1
+    echo "Accept license..." && \
-RUN dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb
+    echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \
-RUN rm /tmp/libk4a* /tmp/k4a*
+    echo 'libk4abt1.0	libk4abt1.0/accepted-eula-hash	string	03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \
+    dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \
+    dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \
+    dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \
+    dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \
+    apt-get install -y libsoundio1 && \
+    dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \
+    rm /tmp/libk4a* /tmp/k4a*; fi
 
-# zed open capture
+# libfreenect2 
-RUN apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \
-RUN git clone https://github.com/stereolabs/zed-open-capture.git
+    apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \
-RUN cd zed-open-capture && \
+    git clone https://github.com/OpenKinect/libfreenect2 && \
+    cd libfreenect2 && \
     mkdir build && \
     cd build && \
     cmake .. && \
     make -j$(nproc) && \
     make install && \
     cd && \
-    rm -r zed-open-capture
+    rm -r libfreenect2; fi
 
-# AliceVision
+# zed open capture
-# Issue: It could be possible to use version >2.2, but there is a seg fault after texturing the mesh (see #564).
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \
-RUN apt-get update && apt-get install -y \
+    apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
+    git clone https://github.com/stereolabs/zed-open-capture.git && \
+    cd zed-open-capture && \
+    mkdir build && \
+    cd build && \
+    cmake .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r zed-open-capture; fi
+
+# AliceVision v2.4.0 modified (Sept 13 2021)
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \
+apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
       libsuitesparse-dev \
       libceres-dev \
       xorg-dev \
       libglu1-mesa-dev \
-      wget \
+      wget; fi
-      python-is-python3
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \
-RUN git clone https://github.com/OpenImageIO/oiio.git
+    cd oiio && \
-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
+    rm -r oiio; fi
-RUN git clone https://github.com/alembic/alembic.git
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \
-RUN cd alembic && \
+    cd assimp && \
-    git checkout 1.7.12 && \
+    git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \
     mkdir build && \
     cd build && \
     cmake .. && \
     make -j$(nproc) && \
     make install && \
     cd && \
-    rm -r alembic
+    rm -r assimp; fi
-RUN git clone -b v1.7.1 https://github.com/alicevision/geogram.git
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \
-RUN cd geogram && \
+    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
+    rm -r geogram; fi
-RUN git clone -b v2.2.0 https://github.com/alicevision/AliceVision.git --recursive
+RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \
-RUN cd AliceVision && \
+    cd AliceVision && \
-    wget https://gist.githubusercontent.com/matlabbe/469bba5e7733ad6f2e3d7857b84f1f9e/raw/f0545b36028a1156e857ed433547fdade0cbf53f/alice_vision_eigen.patch && \
+    git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \
-    git apply alice_vision_eigen.patch && \
+    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 && \
-    rm -r AliceVision
+    rm -r AliceVision; fi
-    
-# PDAL
-RUN apt-get install -y libpdal-dev
-
-# Clone source code
-ARG CACHE_DATE=2016-01-01
-RUN git clone https://github.com/introlab/rtabmap.git
 
 RUN rm /bin/sh && ln -s /bin/bash /bin/sh
 
+# Clone source code
+ARG CACHE_DATE=2016-01-01
+
 # Build RTAB-Map project
 RUN source /ros_entrypoint.sh && \
+    git clone https://github.com/introlab/rtabmap.git && \
     cd rtabmap/build && \
     cmake -DWITH_ALICE_VISION=ON .. && \
-    make -j2 && \
+    make -j$(nproc) && \
     make install && \
     cd ../.. && \
     rm -rf rtabmap && \

docker/focal/nvidia/Dockerfile

@@ -5,3 +5,9 @@ ENV NVIDIA_VISIBLE_DEVICES \
     ${NVIDIA_VISIBLE_DEVICES:-all}
 ENV NVIDIA_DRIVER_CAPABILITIES \
     ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
+    
+# Will be used to read/store databases on host
+RUN mkdir -p /root/Documents/RTAB-Map
+
+# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776):
+# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -122,6 +122,7 @@ private Q_SLOTS:
 	void detectMoreLoopClosures();
 	void updateAllNeighborCovariances();
 	void updateAllLoopClosureCovariances();
+	void updateAllLandmarkCovariances();
 	void refineAllNeighborLinks();
 	void refineAllLoopClosureLinks();
 	void resetAllChanges();

guilib/include/rtabmap/gui/EditConstraintDialog.h

@@ -42,7 +42,7 @@ class RTABMAPGUI_EXP EditConstraintDialog : public QDialog
 	Q_OBJECT
 
 public:
-	EditConstraintDialog(const Transform & constraint, double linearSigma = 0, double angularSigma = 0, QWidget * parent = 0);
+	EditConstraintDialog(const Transform & constraint, double linearSigma = 1, double angularSigma = 1, QWidget * parent = 0);
 
 	virtual ~EditConstraintDialog();
 	Transform getTransform() const;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -226,7 +226,7 @@ public:
 	double getSubtractFilteringAngle() const;
 
 	bool getGridMapShown() const;
-	bool isGridMapFrom3DCloud() const;
+	int getGridMapSensor() const;
 	bool projMapFrame() const;
 	double projMaxGroundAngle() const;
 	double projMaxGroundHeight() const;
@@ -339,6 +339,7 @@ private Q_SLOTS:
 	void updateKpROI();
 	void updateStereoDisparityVisibility();
 	void updateFeatureMatchingVisibility();
+	void updateOdometryStackedIndex(int index);
 	void useOdomFeatures();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
@@ -414,7 +415,7 @@ private:
 	void addParameters(const QGroupBox * box);
 	QList<QGroupBox*> getGroupBoxes();
 	void readSettingsBegin();
-	Camera * createCamera(Src driver, const QString & device, const QString & calibrationPath, bool useRawImages, bool useColor, bool odomOnly); // return camera should be deleted if not null
+	Camera * createCamera(Src driver, const QString & device, const QString & calibrationPath, bool useRawImages, bool useColor, bool odomOnly, bool odomSensorExtrinsicsCalib); // return camera should be deleted if not null
 
 protected:
 	PANEL_FLAGS _obsoletePanels;

guilib/src/CloudViewer.cpp

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

guilib/src/DatabaseViewer.cpp

@@ -292,6 +292,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
 	connect(ui_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances()));
 	connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances()));
+	connect(ui_->actionUpdate_all_landmark_covariances, SIGNAL(triggered()), this, SLOT(updateAllLandmarkCovariances()));
 	connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
 	connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
 	connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
@@ -1757,6 +1758,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 +1774,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])));
@@ -1818,7 +1831,8 @@ void DatabaseViewer::updateIds()
 				ids.find(jter->second.from()) != ids.end() &&
 				(ids.find(jter->second.to()) != ids.end() || jter->second.to()<0) && // to add landmark links
 				graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
-				invertedLinkIter != links.end())
+				invertedLinkIter != links.end() &&
+				w != -9)
 			{
 				// check if user_data is set in opposite direction
 				if(jter->second.userDataCompressed().cols == 0 &&
@@ -1983,6 +1997,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());
@@ -4084,7 +4130,27 @@ void DatabaseViewer::updateAllNeighborCovariances()
 }
 void DatabaseViewer::updateAllLoopClosureCovariances()
 {
-	updateAllCovariances(loopLinks_);
+	QList<rtabmap::Link> links;
+	for(int i=0; i<loopLinks_.size(); ++i)
+	{
+		if(loopLinks_.at(i).type() != Link::kLandmark)
+		{
+			links.push_back(loopLinks_.at(i));
+		}
+	}
+	updateAllCovariances(links);
+}
+void DatabaseViewer::updateAllLandmarkCovariances()
+{
+	QList<rtabmap::Link> links;
+	for(int i=0; i<loopLinks_.size(); ++i)
+	{
+		if(loopLinks_.at(i).type() == Link::kLandmark)
+		{
+			links.push_back(loopLinks_.at(i));
+		}
+	}
+	updateAllCovariances(links);
 }
 
 void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
@@ -4092,10 +4158,10 @@ void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
 	if(links.size())
 	{
 		bool ok = false;
-		double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m)"), 0.01, 0.0001, 9, 4, &ok);
+		double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m) 0=inf"), 0.01, 0.0, 9, 4, &ok);
 		if(!ok) return;
 		double linearVar = stddev*stddev;
-		stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg)"), 1, 0.01, 45, 2, &ok)*M_PI/180.0;
+		stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg) 0=inf"), 1, 0.0, 90, 2, &ok)*M_PI/180.0;
 		if(!ok) return;
 		double angularVar = stddev*stddev;
 
@@ -4107,8 +4173,22 @@ void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
 		progressDialog->show();
 
 		cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
-		infMatrix(cv::Range(0,3), cv::Range(0,3))/=linearVar;
+		if(linearVar == 0.0)
-		infMatrix(cv::Range(3,6), cv::Range(3,6))/=angularVar;
+		{
+			infMatrix(cv::Range(0,3), cv::Range(0,3)) /= 9999.9;
+		}
+		else
+		{
+			infMatrix(cv::Range(0,3), cv::Range(0,3)) /= linearVar;
+		}
+		if(angularVar == 0.0)
+		{
+			infMatrix(cv::Range(3,6), cv::Range(3,6)) /= 9999.9;
+		}
+		else
+		{
+			infMatrix(cv::Range(3,6), cv::Range(3,6)) /= angularVar;
+		}
 
 		for(int i=0; i<links.size(); ++i)
 		{
@@ -4945,6 +5025,7 @@ void DatabaseViewer::update(int value,
 								float xMin=0.0f, yMin=0.0f;
 								cv::Mat map8S;
 								ParametersMap parameters = ui_->parameters_toolbox->getParameters();
+								parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
 								float gridCellSize = Parameters::defaultGridCellSize();
 								Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
 #ifdef RTABMAP_OCTOMAP
@@ -4955,7 +5036,7 @@ void DatabaseViewer::update(int value,
 								else
 #endif
 								{
-									OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+									OccupancyGrid grid(parameters);
 									grid.setCellSize(gridCellSize);
 									grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
 									grid.update(poses);
@@ -5558,24 +5639,41 @@ void DatabaseViewer::editConstraint()
 {
 	if(ids_.size())
 	{
-		Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
+		Link link;
+		if(ui_->label_type->text().toInt() == Link::kLandmark)
+		{
+			int position = ui_->horizontalSlider_loops->value();
+			link = loopLinks_.at(position);
+		}
+		else
+		{
+			link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
+		}
 		if(link.isValid())
 		{
 			cv::Mat covBefore = link.infMatrix().inv();
 			EditConstraintDialog dialog(link.transform(),
-					covBefore.at<double>(0,0)!=1.0?std::sqrt(covBefore.at<double>(0,0)):0,
+					covBefore.at<double>(0,0)<9999.0?std::sqrt(covBefore.at<double>(0,0)):0.0,
-					covBefore.at<double>(5,5)!=1.0?std::sqrt(covBefore.at<double>(5,5)):0);
+					covBefore.at<double>(5,5)<9999.0?std::sqrt(covBefore.at<double>(5,5)):0.0);
 			if(dialog.exec() == QDialog::Accepted)
 			{
 				bool updated = false;
 				cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
 				if(dialog.getLinearVariance()>0)
 				{
-					covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance();
+					covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance();
+				}
+				else
+				{
+					covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
 				}
 				if(dialog.getAngularVariance()>0)
 				{
-					covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance();
+					covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
+				}
+				else
+				{
+					covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
 				}
 				Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
 				std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
@@ -5610,11 +5708,19 @@ void DatabaseViewer::editConstraint()
 				cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
 				if(dialog.getLinearVariance()>0)
 				{
-					covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance();
+					covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance();
+				}
+				else
+				{
+					covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
 				}
 				if(dialog.getAngularVariance()>0)
 				{
-					covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance();
+					covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
+				}
+				else
+				{
+					covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
 				}
 				int from = ids_.at(ui_->horizontalSlider_A->value());
 				int to = ids_.at(ui_->horizontalSlider_B->value());
@@ -6271,14 +6377,20 @@ void DatabaseViewer::updateConstraintButtons()
 	ui_->pushButton_reject->setEnabled(false);
 	ui_->toolButton_constraint->setEnabled(false);
 
+	Link currentLink;
+	int from;
+	int to;
 	if(ui_->label_type->text().toInt() == Link::kLandmark)
 	{
-		ui_->pushButton_reject->setEnabled(true);
+		//check for modified link
-		return;
+		currentLink = loopLinks_.at(ui_->horizontalSlider_loops->value());
+		from = currentLink.from();
+		to = currentLink.to();
 	}
-
+	else
-	int from = ids_.at(ui_->horizontalSlider_A->value());
+	{
-	int to = ids_.at(ui_->horizontalSlider_B->value());
+		from = ids_.at(ui_->horizontalSlider_A->value());
+		to = ids_.at(ui_->horizontalSlider_B->value());
 		if(from!=to && from && to &&
 		   odomPoses_.find(from) != odomPoses_.end() &&
 		   odomPoses_.find(to) != odomPoses_.end() &&
@@ -6293,7 +6405,8 @@ void DatabaseViewer::updateConstraintButtons()
 			}
 		}
 
-	Link currentLink = findActiveLink(from ,to);
+		currentLink = findActiveLink(from ,to);
+	}
 
 	if(currentLink.isValid() &&
 		((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
@@ -6304,19 +6417,13 @@ void DatabaseViewer::updateConstraintButtons()
 		}
 
 		//check for modified link
-		bool modified = false;
 		std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
 		if(iter != linksRefined_.end())
 		{
 			currentLink = iter->second;
 			ui_->pushButton_reset->setEnabled(true);
-			modified = true;
 		}
-		if(!modified)
+		ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to() && currentLink.type() != Link::kLandmark);
-		{
-			ui_->pushButton_reset->setEnabled(false);
-		}
-		ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to());
 		ui_->toolButton_constraint->setEnabled(true);
 	}
 }
@@ -6503,6 +6610,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				ui_->graphViewer->updatePosterior(colors, 1, 1);
 			}
 		}
+		QGraphicsRectItem * rectScaleItem = 0;
 		ui_->graphViewer->clearMap();
 		occupancyGridViewer_->clear();
 		if(graph.size() && localMaps.size() &&
@@ -6588,6 +6696,70 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 						occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
 						occupancyGridViewer_->refreshView();
 					}
+
+					// Zoom to ignore unknowns
+					int xFirst = 0;
+					int yFirst = 0;
+					int xLast = map.cols;
+					int yLast = map.rows;
+					bool firstSet = false;
+					bool lastSet = false;
+					for(int x=0; x<map.cols && (!firstSet || !lastSet); ++x)
+					{
+						for(int y=0; y<map.rows; ++y)
+						{
+							// check for first
+							if(!firstSet && map.at<char>(y, x) != -1)
+							{
+								xFirst = x;
+								firstSet = true;
+							}
+							// check for last
+							int opp = map.cols-(x+1);
+							if(!lastSet && map.at<char>(y, opp) != -1)
+							{
+								xLast = opp;
+								lastSet = true;
+							}
+						}
+					}
+					firstSet = false;
+					lastSet = false;
+					for(int y=0; y<map.rows && (!firstSet || !lastSet); ++y)
+					{
+						for(int x=0; x<map.cols; ++x)
+						{
+							// check for first
+							if(!firstSet && map.at<char>(y, x) != -1)
+							{
+								yFirst = y;
+								firstSet = true;
+							}
+							// check for last
+							int opp = map.rows-(y+1);
+							if(!lastSet && map.at<char>(map.rows-(y+1), x) != -1)
+							{
+								yLast = opp;
+								lastSet = true;
+							}
+						}
+					}
+					// Only zoom if there are significant unknowns
+					if( (xLast > xFirst && yLast > yFirst) &&
+						(xFirst > 50 ||
+						 xLast < map.cols-50 ||
+						 yFirst > 50 ||
+						 yLast < map.rows-50))
+					{
+						rectScaleItem = ui_->graphViewer->scene()->addRect(
+								xFirst-25,
+								yFirst-25,
+								xLast-xFirst+50,
+								yLast-yFirst+50);
+						rectScaleItem->setTransform(QTransform::fromScale(cellSize*100.0f, -cellSize*100.0f), true);
+						rectScaleItem->setRotation(90);
+						rectScaleItem->setPos(-yMin*100.0f, -xMin*100.0f);
+					}
 				}
 			}
 
@@ -6714,6 +6886,13 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			}
 		}
 		ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+		if(rectScaleItem != 0)
+		{
+			ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio);
+			ui_->graphViewer->scene()->removeItem(rectScaleItem);
+			delete rectScaleItem;
+		}
+
 		ui_->graphViewer->update();
 		ui_->label_iterations->setNum(value);
 
@@ -7511,8 +7690,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(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() || !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 +7734,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 +7981,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 +7992,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())
 		{
@@ -8106,6 +8329,14 @@ void DatabaseViewer::resetConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
+	if(ui_->label_type->text().toInt() == Link::kLandmark)
+	{
+		int position = ui_->horizontalSlider_loops->value();
+		const rtabmap::Link & link = loopLinks_.at(position);
+		from = link.from();
+		to = link.to();
+	}
+
 	if(from < to)
 	{
 		int tmp = to;

guilib/src/EditConstraintDialog.cpp

@@ -48,11 +48,12 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double
 	_ui->roll->setValue(roll);
 	_ui->pitch->setValue(pitch);
 	_ui->yaw->setValue(yaw);
+
+	_ui->checkBox_radians->setChecked(true);
 	_ui->linear_sigma->setValue(linearSigma);
 	_ui->angular_sigma->setValue(angularSigma);
 
 	connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits()));
-	_ui->checkBox_radians->setChecked(false);
 }
 
 EditConstraintDialog::~EditConstraintDialog()
@@ -111,7 +112,7 @@ Transform EditConstraintDialog::getTransform() const
 
 double EditConstraintDialog::getLinearVariance() const
 {
-	return _ui->linear_sigma->value();
+	return _ui->linear_sigma->value()*_ui->linear_sigma->value();
 }
 double EditConstraintDialog::getAngularVariance() const
 {
@@ -120,7 +121,8 @@ double EditConstraintDialog::getAngularVariance() const
 	{
 		conversion = M_PI/180.0;
 	}
-	return _ui->angular_sigma->value()*conversion;
+	double value = _ui->angular_sigma->value()*conversion;
+	return value*value;
 }
 
 }

guilib/src/ExportBundlerDialog.cpp

@@ -233,19 +233,20 @@ void ExportBundlerDialog::exportBundler(
 				std::map<int, QColor> colors;
 				for(std::map<int, Transform>::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
 				{
-					if(signatures.find(iter->first) != signatures.end())
+					QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
+					if(ster!= signatures.end())
 					{
-						cv::Mat image = signatures[iter->first].sensorData().imageRaw();
+						cv::Mat image = ster.value().sensorData().imageRaw();
 						if(image.empty())
 						{
-							signatures[iter->first].sensorData().uncompressDataConst(&image, 0, 0, 0);
+							ster.value().sensorData().uncompressDataConst(&image, 0, 0, 0);
 						}
 
 						double maxLinearVel = _ui->doubleSpinBox_linearSpeed->value();
 						double maxAngularVel = _ui->doubleSpinBox_angularSpeed->value();
 						double laplacianThr = _ui->doubleSpinBox_laplacianVariance->value();
 						bool blurryImage = false;
-						const std::vector<float> & velocity = signatures[iter->first].getVelocity();
+						const std::vector<float> & velocity = ster.value().getVelocity();
 						if(maxLinearVel>0.0 || maxAngularVel>0.0)
 						{
 							if(velocity.size() == 6)
@@ -437,15 +438,17 @@ void ExportBundlerDialog::exportBundler(
 					list << p << "\n";
 
 					Transform localTransform;
-					if(signatures[iter->first].sensorData().cameraModels().size())
+					QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
+					UASSERT(ster!=signatures.end());
+					if(ster.value().sensorData().cameraModels().size())
 					{
-						out << signatures[iter->first].sensorData().cameraModels().at(0).fx() << " 0 0\n";
+						out << ster.value().sensorData().cameraModels().at(0).fx() << " 0 0\n";
-						localTransform = signatures[iter->first].sensorData().cameraModels().at(0).localTransform();
+						localTransform = ster.value().sensorData().cameraModels().at(0).localTransform();
 					}
 					else
 					{
-						out << signatures[iter->first].sensorData().stereoCameraModel().left().fx() << " 0 0\n";
+						out << ster.value().sensorData().stereoCameraModel().left().fx() << " 0 0\n";
-						localTransform = signatures[iter->first].sensorData().stereoCameraModel().left().localTransform();
+						localTransform = ster.value().sensorData().stereoCameraModel().left().localTransform();
 					}
 
 					Transform pose = iter->second;

guilib/src/ExportCloudsDialog.cpp

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

guilib/src/GraphViewer.cpp

@@ -67,7 +67,7 @@ class NodeItem: public QGraphicsEllipseItem
 {
 public:
 	// in meter
-	NodeItem(int id, int mapId, const Transform & pose, float radius, int weight, GraphViewer::ViewPlane plane) :
+	NodeItem(int id, int mapId, const Transform & pose, float radius, int weight, GraphViewer::ViewPlane plane, float linkWidth) :
 		QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)),
 		_id(id),
 		_mapId(mapId),
@@ -82,6 +82,9 @@ public:
 		pose.getEulerAngles(r, p, yaw);
 		radius*=100.0f;
 		_line = new QGraphicsLineItem(0,0,-radius*sin(yaw),-radius*cos(yaw), this);
+		QPen pen = _line->pen();
+		pen.setWidth(linkWidth*100.0f);
+		_line->setPen(pen);
 	}
 	virtual ~NodeItem() {}
 
@@ -94,7 +97,9 @@ public:
 		b.setColor(color);
 		this->setBrush(b);
 
-		_line->setPen(QPen(QColor(255-color.red(), 255-color.green(), 255-color.blue())));
+		QPen pen = _line->pen();
+		pen.setColor(QColor(255-color.red(), 255-color.green(), 255-color.blue()));
+		_line->setPen(pen);
 	}
 
 	void setRadius(float radius)
@@ -157,8 +162,8 @@ private:
 class NodeGPSItem: public NodeItem
 {
 public:
-	NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps, GraphViewer::ViewPlane plane) :
+	NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps, GraphViewer::ViewPlane plane, float linkWidth) :
-		NodeItem(id, mapId, pose, radius, -1, plane),
+		NodeItem(id, mapId, pose, radius, -1, plane, linkWidth),
 		_gps(gps)
 	{
 	}
@@ -490,7 +495,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			{
 				// create node item
 				const Transform & pose = iter->second;
-				NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane);
+				NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane, _linkWidth);
 				this->scene()->addItem(item);
 				item->setZValue(iter->first<0?21:20);
 				item->setColor(iter->first<0?QColor(255-_nodeColor.red(), 255-_nodeColor.green(), 255-_nodeColor.blue()):_nodeColor);
@@ -710,7 +715,7 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 			{
 				// create node item
 				const Transform & pose = iter->second;
-				NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane);
+				NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth);
 				this->scene()->addItem(item);
 				item->setZValue(20);
 				item->setColor(_gtPathColor);
@@ -744,6 +749,19 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 					}
 				}
 				if(linkItem == 0)
+				{
+					bool linkFound = iter->first - iterPrevious->first == 1; // if consecutive, add link
+					for(QMultiMap<int, LinkItem*>::iterator kter = _linkItems.find(iterPrevious->first);
+							kter!=_linkItems.end() && kter.key()==iterPrevious->first && !linkFound;
+							++kter)
+					{
+						if(kter.value()->from() == iterPrevious->first && kter.value()->to() == iter->first)
+						{
+							linkFound = true;
+						}
+					}
+
+					if(linkFound)
 					{
 						//create a link item
 						linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1, _viewPlane);
@@ -755,6 +773,7 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 						linkItem->setParentItem(_gtGraphRoot);
 						_gtLinkItems.insert(iterPrevious->first, linkItem);
 					}
+				}
 				if(linkItem)
 				{
 					linkItem->setColor(_gtPathColor);
@@ -840,7 +859,7 @@ void GraphViewer::updateGPSGraph(
 				// create node item
 				const Transform & pose = iter->second;
 				UASSERT(gpsValues.find(iter->first) != gpsValues.end());
-				NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane);
+				NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth);
 				this->scene()->addItem(item);
 				item->setZValue(20);
 				item->setColor(_gpsPathColor);
@@ -964,7 +983,8 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
 		_gridMap->setRotation(90);
 		_gridMap->setPixmap(QPixmap::fromImage(image));
 		_gridMap->setPos(-yMin*100.0f, -xMin*100.0f);
-		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+		// Re-shrink the scene to it's bounding contents
+		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());
 	}
 	else
 	{

guilib/src/MainWindow.cpp

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

guilib/src/PreferencesDialog.cpp

@@ -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()));
@@ -1215,7 +1219,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_grid_footprintWidth->setObjectName(Parameters::kGridFootprintWidth().c_str());
 	_ui->doubleSpinBox_grid_footprintHeight->setObjectName(Parameters::kGridFootprintHeight().c_str());
 	_ui->checkBox_grid_flatObstaclesDetected->setObjectName(Parameters::kGridFlatObstacleDetected().c_str());
-	_ui->groupBox_grid_fromDepthImage->setObjectName(Parameters::kGridFromDepth().c_str());
+	_ui->comboBox_grid_sensor->setObjectName(Parameters::kGridSensor().c_str());
 	_ui->checkBox_grid_projMapFrame->setObjectName(Parameters::kGridMapFrameProjection().c_str());
 	_ui->doubleSpinBox_grid_maxGroundAngle->setObjectName(Parameters::kGridMaxGroundAngle().c_str());
 	_ui->spinBox_grid_normalK->setObjectName(Parameters::kGridNormalK().c_str());
@@ -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");
@@ -4369,7 +4377,8 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 		{
 			//backward compatibility
 			std::string valueCpy = value;
-			if(key.compare(Parameters::kIcpStrategy()) == 0)
+			if( key.compare(Parameters::kIcpStrategy()) == 0 ||
+				key.compare(Parameters::kGridSensor()) == 0)
 			{
 				if(value.compare("true") == 0)
 				{
@@ -4913,6 +4922,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 +5193,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);
@@ -5574,9 +5596,9 @@ bool PreferencesDialog::getGridMapShown() const
 {
 	return _ui->checkBox_map_shown->isChecked();
 }
-bool PreferencesDialog::isGridMapFrom3DCloud() const
+int PreferencesDialog::getGridMapSensor() const
 {
-	return _ui->groupBox_grid_fromDepthImage->isChecked();
+	return _ui->comboBox_grid_sensor->currentIndex();
 }
 bool PreferencesDialog::projMapFrame() const
 {
@@ -5835,6 +5857,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 		 !_ui->checkBox_stereo_rectify->isChecked()) || 
 		useRawImages, 
 		useColor, 
+		false,
 		false);
 }
 
@@ -5844,7 +5867,8 @@ Camera * PreferencesDialog::createCamera(
 		const QString & calibrationPath,
 		bool useRawImages,
 		bool useColor,
-		bool odomOnly)
+		bool odomOnly,
+		bool odomSensorExtrinsicsCalib)
 {
 	if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed))
 	{
@@ -5992,7 +6016,7 @@ Camera * PreferencesDialog::createCamera(
 			if(driver == kSrcStereoRealSense2)
 			{
 				((CameraRealSense2*)camera)->setImagesRectified(!useRawImages);
-				((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly);
+				((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly, odomSensorExtrinsicsCalib);
 			}
 			else
 			{
@@ -6332,6 +6356,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());
 			}
 		}
 	}
@@ -6360,7 +6385,7 @@ Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & ti
 		timeOffset = _ui->doubleSpinBox_odom_sensor_time_offset->value()/1000.0;
 		scaleFactor = (float)_ui->doubleSpinBox_odom_sensor_scale_factor->value();
 
-		return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true);
+		return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true, false);
 	}
 	return 0;
 }
@@ -7037,7 +7062,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics()
 						odomDriver,
 						_ui->lineEdit_odomSourceDevice->text(),
 						_ui->lineEdit_odom_sensor_path_calibration->text(),
-						false, true, false); // Odom sensor
+						false, true, false, true); // Odom sensor
 				if(!camera)
 				{
 					return;

guilib/src/ui/DatabaseViewer.ui

@@ -745,6 +745,7 @@
     <addaction name="actionRefine_all_loop_closure_links"/>
     <addaction name="actionUpdate_all_neighbor_covariances"/>
     <addaction name="actionUpdate_all_loop_closure_covariances"/>
+    <addaction name="actionUpdate_all_landmark_covariances"/>
     <addaction name="separator"/>
     <addaction name="actionRegenerate_local_grid_maps"/>
     <addaction name="actionRegenerate_local_grid_maps_selected"/>
@@ -1542,7 +1543,7 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>-322</y>
+          <y>0</y>
           <width>518</width>
           <height>911</height>
          </rect>
@@ -2137,7 +2138,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>298</width>
+          <width>226</width>
           <height>192</height>
          </rect>
         </property>
@@ -3015,6 +3016,11 @@
     <string>RGBD-SLAM ID format (*.txt)</string>
    </property>
   </action>
+  <action name="actionUpdate_all_landmark_covariances">
+   <property name="text">
+    <string>Update all landmark covariances...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/editConstraintDialog.ui

@@ -258,7 +258,7 @@
    <item>
     <widget class="QLabel" name="label_9">
      <property name="text">
-      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Setting &amp;sigma; to 0 will set identity covariance.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Setting σ to 0 will set 9999 covariance.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
      </property>
     </widget>
    </item>

guilib/src/ui/exportCloudsDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>814</width>
+    <width>1032</width>
-    <height>680</height>
+    <height>869</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-1225</y>
+        <y>-3537</y>
-        <width>780</width>
+        <width>998</width>
-        <height>5739</height>
+        <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>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.20.13</version>
+  <version>0.20.15</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>

tools/CMakeLists.txt

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

tools/CleanupLocalGrids/CMakeLists.txt

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

tools/CleanupLocalGrids/main.cpp

@@ -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;
+}

tools/DetectMoreLoopClosures/main.cpp

@@ -211,6 +211,11 @@ int main(int argc, char * argv[])
 	}
 	delete driver;
 
+	for(ParametersMap::iterator iter=inputParams.begin(); iter!=inputParams.end(); ++iter)
+	{
+		printf(" Using parameter \"%s=%s\" from arguments\n", iter->first.c_str(), iter->second.c_str());
+	}
+
 	// Get the global optimized map
 	Rtabmap rtabmap;
 	printf("Initialization...\n");

tools/Export/main.cpp

@@ -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_size  #     Texture size 1024, 2048, 4096, 8192, 16384 (default 8192).\n"
-			"    --texture_count #     Maximum textures generated (default 1).\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"
@@ -89,14 +90,22 @@ void showUsage()
 			"    --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_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"
@@ -106,6 +115,9 @@ void showUsage()
 			"    --ymax #              Maximum range on Y axis to keep nodes to export.\n"
 			"    --zmin #              Minimum range on Z axis to keep nodes to export.\n"
 			"    --zmax #              Maximum range on Z axis to keep nodes to export.\n"
+			"    --filter_ceiling #    Filter points over a custom height (default 0 m, 0=disabled).\n"
+			"    --filter_floor #      Filter points below a custom height (default 0 m, 0=disabled).\n"
+
 			"\n%s", Parameters::showUsage());
 	;
 	exit(1);
@@ -132,24 +144,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;
@@ -159,6 +180,8 @@ int main(int argc, char * argv[])
 	std::string outputName;
 	std::string outputDir;
 	cv::Vec3f min, max;
+	float filter_ceiling = 0.0f;
+	float filter_floor = 0.0f;
 	for(int i=1; i<argc; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -267,6 +290,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 +341,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 +364,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;
@@ -549,6 +661,41 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--filter_ceiling") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				filter_ceiling = uStr2Float(argv[i]);
+				if(filter_floor!=0.0f && filter_ceiling != 0.0f && filter_ceiling<filter_floor)
+				{
+					printf("Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--filter_floor") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				filter_floor = uStr2Float(argv[i]);
+				if(filter_floor!=0.0f && filter_ceiling != 0.0f && filter_ceiling<filter_floor)
+				{
+					printf("Option --filter_ceiling (%f) should be higher than --filter_floor option (%f)!\n", filter_ceiling, filter_floor);
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+
 	}
 
 	if(decimation < 1)
@@ -683,17 +830,7 @@ int main(int argc, char * argv[])
 	{
 		printf("Global bundle adjustment...\n");
 		OptimizerG2O g2o(parameters);
-		std::map<int, cv::Point3f> points3DMap;
+		optimizedPoses = ((Optimizer*)&g2o)->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
-		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);
 		printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
 	}
 
@@ -954,6 +1091,21 @@ int main(int argc, char * argv[])
 
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport = mergedClouds;
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport = mergedCloudsI;
+
+		if(filter_ceiling != 0.0 || filter_floor != 0.0f)
+		{
+			printf("Passthrough filtering of the assembled cloud along z axis... (min=%f, max=%f, %d points)\n", filter_floor, filter_ceiling, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			if(!cloudToExport->empty())
+			{
+				cloudToExport = util3d::passThrough(cloudToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+			}
+			if(!cloudIToExport->empty())
+			{
+				cloudIToExport = util3d::passThrough(cloudIToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+			}
+			printf("Passthrough filtering of the assembled cloud alog z axis.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+		}
+
 		if(voxelSize>0.0f)
 		{
 			printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
@@ -969,6 +1121,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 +1148,7 @@ int main(int argc, char * argv[])
 						0,
 						std::vector<float>(),
 						distanceToCamPolicy);
+				pointToCamIntensity.resize(pointToPixel.size());
 			}
 
 			// color the cloud
@@ -1007,6 +1161,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 +1174,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 +1218,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 +1267,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())
+					{
+						if(!pointToCamIntensity.empty())
+						{
+							savePDALFile(outputPath, *cloudToExport, pointToCamId, binary, pointToCamIntensity);
+						}
+						else
+						{
 							savePDALFile(outputPath, *cloudToExport, pointToCamId, binary);
+						}
+					}
 					else if(!cloudIToExport->empty())
 						savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary);
 				}
@@ -1102,10 +1287,18 @@ int main(int argc, char * argv[])
 #endif
 				{
 					if(!pointToCamId.empty())
+					{
+						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);
 					else if(!cloudIToExport->empty())
@@ -1152,7 +1345,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 +1495,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 +1515,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());
 							}

tools/GlobalBundleAdjustment/CMakeLists.txt

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

tools/GlobalBundleAdjustment/main.cpp

@@ -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;
+}

tools/Info/main.cpp

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

tools/Reprocess/main.cpp

@@ -55,7 +55,8 @@ void showUsage()
 			"   rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n"
 			"\n"
 			"   For the second example, only parameters from the first database are used.\n"
-			"   If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database.\n"
+			"   If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database,\n"
+			"   then localization-only is done with next databases against the first one.\n"
 			"   To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n"
 			"   To upgrade version of an old database to newest version:\n"
 			"      rtabmap-reprocess --Db/TargetVersion \"\" \"input.db\" \"output.db\"\n"
@@ -68,10 +69,14 @@ 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"
+			"     -loc_null   On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
+			"     -gt         When reprocessing a single database, load its original optimized graph, then \n"
+			"                 set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\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 +216,7 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
+	bool save2DMap = false;
 	bool assemble2dMap = false;
 	bool assemble3dMap = false;
 	bool assemble2dOctoMap = false;
@@ -219,6 +225,8 @@ int main(int argc, char * argv[])
 	int startId = 0;
 	int stopId = 0;
 	int framesToSkip = 0;
+	bool locNull = false;
+	bool originalGraphAsGT = false;
 	bool scanFromDepth = false;
 	int scanDecimation = 1;
 	float scanRangeMin = 0.0f;
@@ -295,11 +303,26 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
+		{
+			locNull = true;
+			printf("In localization mode, when restarting a new session, the current localization pose is set to null (-loc_null option).\n");
+		}
+		else if(strcmp(argv[i], "-gt") == 0 || strcmp(argv[i], "--gt") == 0)
+		{
+			originalGraphAsGT = true;
+			printf("Original graph is used as ground truth for output database (-gt option).\n");
+		}
 		else if(strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--p") == 0)
 		{
 			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;
@@ -522,6 +545,13 @@ int main(int argc, char * argv[])
 	{
 		totalIds = ids.size();
 	}
+
+	std::map<int, Transform> gt;
+	if(databases.size() == 1 && originalGraphAsGT)
+	{
+		gt = dbDriver->loadOptimizedPoses();
+	}
+
 	dbDriver->closeConnection(false);
 
 	// Count remaining ids in the other databases
@@ -566,6 +596,11 @@ int main(int argc, char * argv[])
 	Rtabmap rtabmap;
 	rtabmap.init(parameters, outputDatabasePath);
 
+	if(!incrementalMemory && locNull)
+	{
+		rtabmap.setInitialPose(Transform());
+	}
+
 	bool rgbdEnabled = Parameters::defaultRGBDEnabled();
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
@@ -616,8 +651,18 @@ int main(int argc, char * argv[])
 					lastLocalizationOdomPose = info.odomPose;
 				}
 				rtabmap.triggerNewMap();
+				if(!incrementalMemory && locNull)
+				{
+					rtabmap.setInitialPose(Transform());
+				}
 				inMotion = true;
 			}
+
+			if(originalGraphAsGT)
+			{
+				data.setGroundTruth(gt.find(data.id()) != gt.end()?gt.at(data.id()):Transform());
+			}
+
 			UTimer t;
 			if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
 			{
@@ -855,6 +900,18 @@ int main(int argc, char * argv[])
 		float xMin,yMin;
 		cv::Mat map = grid.getMap(xMin, yMin);
 		if(!map.empty())
+		{
+			if(save2DMap)
+			{
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(outputDatabasePath))
+				{
+					driver->save2DMap(map, xMin, yMin, grid.getCellSize());
+					printf("Saving occupancy grid to database... done!\n");
+				}
+				delete driver;
+			}
+			else
 			{
 				cv::Mat map8U(map.rows, map.cols, CV_8U);
 				//convert to gray scaled map
@@ -879,6 +936,7 @@ int main(int argc, char * argv[])
 						map8U.at<unsigned char>(i, j) = gray;
 					}
 				}
+
 				if(cv::imwrite(outputPath, map8U))
 				{
 					printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
@@ -888,6 +946,7 @@ int main(int argc, char * argv[])
 					printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
 				}
 			}
+		}
 		else
 		{
 			printf("2D map is empty! Cannot save it!\n");
@@ -936,6 +995,18 @@ int main(int argc, char * argv[])
 		float xMin,yMin,cellSize;
 		cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
 		if(!map.empty())
+		{
+			if(save2DMap)
+			{
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(outputDatabasePath))
+				{
+					driver->save2DMap(map, xMin, yMin, cellSize);
+					printf("Saving occupancy grid to database... done!\n");
+				}
+				delete driver;
+			}
+			else
 			{
 				cv::Mat map8U(map.rows, map.cols, CV_8U);
 				//convert to gray scaled map
@@ -969,6 +1040,7 @@ int main(int argc, char * argv[])
 					printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
 				}
 			}
+		}
 		else
 		{
 			printf("OctoMap 2D projection map is empty! Cannot save it!\n");