Converted an assert to an error.

* Moved bin directory inside build directory (to make easier different builds with same source directory)

* Switched include order to avoid problems with remaining Version.h still in source directory taen before the one in binary dir. Fixed android build (updated res tool search path).

* workflow-cmake: fixed path to bin directory

.github/workflows/cmake.yml

@@ -52,7 +52,7 @@ jobs:
       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
 
     - name: Info
-      working-directory: ${{github.workspace}}/bin
+      working-directory: ${{github.workspace}}/build/bin
       run: |
         source /opt/ros/${{ matrix.ros_distro }}/setup.bash
         ./rtabmap-console --version

.github/workflows/docker.yml

@@ -17,36 +17,55 @@ jobs:
           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
@@ -62,6 +81,7 @@ jobs:
         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 }}

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 14)
+SET(RTABMAP_PATCH_VERSION 16)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -123,9 +123,9 @@ OPTION( BUILD_SHARED_LIBS "Set to OFF to build static libraries" ON )
 
 
 ####### OUTPUT DIR #######
-SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
-SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
-SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin)
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin)
+SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib)
 
 # Avoid Visual Studio bin/Release and bin/Debug sub directories
 SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
@@ -181,12 +181,13 @@ option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
 option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_TORO          "Include TORO support"                 ON)
-option(WITH_CERES         "Include Ceres support"                ON)
+option(WITH_CERES         "Include Ceres support"                OFF)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
-option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_CVSBA         "Include cvsba support"                OFF)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
-option(WITH_CCCORELIB     "Include CCCoreLib support"            ON)
-option(WITH_LOAM          "Include LOAM support"                 ON)
+option(WITH_CCCORELIB     "Include CCCoreLib support"            OFF)
+option(WITH_OPEN3D        "Include Open3D support"               OFF)
+option(WITH_LOAM          "Include LOAM support"                 OFF)
 option(WITH_FLOAM         "Include FLOAM support"                OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
@@ -195,19 +196,19 @@ option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
-option(WITH_DEPTHAI       "Include depthai-core support"         ON)
+option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
-option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
-option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
+option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
+option(WITH_OPENCHISEL    "Include open_chisel support"          OFF)
 option(WITH_ALICE_VISION  "Include AliceVision support"          OFF)
-option(WITH_FOVIS         "Include FOVIS support"                ON)
-option(WITH_VISO2         "Include VISO2 support"                ON)
-option(WITH_DVO           "Include DVO support"                  ON)
-option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" ON)
-option(WITH_OKVIS         "Include OKVIS support"                ON)
-option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"           OFF)
-option(WITH_VINS          "Include VINS-Fusion support"          ON)
-option(WITH_OPENVINS      "Include OpenVINS support"             ON)
+option(WITH_FOVIS         "Include FOVIS support"                OFF)
+option(WITH_VISO2         "Include VISO2 support"                OFF)
+option(WITH_DVO           "Include DVO support"                  OFF)
+option(WITH_ORB_SLAM      "Include ORB_SLAM2 or ORB_SLAM3  support" OFF)
+option(WITH_OKVIS         "Include OKVIS support"                OFF)
+option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
+option(WITH_VINS          "Include VINS-Fusion support"          OFF)
+option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
@@ -489,6 +490,19 @@ IF(WITH_CCCORELIB)
     ENDIF(CCCoreLib_FOUND)
 ENDIF(WITH_CCCORELIB)
 
+IF(WITH_OPEN3D)
+    IF(${CMAKE_VERSION} VERSION_LESS "3.19.0") 
+       MESSAGE(WARNING "Open3D requires CMake version >=3.19 (current is ${CMAKE_VERSION})")
+    ELSE()
+       # Build Open3D like this to avoid linker errors in rtabmap:
+       # cmake -DBUILD_SHARED_LIBS=ON -DGLIBCXX_USE_CXX11_ABI=ON -DCMAKE_BUILD_TYPE=Release ..
+       find_package(Open3D QUIET)
+       IF(Open3D_FOUND)
+          MESSAGE(STATUS "Found Open3D: ${Open3DINCLUDE_DIRS}")
+       ENDIF(Open3D_FOUND)
+    ENDIF()
+ENDIF(WITH_OPEN3D)
+
 IF(WITH_LOAM)
     find_package(loam_velodyne QUIET)
     IF(loam_velodyne_FOUND)
@@ -695,7 +709,7 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
 IF(NOT MSVC)
-	IF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+	IF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)
 	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
 	 include(CheckCXXCompilerFlag)
 	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
@@ -705,7 +719,7 @@ IF(NOT MSVC)
 	  ELSE()
 	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
 	  ENDIF()
-	ENDIF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+	ENDIF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)
 
 	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
 	       G2O_FOUND OR
@@ -801,9 +815,9 @@ IF(NOT GTSAM_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
 ENDIF()
-IF(NOT CERES_FOUND)
+IF(NOT WITH_CERES OR NOT CERES_FOUND)
    SET(CERES "//")
-ENDIF(NOT CERES_FOUND)
+ENDIF(NOT WITH_CERES OR NOT CERES_FOUND)
 IF(NOT WITH_TORO)
    SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -821,6 +835,9 @@ ENDIF(NOT libpointmatcher_FOUND)
 IF(NOT CCCoreLib_FOUND)
    SET(CCCORELIB "//")
 ENDIF(NOT CCCoreLib_FOUND)
+IF(NOT Open3D_FOUND)
+   SET(OPEN3D "//")
+ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
@@ -965,7 +982,7 @@ ENDIF()
 IF(NOT TORCH_FOUND)
    SET(TORCH "//")
 ENDIF()
-IF(NOT Python3_FOUND)
+IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
    SET(PYTHON "//")
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
@@ -981,7 +998,7 @@ IF(NOT WITH_MADGWICK)
    SET(MADGWICK "//")
 ENDIF()
 
-CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
+CONFIGURE_FILE(Version.h.in ${CMAKE_CURRENT_BINARY_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
 
 ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
@@ -1025,7 +1042,8 @@ file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}
 file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
    
 # ... for the build tree
-set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
+set(CONF_INCLUDE_DIRS "${PROJECT_BINARY_DIR}/corelib/include"
+                      "${PROJECT_SOURCE_DIR}/corelib/include" 
                       "${PROJECT_SOURCE_DIR}/guilib/include" 
                       "${PROJECT_SOURCE_DIR}/utilite/include")
 set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
@@ -1236,7 +1254,7 @@ ELSE()
 MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
 ENDIF()
 
-IF(Python3_FOUND)
+IF(WITH_PYTHON AND Python3_FOUND)
 MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}            = YES (License: PSF)")
 ELSEIF(NOT WITH_PYTHON)
 MESSAGE(STATUS "  With Python3              = NO (WITH_PYTHON=OFF)")
@@ -1290,12 +1308,12 @@ ELSE()
 MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()
 
-IF(CERES_FOUND)
-MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = YES (License: BSD)")
+IF(WITH_CERES AND CERES_FOUND)
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}          = YES (License: BSD)")
 ELSEIF(NOT WITH_CERES)
-MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (WITH_CERES=OFF)")
+MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
 ELSE()
-MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (Ceres not found)")
+MESSAGE(STATUS " *With Ceres                = NO (Ceres not found)")
 ENDIF()
 
 IF(G2O_FOUND OR GTSAM_FOUND)
@@ -1332,6 +1350,16 @@ ELSE()
 MESSAGE(STATUS "  With CCCoreLib            = NO (CCCoreLib not found)")
 ENDIF()
 
+IF(Open3D_FOUND)
+MESSAGE(STATUS "  With Open3D               = YES (License: MIT)")
+ELSEIF(NOT WITH_OPEN3D)
+MESSAGE(STATUS "  With Open3D               = NO (WITH_OPEN3D=OFF)")
+ELSEIF(${CMAKE_VERSION} VERSION_LESS "3.19.0")
+MESSAGE(STATUS "  With Open3D               = NO (Open3D requires CMake>=3.19)")
+ELSE()
+MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)
@@ -1566,12 +1594,12 @@ ENDIF()
 MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")
 MESSAGE(STATUS "--------------------------------------------")
 
-IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
+IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT WITH_CERES AND NOT CERES_FOUND)
 MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of these options:
        g2o (https://github.com/RainerKuemmerle/g2o)
        GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
        Ceres (http://ceres-solver.org)
        set -DWITH_TORO=ON")
-ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
+ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT WITH_CERES AND NOT CERES_FOUND)
 
 # vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :

README.md

@@ -1,4 +1,4 @@
-rtabmap ![.](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap
 =======
 
 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)

Version.h.in

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CVSBA@#define RTABMAP_CVSBA
 @POINTMATCHER@#define RTABMAP_POINTMATCHER
 @CCCORELIB@#define RTABMAP_CCCORELIB
+@OPEN3D@#define RTABMAP_OPEN3D
 @FASTCV@#define RTABMAP_FASTCV
 @PDAL@#define RTABMAP_PDAL
 @LOAM@#define RTABMAP_LOAM

app/android/jni/CMakeLists.txt

@@ -3,6 +3,7 @@ SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}
     ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
     ${CMAKE_CURRENT_SOURCE_DIR}/third-party/include
+	${PROJECT_BINARY_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/utilite/include
     ${OpenCV_INCLUDE_DIRS}

app/android/jni/RTABMapApp.cpp

@@ -448,7 +448,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			poses,
 			links,
 			true,
-			true,
+			false, // Make sure poses are the same than optimized mesh (in case we switched RGBD/OptimizedFromGraphEnd)
 			&signatures,
 			true,
 			true,
@@ -1399,7 +1399,14 @@ int RTABMapApp::Render()
                     }
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && landmark!=0)
 					{
-						main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        if(rejected)
+                        {
+                            main_scene_.setBackgroundColor(0.5, 0.325f, 0); // dark orange
+                        }
+                        else
+                        {
+                            main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+                        }
 					}
 					else if(rtabmapThread_ && rtabmapThread_->isRunning() && rejected>0)
 					{

app/android/jni/scene.cpp

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

app/ios/RTABMapApp.xcodeproj/project.pbxproj

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

app/ios/RTABMapApp/Libraries/install_deps.sh

@@ -55,9 +55,11 @@ cd $pwd
 
 # FLANN
 echo "wget flann..."
-curl -L http://www.cs.ubc.ca/research/flann/uploads/FLANN/flann-1.8.4-src.zip -o flann-1.8.4-src.zip
-unzip -qq flann-1.8.4-src.zip
-cd flann-1.8.4-src
+git clone https://github.com/flann-lib/flann.git
+cd flann
+git checkout 1.8.4
+curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
+git apply flann_ios.patch
 mkdir build
 cd build
 # comment "add_subdirectory( test )" in top CMakeLists.txt
@@ -66,7 +68,7 @@ cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_O
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
-#rm -r flann-1.8.4-src.zip flann-1.8.4-src
+#rm -r flann
 
 # GTSAM
 git clone https://bitbucket.org/gtborg/gtsam.git
@@ -134,6 +136,8 @@ cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.4.2
+curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
+git apply opencv_ios.patch
 mkdir build
 cd build
 # add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
@@ -145,6 +149,10 @@ cd $pwd
 
 mkdir rtabmap
 cd rtabmap
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../..
+cmake -DANDROID_PREBUILD=ON ../../../../..
+cmake --build . --config Release
+mkdir ios
+cd ios
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install

app/ios/RTABMapApp/ViewController.swift

@@ -1340,6 +1340,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         rtabmap!.setMappingParameter(key: "Vis/FeatureType", value: defaults.string(forKey: "FeatureType")!);
         rtabmap!.setMappingParameter(key: "Mem/NotLinkedNodesKept", value: defaults.bool(forKey: "SaveAllFramesInDatabase") ? "true" : "false");
         rtabmap!.setMappingParameter(key: "RGBD/OptimizeFromGraphEnd", value: defaults.bool(forKey: "OptimizationfromGraphEnd") ? "true" : "false");
+        rtabmap!.setMappingParameter(key: "RGBD/MaxOdomCacheSize", value: defaults.string(forKey: "MaximumOdometryCacheSize")!);
         rtabmap!.setMappingParameter(key: "Optimizer/Strategy", value: defaults.string(forKey: "GraphOptimizer")!);
         rtabmap!.setMappingParameter(key: "RGBD/ProximityBySpace", value: defaults.string(forKey: "ProximityDetection")!);
 

app/ios/Settings.bundle/Mapping.plist

@@ -552,6 +552,50 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
+        <dict>
+            <key>Type</key>
+            <string>PSGroupSpecifier</string>
+            <key>FooterText</key>
+            <string>Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don't teleport to a location very similar to one we previously localized on.</string>
+        </dict>
+        <dict>
+            <key>Type</key>
+            <string>PSMultiValueSpecifier</string>
+            <key>Title</key>
+            <string>Maximum odometry cache size</string>
+            <key>Key</key>
+            <string>MaximumOdometryCacheSize</string>
+            <key>DefaultValue</key>
+            <string>10</string>
+            <key>Titles</key>
+            <array>
+                <string>500</string>
+                <string>200</string>
+                <string>100</string>
+                <string>75</string>
+                <string>50</string>
+                <string>40</string>
+                <string>30</string>
+                <string>20</string>
+                <string>10</string>
+                <string>5</string>
+                <string>Disabled</string>
+            </array>
+            <key>Values</key>
+            <array>
+                <string>500</string>
+                <string>200</string>
+                <string>100</string>
+                <string>75</string>
+                <string>50</string>
+                <string>40</string>
+                <string>30</string>
+                <string>20</string>
+                <string>10</string>
+                <string>5</string>
+                <string>0</string>
+            </array>
+        </dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>

app/ios/Settings.bundle/Root.plist

@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.12</string>
+			<string>0.20.16</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

app/src/CMakeLists.txt

@@ -4,6 +4,7 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+    ${PROJECT_BINARY_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/guilib/include

bin/.gitignore

@@ -1,5 +0,0 @@
-# Ignore everything in this directory
-*
-# Except this file
-!.gitignore
-!data

corelib/include/rtabmap/core/.gitignore

@@ -1 +0,0 @@
-/Version.h

corelib/include/rtabmap/core/Graph.h

@@ -133,7 +133,8 @@ std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
 		int to,
-		bool checkBothWays = true);
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
 std::multimap<int, int>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, int> & links,
 		int from,

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/OctoMap.h

@@ -74,6 +74,8 @@ public:
 	void expandNode();
 	bool createChild(unsigned int i);
 
+	void updateOccupancyTypeChildren();
+
 private:
 	int nodeRefId_;
 	int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground

corelib/include/rtabmap/core/Odometry.h

@@ -55,7 +55,8 @@ public:
 		kTypeMSCKF = 8,
 		kTypeVINS = 9,
 		kTypeOpenVINS = 10,
-		kTypeFLOAM = 11
+		kTypeFLOAM = 11,
+		kTypeOpen3D = 12
 	};
 
 public:

corelib/include/rtabmap/core/Parameters.h

@@ -202,7 +202,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, ImageKept,                   bool, false,    "Keep raw images in RAM.");
     RTABMAP_PARAM(Mem, BinDataKept,                 bool, true,     "Keep binary data in db.");
     RTABMAP_PARAM(Mem, RawDescriptorsKept,          bool, true,     "Raw descriptors kept in memory.");
-    RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
+    RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labeled as \"map#\" where # is the map ID.");
     RTABMAP_PARAM(Mem, SaveDepth16Format,           bool, false,    "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
     RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
     RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
@@ -373,7 +373,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
     RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
     RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
-    RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  0,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false) and when %s!=0. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  10,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
 
     // Local/Proximity loop closure detection
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
@@ -432,7 +432,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
 
     // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
     RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
     RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
     RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
@@ -572,6 +572,10 @@ class RTABMAP_EXP Parameters
     // Odometry VINS
     RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");
 
+    // Odometry Open3D
+    RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
+    RTABMAP_PARAM(OdomOpen3D, Method,           int, 0,  "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");
+
     // Common registration parameters
     RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
     RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -723,15 +727,15 @@ class RTABMAP_EXP Parameters
 #endif
 
     // Occupancy Grid
-    RTABMAP_PARAM(Grid, FromDepth,               bool,   true,    "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
+    RTABMAP_PARAM(Grid, Sensor,                  int,    1,       "Create occupancy grid from selected sensor: 0=laser scan, 1=depth image(s) or 2=both laser scan and depth image(s).");
     RTABMAP_PARAM(Grid, DepthDecimation,         unsigned int,  4, uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
     RTABMAP_PARAM(Grid, RangeMin,                float,  0.0,     "Minimum range from sensor.");
     RTABMAP_PARAM(Grid, RangeMax,                float,  5.0,     "Maximum range from sensor. 0=inf.");
-    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));
+    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s>=1] Region of interest ratios [left, right, top, bottom].", kGridSensor().c_str()));
     RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
     RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
     RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
-    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=0 or 2] Decimation of the laser scan before creating cloud.", kGridSensor().c_str()));
     RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
     RTABMAP_PARAM(Grid, PreVoxelFiltering,       bool,   true,    uFormat("Input cloud is downsampled by voxel filter (voxel size is \"%s\") before doing segmentation of obstacles and ground.", kGridCellSize().c_str()));
     RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
@@ -745,9 +749,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
 #ifdef RTABMAP_OCTOMAP
-    RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #else
-    RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #endif
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
@@ -830,7 +834,11 @@ public:
     static bool isFeatureParameter(const std::string & param);
     static ParametersMap getDefaultOdometryParameters(bool stereo = false, bool vis = true, bool icp = false);
     static ParametersMap getDefaultParameters(const std::string & group);
-    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);
+    /**
+     * If remove=false: keep only parameters of the specified group.
+     * If remove=true: remove parameters of the specified group.
+     */
+    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);
 
     static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
     static void writeINI(const std::string & configFile, const ParametersMap & parameters);

corelib/include/rtabmap/core/Rtabmap.h

@@ -361,7 +361,6 @@ private:
 	std::map<int, Transform> _globalScanMapPoses;
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
-	std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link
 	std::vector<float> _odomCorrectionAcc;
 
 	// Planning stuff

corelib/include/rtabmap/core/Statistics.h

@@ -147,6 +147,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_id,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
+	RTABMAP_STATS(Memory, Odom_cache_poses,);
+	RTABMAP_STATS(Memory, Odom_cache_links,);
 	RTABMAP_STATS(Memory, Small_movement,);
 	RTABMAP_STATS(Memory, Fast_movement,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
@@ -254,6 +256,8 @@ public:
 	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 	void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
 	void setWmState(const std::vector<int> & state) {_wmState = state;}
+	void setOdomCachePoses(const std::map<int, Transform> & poses) {_odomCachePoses = poses;}
+	void setOdomCacheConstraints(const std::multimap<int, Link> & constraints) {_odomCacheConstraints = constraints;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -281,6 +285,8 @@ public:
 	int currentGoalId() const {return _currentGoalId;}
 	const std::map<int, int> & reducedIds() const {return _reducedIds;}
 	const std::vector<int> & wmState() const {return _wmState;}
+	const std::map<int, Transform> & odomCachePoses() const {return _odomCachePoses;}
+	const std::multimap<int, Link> & odomCacheConstraints() const {return _odomCacheConstraints;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -316,6 +322,9 @@ private:
 
 	std::vector<int> _wmState;
 
+	std::map<int, Transform> _odomCachePoses;
+	std::multimap<int, Link> _odomCacheConstraints;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

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/OdometryOpen3D.h

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

corelib/src/CMakeLists.txt

@@ -93,6 +93,7 @@ SET(SRC_FILES
     odometry/OdometryMSCKF.cpp
     odometry/OdometryVINS.cpp
     odometry/OdometryOpenVINS.cpp
+    odometry/OdometryOpen3D.cpp
     
     IMU.cpp
     IMUThread.cpp
@@ -142,6 +143,7 @@ IF(MSVC)
 ENDIF(MSVC)
 
 SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_BINARY_DIR}/../include
     ${PROJECT_SOURCE_DIR}/utilite/include
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 	${CMAKE_CURRENT_SOURCE_DIR}
@@ -193,7 +195,7 @@ IF(TORCH_FOUND)
 	)
 ENDIF(TORCH_FOUND)
 
-IF(Python3_FOUND)
+IF(WITH_PYTHON AND Python3_FOUND)
 	SET(LIBRARIES
 		${LIBRARIES}
 		Python3::Python
@@ -209,7 +211,7 @@ IF(Python3_FOUND)
 	    ${CMAKE_CURRENT_SOURCE_DIR}/python
 		${INCLUDE_DIRS}
 	)
-ENDIF(Python3_FOUND)
+ENDIF(WITH_PYTHON AND Python3_FOUND)
 
 
 
@@ -420,7 +422,7 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)
 
-IF(CERES_FOUND)
+IF(WITH_CERES AND CERES_FOUND)
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
 		${CERES_INCLUDE_DIRS}
@@ -429,7 +431,7 @@ IF(CERES_FOUND)
 		${LIBRARIES}
 		${CERES_LIBRARIES}
 	)
-ENDIF(CERES_FOUND)
+ENDIF(WITH_CERES AND CERES_FOUND)
 
 IF(libpointmatcher_FOUND)
 	SET(INCLUDE_DIRS 
@@ -449,6 +451,13 @@ IF(CCCoreLib_FOUND)
 	)
 ENDIF(CCCoreLib_FOUND)
 
+IF(Open3D_FOUND)
+    SET(LIBRARIES
+        ${LIBRARIES}
+        Open3D::Open3D
+    )
+ENDIF(Open3D_FOUND)
+
 IF(FastCV_FOUND)
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
@@ -709,10 +718,10 @@ endforeach(arg ${RESOURCES})
 #MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
 #MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")
 
-IF(ANDROID)
+IF(ANDROID OR IOS)
 
    IF(NOT RTABMAP_RES_TOOL)
-      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
+      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${PROJECT_BINARY_DIR}/../bin)
       IF(NOT RTABMAP_RES_TOOL)
          MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
       ENDIF(NOT RTABMAP_RES_TOOL)
@@ -765,4 +774,11 @@ install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/
 		COMPONENT devel 
 		FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 
 		PATTERN ".svn" EXCLUDE)
+		
+# For generated Version.h
+install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/../include/ 
+        DESTINATION "${INSTALL_INCLUDE_DIR}"
+        COMPONENT devel 
+        FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 
+        PATTERN ".svn" EXCLUDE)
 

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/Features2d.cpp

@@ -2089,7 +2089,18 @@ std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat &
 {
 #ifdef RTABMAP_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	UASSERT_MSG(roi.x==0 && roi.y ==0, "Not supporting ROI");
+	if(roi.x!=0 || roi.y !=0)
+	{
+		UERROR("SuperPoint: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
+				roi.x, roi.y, roi.width, roi.height,
+				Parameters::kKpRoiRatios().c_str(),
+				Parameters::kVisRoiRatios().c_str(),
+				Parameters::kVisGridRows().c_str(),
+				Parameters::kVisGridCols().c_str(),
+				Parameters::kKpGridRows().c_str(),
+				Parameters::kKpGridCols().c_str());
+		return std::vector<cv::KeyPoint>();
+	}
 	return superPoint_->detect(image, mask);
 #else
 	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");

corelib/src/Graph.cpp

@@ -989,12 +989,13 @@ std::multimap<int, Link>::iterator findLink(
 		std::multimap<int, Link> & links,
 		int from,
 		int to,
-		bool checkBothWays)
+		bool checkBothWays,
+		Link::Type type)
 {
 	std::multimap<int, Link>::iterator iter = links.find(from);
 	while(iter != links.end() && iter->first == from)
 	{
-		if(iter->second.to() == to)
+		if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
 		{
 			return iter;
 		}
@@ -1007,7 +1008,7 @@ std::multimap<int, Link>::iterator findLink(
 		iter = links.find(to);
 		while(iter != links.end() && iter->first == to)
 		{
-			if(iter->second.to() == from)
+			if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
 			{
 				return iter;
 			}

corelib/src/Memory.cpp

@@ -2732,13 +2732,13 @@ Transform Memory::computeTransform(
 
 	// make sure we have all data needed
 	// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
-	if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
+	if(((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) && fromS.sensorData().imageCompressed().empty()) ||
 	   (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
 	{
 		fromS.sensorData() = getNodeData(fromS.id(), true, true, true, true);
 	}
-	if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
+	if(((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) && toS.sensorData().imageCompressed().empty()) ||
 	   (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
 	{
@@ -2748,27 +2748,27 @@ Transform Memory::computeTransform(
 	cv::Mat imgBuf, depthBuf, userBuf;
 	LaserScan laserBuf;
 	fromS.sensorData().uncompressData(
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&imgBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&depthBuf:0,
 			_registrationPipeline->isScanRequired()?&laserBuf:0,
 			_registrationPipeline->isUserDataRequired()?&userBuf:0);
 	toS.sensorData().uncompressData(
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
-			(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&imgBuf:0,
+			(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&depthBuf:0,
 			_registrationPipeline->isScanRequired()?&laserBuf:0,
 			_registrationPipeline->isUserDataRequired()?&userBuf:0);
 
 
 	// compute transform fromId -> toId
 	std::vector<int> inliersV;
-	if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
+	if((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) ||
 		(fromS.getWords().size() && toS.getWords().size()) ||
 		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
 	{
 		Signature tmpFrom = fromS;
 		Signature tmpTo = toS;
 
-		if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
+		if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
 		{
 			UDEBUG("");
 			tmpFrom.removeAllWords();
@@ -4666,6 +4666,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				if(!imagesRectified && decimatedData.cameraModels().size())
 				{
+					UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
 					std::vector<cv::KeyPoint> keypointsValid;
 					keypointsValid.reserve(keypoints.size());
 					cv::Mat descriptorsValid;
@@ -4859,6 +4860,144 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 		}
+		else if(!imagesRectified && !data.cameraModels().empty())
+		{
+			std::vector<cv::KeyPoint> keypointsValid;
+			keypointsValid.reserve(keypoints.size());
+			cv::Mat descriptorsValid;
+			descriptorsValid.reserve(descriptors.rows);
+			std::vector<cv::Point3f> keypoints3DValid;
+			keypoints3DValid.reserve(keypoints3D.size());
+
+			//undistort keypoints before projection (RGB-D)
+			if(data.cameraModels().size() == 1)
+			{
+				std::vector<cv::Point2f> pointsIn, pointsOut;
+				cv::KeyPoint::convert(keypoints,pointsIn);
+				if(data.cameraModels()[0].D_raw().cols == 6)
+				{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+					// Equidistant / FishEye
+					// get only k parameters (k1,k2,p1,p2,k3,k4)
+					cv::Mat D(1, 4, CV_64FC1);
+					D.at<double>(0,0) = data.cameraModels()[0].D_raw().at<double>(0,0);
+					D.at<double>(0,1) = data.cameraModels()[0].D_raw().at<double>(0,1);
+					D.at<double>(0,2) = data.cameraModels()[0].D_raw().at<double>(0,4);
+					D.at<double>(0,3) = data.cameraModels()[0].D_raw().at<double>(0,5);
+					cv::fisheye::undistortPoints(pointsIn, pointsOut,
+							data.cameraModels()[0].K_raw(),
+							D,
+							data.cameraModels()[0].R(),
+							data.cameraModels()[0].P());
+				}
+				else
+#else
+					UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+							CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+				}
+#endif
+				{
+					//RadialTangential
+					cv::undistortPoints(pointsIn, pointsOut,
+							data.cameraModels()[0].K_raw(),
+							data.cameraModels()[0].D_raw(),
+							data.cameraModels()[0].R(),
+							data.cameraModels()[0].P());
+				}
+				UASSERT(pointsOut.size() == keypoints.size());
+				for(unsigned int i=0; i<pointsOut.size(); ++i)
+				{
+					if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<data.cameraModels()[0].imageWidth() &&
+					   pointsOut.at(i).y>=0 && pointsOut.at(i).y<data.cameraModels()[0].imageHeight())
+					{
+						keypointsValid.push_back(keypoints.at(i));
+						keypointsValid.back().pt.x = pointsOut.at(i).x;
+						keypointsValid.back().pt.y = pointsOut.at(i).y;
+						descriptorsValid.push_back(descriptors.row(i));
+						if(!keypoints3D.empty())
+						{
+							keypoints3DValid.push_back(keypoints3D.at(i));
+						}
+					}
+				}
+			}
+			else
+			{
+				float subImageWidth;
+				if(!data.imageRaw().empty())
+				{
+					UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+					subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+				}
+				else
+				{
+					UASSERT(data.cameraModels()[0].imageWidth()>0);
+					subImageWidth = data.cameraModels()[0].imageWidth();
+				}
+
+				for(unsigned int i=0; i<keypoints.size(); ++i)
+				{
+					int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+					UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)data.cameraModels().size(),
+							uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+									cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str());
+
+					std::vector<cv::Point2f> pointsIn, pointsOut;
+					pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+					if(data.cameraModels()[cameraIndex].D_raw().cols == 6)
+					{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+						// Equidistant / FishEye
+						// get only k parameters (k1,k2,p1,p2,k3,k4)
+						cv::Mat D(1, 4, CV_64FC1);
+						D.at<double>(0,0) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
+						D.at<double>(0,1) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+						D.at<double>(0,2) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+						D.at<double>(0,3) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+						cv::fisheye::undistortPoints(pointsIn, pointsOut,
+								data.cameraModels()[cameraIndex].K_raw(),
+								D,
+								data.cameraModels()[cameraIndex].R(),
+								data.cameraModels()[cameraIndex].P());
+					}
+					else
+#else
+						UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+								CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+					}
+#endif
+					{
+						//RadialTangential
+						cv::undistortPoints(pointsIn, pointsOut,
+								data.cameraModels()[cameraIndex].K_raw(),
+								data.cameraModels()[cameraIndex].D_raw(),
+								data.cameraModels()[cameraIndex].R(),
+								data.cameraModels()[cameraIndex].P());
+					}
+
+					if(pointsOut[0].x>=0 && pointsOut[0].x<data.cameraModels()[cameraIndex].imageWidth() &&
+					   pointsOut[0].y>=0 && pointsOut[0].y<data.cameraModels()[cameraIndex].imageHeight())
+					{
+						keypointsValid.push_back(keypoints.at(i));
+						keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+						keypointsValid.back().pt.y = pointsOut[0].y;
+						descriptorsValid.push_back(descriptors.row(i));
+						if(!keypoints3D.empty())
+						{
+							keypoints3DValid.push_back(keypoints3D.at(i));
+						}
+					}
+				}
+			}
+
+			keypoints = keypointsValid;
+			descriptors = descriptorsValid;
+			keypoints3D = keypoints3DValid;
+
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+			UDEBUG("time rectification = %fs", t);
+		}
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
 		UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
@@ -5648,7 +5787,24 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
                 }
                 
             }
-			Link landmark(s->id(), landmarkId, Link::kLandmark, iter->second.pose(), iter->second.covariance().inv(), landmarkSize);
+
+			Transform landmarkPose = iter->second.pose();
+			if(_registrationPipeline->force3DoF())
+			{
+				// For 2D slam, make sure the landmark z axis is up
+				rtabmap::Transform tx = landmarkPose.rotation() * rtabmap::Transform(1,0,0,0,0,0);
+				rtabmap::Transform ty = landmarkPose.rotation() * rtabmap::Transform(0,1,0,0,0,0);
+				if(fabs(tx.z()) > 0.9)
+				{
+					landmarkPose*=rtabmap::Transform(0,0,0,0,(tx.z()>0?1:-1)*M_PI/2,0);
+				}
+				else if(fabs(ty.z()) > 0.9)
+				{
+					landmarkPose*=rtabmap::Transform(0,0,0,(ty.z()>0?-1:1)*M_PI/2,0,0);
+				}
+			}
+
+			Link landmark(s->id(), landmarkId, Link::kLandmark, landmarkPose, iter->second.covariance().inv(), landmarkSize);
 			s->addLandmark(landmark);
 
 			// Update landmark index

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

corelib/src/OctoMap.cpp

@@ -106,6 +106,23 @@ bool RtabmapColorOcTreeNode::createChild(unsigned int i) {
 #endif
 }
 
+void RtabmapColorOcTreeNode::updateOccupancyTypeChildren()
+{
+	if (children != NULL){
+		int type = kTypeUnknown;
+		for (int i=0; i<8 && type != kTypeObstacle; i++) {
+			RtabmapColorOcTreeNode* child = static_cast<RtabmapColorOcTreeNode*>(children[i]);
+
+			if (child != NULL && child->getOccupancyType() >= kTypeEmpty) {
+				if(type == kTypeUnknown) {
+					type = child->getOccupancyType();
+				}
+			}
+		}
+		type_ = type;
+	}
+}
+
 RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
 	: OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
 	RtabmapColorOcTreeMemberInit.ensureLinking();
@@ -231,6 +248,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
 		}
 		node->updateOccupancyChildren();
 		node->updateColorChildren();
+		node->updateOccupancyTypeChildren();
 	}
 #else
 	// only recurse and update for inner nodes:
@@ -245,6 +263,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
 	  }
 	  node->updateOccupancyChildren();
 	  node->updateColorChildren();
+	  node->updateOccupancyTypeChildren();
 	}
 #endif
 }
@@ -1209,21 +1228,23 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	int oi=0;
 	cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0);
 	cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0);
+	float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
 	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
 		octomap::point3d pt = octree_->keyToCoord(it.getKey());
-		if(octree_->isNodeOccupied(*it) && it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
+		if(octree_->isNodeOccupied(*it) &&
+		   it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
 		{
 			// projected on ground
-			oPtr[oi][0] = pt.x();
-			oPtr[oi][1] = pt.y();
+			oPtr[oi][0] = pt.x()-halfCellSize;
+			oPtr[oi][1] = pt.y()-halfCellSize;
 			++oi;
 		}
 		else
 		{
 			// projected on ground
-			gPtr[gi][0] = pt.x();
-			gPtr[gi][1] = pt.y();
+			gPtr[gi][0] = pt.x()-halfCellSize;
+			gPtr[gi][1] = pt.y()-halfCellSize;
 			++gi;
 		}
 	}

corelib/src/Odometry.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINS.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
+#include "rtabmap/core/odometry/OdometryOpen3D.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -103,6 +104,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeOpenVINS:
 		odometry = new OdometryOpenVINS(parameters);
 		break;
+	case Odometry::kTypeOpen3D:
+		odometry = new OdometryOpen3D(parameters);
+		break;
 	default:
 		UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
 		odometry = new OdometryF2M(parameters);
@@ -303,9 +307,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation*
 					data.imu().localTransform().rotation().inverse();
 
-			IMU imu2 = data.imu();
-			imu2.convertToBaseFrame();
-
 			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
 				this->framesProcessed() == 0)
 			{

corelib/src/Optimizer.cpp

@@ -216,15 +216,15 @@ void Optimizer::getConnectedGraph(
 
 	while(nextPoses.size())
 	{
-		int fromId = *nextPoses.rbegin(); // fill up all nodes before landmarks
+		int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
 		nextPoses.erase(*nextPoses.rbegin());
 
 		if(posesOut.empty())
 		{
-			posesOut.insert(std::make_pair(fromId, posesIn.find(fromId)->second));
+			posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
 
 			// add prior links
-			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
+			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
 			{
 				if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
 				{
@@ -233,12 +233,12 @@ void Optimizer::getConnectedGraph(
 			}
 		}
 
-		for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
+		for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
 		{
 			int toId = iter->second;
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
-				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
+				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
 				if(nextPoses.find(toId) == nextPoses.end())
 				{
 					if(!uContains(posesOut, toId))
@@ -246,7 +246,7 @@ void Optimizer::getConnectedGraph(
 						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
 						{
 							Transform t;
-							if(kter->second.from()==fromId)
+							if(kter->second.from()==currentId)
 							{
 								t = kter->second.transform();
 							}
@@ -254,11 +254,11 @@ void Optimizer::getConnectedGraph(
 							{
 								t = kter->second.transform().inverse();
 							}
-							posesOut.insert(std::make_pair(toId, (posesOut.at(fromId) * t).to3DoF()));
+							posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
 						}
 						else
 						{
-							Transform t = posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse());
+							Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
 							posesOut.insert(std::make_pair(toId, t));
 						}
 						// add prior links
@@ -274,7 +274,7 @@ void Optimizer::getConnectedGraph(
 					}
 
 					// only add unique links
-					if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
+					if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
 					{
 						if(kter->second.to() < 0)
 						{

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

corelib/src/Signature.cpp

@@ -118,7 +118,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
 }
 void Signature::addLink(const Link & link)
 {
-	UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
+	UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
 	UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());

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

@@ -178,8 +178,6 @@ void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
 void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
 {
 #ifdef RTABMAP_OPENNI2
-	UASSERT(horizontal >= 0);
-	UASSERT(vertical >= 0);
 	_depthHShift = horizontal;
 	_depthVShift = vertical;
 #endif
@@ -538,10 +536,19 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 
 				if(_type==kTypeColorDepth)
 				{
-					if (_depthHShift > 0 || _depthVShift > 0)
+					if (_depthHShift != 0 || _depthVShift != 0)
 					{
 						cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
-						depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
+						depth(cv::Rect(
+								_depthHShift>0?_depthHShift:0,
+								_depthVShift>0?_depthVShift:0,
+								depth.cols - abs(_depthHShift),
+								depth.rows - abs(_depthVShift))).copyTo(
+							   out(cv::Rect(
+								_depthHShift<0?-_depthHShift:0,
+								_depthVShift<0?-_depthVShift:0,
+								depth.cols - abs(_depthHShift),
+								depth.rows - abs(_depthVShift))));
 						depth = out;
 					}
 

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;
-						rgbIntrinsics_ = intrinsic;
+						if(odometryOnlyLeftStream_)
+						{
+							auto intrinsic = video_profile.get_intrinsics();
+							UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f",
+									intrinsic.width, intrinsic.height,
+									intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
+									intrinsic.model,
+									intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3]);
+							cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
+							K.at<double>(0,0) = intrinsic.fx;
+							K.at<double>(1,1) = intrinsic.fy;
+							K.at<double>(0,2) = intrinsic.ppx;
+							K.at<double>(1,2) = intrinsic.ppy;
+							UASSERT(intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4); // we expect fisheye 4 values
+							cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+							D.at<double>(0,0) = intrinsic.coeffs[0];
+							D.at<double>(0,1) = intrinsic.coeffs[1];
+							D.at<double>(0,4) = intrinsic.coeffs[2];
+							D.at<double>(0,5) = intrinsic.coeffs[3];
+							cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+							P.at<double>(0,0) = intrinsic.fx;
+							P.at<double>(1,1) = intrinsic.fy;
+							P.at<double>(0,2) = intrinsic.ppx;
+							P.at<double>(1,2) = intrinsic.ppy;
+							cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+							model_ = CameraModel(camera_name, cv::Size(intrinsic.width, intrinsic.height), K, D, R, P, this->getLocalTransform());
+							if(rectifyImages_)
+								model_.initRectificationMap();
+						}
 					}
-					else
+					else if(!odometryOnlyLeftStream_)
 					{
 						// RIGHT FISHEYE
 						depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						depthIntrinsics_ = intrinsic;
 					}
 					added = true;
 				}
@@ -961,7 +980,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 {
 			 serial = cameraName;
 		 }
-		if(!calibrationFolder.empty() && !serial.empty())
+		if(!odometryImagesDisabled_ &&
+		   !odometryOnlyLeftStream_ &&
+		   !calibrationFolder.empty() && !serial.empty())
 		{
 			if(!stereoModel_.load(calibrationFolder, serial, false))
 			{
@@ -1005,7 +1026,10 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 
 			Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 			this->setLocalTransform(this->getLocalTransform() * opticalTransform.inverse());
-			stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			if(odometryOnlyLeftStream_)
+				model_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			else
+				stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
 			imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
 
 			if(odometryImagesDisabled_)
@@ -1027,9 +1051,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
 			 imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
 			 UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
-			 stereoModel_.setLocalTransform(this->getLocalTransform());
+			 if(odometryOnlyLeftStream_)
+				 model_.setLocalTransform(this->getLocalTransform());
+			 else
+				 stereoModel_.setLocalTransform(this->getLocalTransform());
 		}
-		if(rectifyImages_ && !stereoModel_.isValidForRectification())
+		if(!odometryImagesDisabled_ && rectifyImages_ && !model_.isValidForRectification() && !stereoModel_.isValidForRectification())
 		{
 			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 			return false;
@@ -1192,6 +1219,7 @@ void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
 	{
 		odometryProvided_ = true;
 		odometryImagesDisabled_ = false;
+		odometryOnlyLeftStream_ = false;
 	}
 #endif
 }
@@ -1210,7 +1238,7 @@ void CameraRealSense2::setImagesRectified(bool enabled)
 #endif
 }
 
-void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
+void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, bool onlyLeftStream)
 {
 #ifdef RTABMAP_REALSENSE2
 	if(dualMode_ && !enabled)
@@ -1220,6 +1248,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
 	}
 	odometryProvided_ = enabled;
 	odometryImagesDisabled_ = enabled && imageStreamsDisabled;
+	odometryOnlyLeftStream_ = enabled && !imageStreamsDisabled && onlyLeftStream;
 #endif
 }
 
@@ -1374,27 +1403,48 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
 				}
 			}
-			else if(is_left_fisheye_arrived && is_right_fisheye_arrived)
+			else if(is_left_fisheye_arrived)
 			{
-				auto from_image_frame = depth_frame.as<rs2::video_frame>();
-				cv::Mat left,right;
-				if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+				if(odometryOnlyLeftStream_)
 				{
-					left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
-					right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
-				}
-				else
-				{
-					left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
-					right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
-				}
+					cv::Mat left;
+					if(rectifyImages_ && model_.isValidForRectification())
+					{
+						left = model_.rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+					}
+					else
+					{
+						left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+					}
 
-				if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
-				{
-					stereoModel_.setImageSize(left.size());
-				}
+					if(model_.imageHeight() == 0 || model_.imageWidth() == 0)
+					{
+						model_.setImageSize(left.size());
+					}
 
-				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+					data = SensorData(left, cv::Mat(), model_, this->getNextSeqID(), stamp);
+				}
+				else if(is_right_fisheye_arrived)
+				{
+					cv::Mat left,right;
+					if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+					{
+						left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+						right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
+					}
+					else
+					{
+						left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+						right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
+					}
+
+					if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+					{
+						stereoModel_.setImageSize(left.size());
+					}
+
+					data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+				}
 			}
 			else
 			{
@@ -1471,6 +1521,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				lastImuStamp_ = imuStamp;
 			}
 		}
+		else if(frameset.size()==1 && frameset[0].get_profile().stream_type() == RS2_STREAM_FISHEYE)
+		{
+			UERROR("Missing frames (received %d, needed=%d). For T265 camera, "
+					"either use realsense sdk v2.42.0, or apply "
+					"this patch (https://github.com/IntelRealSense/librealsense/issues/9030#issuecomment-962223017) "
+					"to fix this problem.", (int)frameset.size(), desiredFramesetSize);
+		}
 		else
 		{
 			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);

corelib/src/camera/CameraStereoImages.cpp

@@ -166,6 +166,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
 	{
 		if(camera2_)
 		{
+			camera2_->setBayerMode(this->getBayerMode());
 			right = camera2_->takeImage(info);
 		}
 		else

corelib/src/odometry/OdometryOpen3D.cpp

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

corelib/src/optimizer/OptimizerG2O.cpp

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

corelib/src/optimizer/OptimizerGTSAM.cpp

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

corelib/src/util3d.cpp

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

corelib/src/util3d_mapping.cpp

@@ -330,8 +330,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 	{
 		//Get map size
 		float margin = cellSize*10.0f;
-		xMin = minX-margin;
-		yMin = minY-margin;
+		xMin = minX-margin-cellSize/2.0f;
+		yMin = minY-margin-cellSize/2.0f;
 		float xMax = maxX+margin;
 		float yMax = maxY+margin;
 		if(fabs((yMax - yMin) / cellSize) > 30000 || // Max 1.5Km/1.5Km at 5 cm/cell -> 900MB