icpCC: when force3DoF is true, set z to 0 in data conversion

* Refactored RegistrationIcp so that libpointmatcher yaml can work with icp odometry (we can then avoid refiltering data with local map of F2M). All data filtering (including libpointmatcher DataFilters) are done at the beginning of the function.

* ICP: Restored ref and data scans order for libpointmatcher (seems more stable this way).

* Fixed compilation error without libpointmatcher

* CCCoreLib integration (Icp/Strategy=2). Icp/PMForce4DoF is now Icp/Force4DoF. Icp/PM is now Icp/Strategy. Icp/PMOutlierRatio is now Icp/OutlierRatio.

* Fixed build without CCCoreLib

* Cleanup RegistrationIcp from third party functions.

* Preferences: disable libpointmatcher and cccorlib options if not available

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 8)
+SET(RTABMAP_PATCH_VERSION 9)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -180,13 +180,16 @@ option(WITH_CERES         "Include Ceres support"                ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
+option(WITH_CCCORELIB     "Include CCCoreLib support"            ON)
 option(WITH_LOAM          "Include LOAM support"                 ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
+option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
 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_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
 option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
@@ -194,7 +197,7 @@ 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_SLAM2     "Include ORB_SLAM2 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)
@@ -411,7 +414,14 @@ IF(WITH_G2O)
     FIND_PACKAGE(G2O QUIET)
     IF(G2O_FOUND)
        MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
-    ENDIF(G2O_FOUND)
+    ELSE()
+       FIND_PACKAGE(g2o QUIET)
+       IF(g2o_FOUND)
+	  SET(G2O_FOUND ${g2o_FOUND})
+	  SET(G2O_CPP11 1)
+	  MESSAGE(STATUS "Found g2o (targets)")
+       ENDIF(g2o_FOUND)
+    ENDIF()
 ENDIF(WITH_G2O)
 
 IF(WITH_GTSAM)
@@ -451,6 +461,13 @@ IF(libpointmatcher_FOUND OR GTSAM_FOUND)
     ENDIF(WIN32)
 ENDIF(libpointmatcher_FOUND OR GTSAM_FOUND)
 
+IF(WITH_CCCORELIB)
+    find_package(CCCoreLib QUIET)
+    IF(CCCoreLib_FOUND)
+       MESSAGE(STATUS "Found CCCoreLib: ${CCCoreLib_INCLUDE_DIRS}")
+    ENDIF(CCCoreLib_FOUND)
+ENDIF(WITH_CCCORELIB)
+
 IF(WITH_LOAM)
     find_package(loam_velodyne QUIET)
     IF(loam_velodyne_FOUND)
@@ -474,6 +491,20 @@ IF(WITH_ZED)
     ENDIF(ZED_FOUND)
 ENDIF(WITH_ZED)
 
+IF(WITH_ZEDOC)
+    find_package(ZEDOC QUIET)
+    IF(ZEDOC_FOUND)
+       MESSAGE(STATUS "Found ZED Open Capture: ${ZEDOC_INCLUDE_DIRS}")
+       ## look for HIDAPI
+       find_package(HIDAPI)
+       IF(HIDAPI_FOUND)
+          MESSAGE(STATUS "Found HIDAPI: ${HIDAPI_INCLUDE_DIRS}")
+       ELSE()
+          MESSAGE(FATAL_ERROR "HIDAPI is required to build with Zed Open Capture! Set -DWITH_ZEDOC=OFF if you don't have HIDAPI.")
+       ENDIF()
+    ENDIF(ZEDOC_FOUND)
+ENDIF(WITH_ZEDOC)
+
 IF(WITH_REALSENSE)
     IF(WITH_REALSENSE_SLAM)
        FIND_PACKAGE(RealSense QUIET COMPONENTS slam)
@@ -506,6 +537,13 @@ IF(WITH_MYNTEYE)
     ENDIF(mynteye_FOUND)
 ENDIF(WITH_MYNTEYE)
 
+IF(WITH_DEPTHAI)
+    FIND_PACKAGE(depthai 2 QUIET)
+    IF(depthai_FOUND)
+       MESSAGE(STATUS "Found depthai-core (targets)")
+    ENDIF(depthai_FOUND)
+ENDIF(WITH_DEPTHAI)
+
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(octomap QUIET)
     IF(octomap_FOUND)
@@ -604,59 +642,53 @@ IF(WITH_FASTCV)
     ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)
 
-IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
+IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
-    FIND_PACKAGE(ORB_SLAM2 QUIET)
+    FIND_PACKAGE(ORB_SLAM QUIET)
-    IF(ORB_SLAM2_FOUND)
+    IF(ORB_SLAM_FOUND)
-       MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
+       MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_INCLUDE_DIRS}")
-       FIND_PACKAGE(Pangolin QUIET)
+    ENDIF(ORB_SLAM_FOUND)
-       IF(NOT Pangolin_FOUND)
+ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
-          SET(ORB_SLAM2_FOUND FALSE)
-          MESSAGE(STATUS "Found ORB_SLAM2 but not Pangolin, disabling ORB_SLAM2.")
-       ELSE()
-          MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
-          SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
-          SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
-       ENDIF()
-    ENDIF(ORB_SLAM2_FOUND)
-ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
-IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND)
+IF(NOT MSVC)
-   #LOAM and PCL>=1.10 require c++14
+	IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
-   IF(NOT MSVC)
+	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
-     include(CheckCXXCompilerFlag)
+	 include(CheckCXXCompilerFlag)
-      CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-      IF(COMPILER_SUPPORTS_CXX14)
+	  IF(COMPILER_SUPPORTS_CXX14)
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-      ELSE()
+	    set(CMAKE_CXX_STANDARD 14)
-        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM (set \"-DWITH_LOAM=OFF\" to build without LOAM).")
+	  ELSE()
-      ENDIF()
+	    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()
-ELSEIF(G2O_FOUND OR 
+	ENDIF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
-       GTSAM_FOUND OR 
+
-       CERES_FOUND OR
+	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
-       ZED_FOUND OR 
+	       G2O_FOUND OR
-       ANDROID OR 
+	       GTSAM_FOUND OR
-       RealSense_FOUND OR 
+	       CERES_FOUND OR
-       realsense2_FOUND OR 
+	       ZED_FOUND OR
-       ORB_SLAM2_FOUND OR 
+	       ZEDOC_FOUND OR
-       okvis_FOUND OR 
+	       ANDROID OR
-       open_chisel_FOUND OR 
+	       RealSense_FOUND OR
-       msckf_vio_FOUND OR 
+	       realsense2_FOUND OR
-       vins_FOUND OR 
+	       ORB_SLAM_FOUND OR
-       libpointmatcher_FOUND)
+	       okvis_FOUND OR
-   #Newest versions require std11
+	       open_chisel_FOUND OR
-   IF(NOT MSVC)
+	       msckf_vio_FOUND OR
-     include(CheckCXXCompilerFlag)
+	       vins_FOUND OR
-      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+	       libpointmatcher_FOUND))
-      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+	 #Newest versions require std11
-      IF(COMPILER_SUPPORTS_CXX11)
+	 include(CheckCXXCompilerFlag)
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-      ELSEIF(COMPILER_SUPPORTS_CXX0X)
+	  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+	  IF(COMPILER_SUPPORTS_CXX11)
-      ELSE()
+	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+	  ELSEIF(COMPILER_SUPPORTS_CXX0X)
-      ENDIF()
+	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-   ENDIF()
+	  ELSE()
+	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+	  ENDIF()
+	ENDIF()
 ENDIF()
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
@@ -740,6 +772,9 @@ ENDIF()
 IF(NOT libpointmatcher_FOUND)
    SET(POINTMATCHER "//")
 ENDIF(NOT libpointmatcher_FOUND)
+IF(NOT CCCoreLib_FOUND)
+   SET(CCCORELIB "//")
+ENDIF(NOT CCCoreLib_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
@@ -789,6 +824,11 @@ IF(NOT ZED_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
 ENDIF()
+IF(NOT ZEDOC_FOUND)
+   SET(ZEDOC "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZEDOC_LIBRARIES}) 
+ENDIF()
 IF(NOT RealSense_FOUND)
    SET(REALSENSE "//")
 ELSE()
@@ -805,6 +845,9 @@ ENDIF()
 IF(NOT mynteye_FOUND)
    SET(MYNTEYE "//")
 ENDIF(NOT mynteye_FOUND)
+IF(NOT depthai_FOUND)
+   SET(DEPTHAI "//")
+ENDIF(NOT depthai_FOUND)
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
 ELSE()
@@ -853,10 +896,10 @@ IF(NOT vins_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
 ENDIF()
-IF(NOT ORB_SLAM2_FOUND)
+IF(NOT ORB_SLAM_FOUND)
-   SET(ORB_SLAM2 "//")
+   SET(ORB_SLAM "//")
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) 
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM_LIBRARIES})
 ENDIF()
 IF(NOT WITH_ORB_OCTREE)
    SET(ORB_OCTREE "//")
@@ -1083,12 +1126,20 @@ IF(OpenCV_FOUND)
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
       IF(NONFREE STREQUAL "//")
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+        IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+        ELSE()
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: Apache 2)")
+        ENDIF()
       ELSE()
         MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
       ENDIF()
     ELSE()
-      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
+      IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
+      ELSE()
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: Apache 2)")
+      ENDIF()
     ENDIF()
   ENDIF()
 ENDIF(OpenCV_FOUND)
@@ -1214,6 +1265,14 @@ ELSE()
 MESSAGE(STATUS " *With libpointmatcher      = NO (libpointmatcher not found)")
 ENDIF()
 
+IF(CCCoreLib_FOUND)
+MESSAGE(STATUS " *With CCCoreLib            = YES (License: GPLv2)")
+ELSEIF(NOT WITH_POINTMATCHER)
+MESSAGE(STATUS " *With CCCoreLib            = NO (WITH_CCCORELIB=OFF)")
+ELSE()
+MESSAGE(STATUS " *With CCCoreLib            = NO (CCCoreLib not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)
@@ -1314,6 +1373,14 @@ ELSE()
 MESSAGE(STATUS "  With ZED                  = NO (ZED sdk and/or cuda not found)")
 ENDIF()
 
+IF(ZEDOC_FOUND)
+MESSAGE(STATUS "  With ZEDOC                = YES")
+ELSEIF(NOT WITH_ZEDOC)
+MESSAGE(STATUS "  With ZEDOC                = NO (WITH_ZEDOC=OFF)")
+ELSE()
+MESSAGE(STATUS "  With ZEDOC                = NO (ZED Open Capture not found)")
+ENDIF()
+
 IF(RealSense_FOUND)
 MESSAGE(STATUS "  With RealSense            = YES (License: Apache-2)")
 IF(RealSenseSlam_FOUND)
@@ -1345,6 +1412,14 @@ ELSE()
 MESSAGE(STATUS "  With MyntEyeS             = NO (mynteye s sdk not found)")
 ENDIF()
 
+IF(depthai_FOUND)
+MESSAGE(STATUS "  With DepthAI              = YES (License: MIT)")
+ELSEIF(NOT WITH_DEPTHAI)
+MESSAGE(STATUS "  With DepthAI              = NO (WITH_DEPTHAI=OFF)")
+ELSE()
+MESSAGE(STATUS "  With DepthAI              = NO (depthai-core not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Odometry Approaches:")
 IF(loam_velodyne_FOUND)
@@ -1403,14 +1478,14 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
-IF(ORB_SLAM2_FOUND)
+IF(ORB_SLAM_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
+MESSAGE(STATUS "  With ORB_SLAM${ORB_SLAM_VERSION}            = YES (License: GPLv3)")
-ELSEIF(NOT WITH_ORB_SLAM2)
+ELSEIF(NOT WITH_ORB_SLAM)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_ORB_SLAM2=OFF)")
+MESSAGE(STATUS "  With ORB_SLAM             = NO (WITH_ORB_SLAM=OFF)")
 ELSEIF(G2O_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
+MESSAGE(STATUS "  With ORB_SLAM             = NO (WITH_G2O should be OFF as ORB_SLAM uses its own g2o version)")
 ELSE()
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
+MESSAGE(STATUS "  With ORB_SLAM             = NO (ORB_SLAM2 and ORB_SLAM3 not found, make sure environment variable ORB_SLAM_ROOT_DIR is set)")
 ENDIF()
 
 MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")

Version.h.in

@@ -51,16 +51,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @K4A@#define RTABMAP_K4A
 @CVSBA@#define RTABMAP_CVSBA
 @POINTMATCHER@#define RTABMAP_POINTMATCHER
+@CCCORELIB@#define RTABMAP_CCCORELIB
 @FASTCV@#define RTABMAP_FASTCV
 @PDAL@#define RTABMAP_PDAL
 @LOAM@#define RTABMAP_LOAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
+@ZEDOC@#define RTABMAP_ZEDOC
 @REALSENSE@#define RTABMAP_REALSENSE
 @REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
 @REALSENSE2@#define RTABMAP_REALSENSE2
 @MYNTEYE@#define RTABMAP_MYNTEYE
+@DEPTHAI@#define RTABMAP_DEPTHAI
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
 @ALICE_VISION@#define RTABMAP_ALICE_VISION
@@ -71,7 +74,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @OKVIS@#define RTABMAP_OKVIS
 @MSCKF_VIO@#define RTABMAP_MSCKF_VIO
 @VINS@#define RTABMAP_VINS
-@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
+@ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE
 @TORCH@#define RTABMAP_TORCH
 @PYTHON@#define RTABMAP_PYTHON

app/android/jni/RTABMapApp.cpp

@@ -2901,7 +2901,7 @@ bool RTABMapApp::exportMesh(
 						// save in database
 						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
-						cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
+						cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
 						std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
 						polygons[0].resize(polygonMesh->polygons.size());
 						for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p)
@@ -2918,7 +2918,7 @@ bool RTABMapApp::exportMesh(
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
-					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
+					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
 
 					// save in database
 					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
@@ -3054,7 +3054,7 @@ bool RTABMapApp::exportMesh(
 
 				// save in database
 				{
-					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds)); // for database
+					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds).data()); // for database
 					boost::mutex::scoped_lock  lock(rtabmapMutex_);
 					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat);
 					success = true;

app/src/CMakeLists.txt

@@ -1,19 +1,6 @@
 
-### Qt Gui stuff ###
-SET(headers_ui 
-	./ObjDeletionHandler.h
-)
-
-#This will generate moc_* for Qt
-IF(QT4_FOUND)
-    QT4_WRAP_CPP(moc_srcs ${headers_ui})
-ELSE()
-    QT5_WRAP_CPP(moc_srcs ${headers_ui})
-ENDIF()
-
 SET(SRC_FILES
     main.cpp
-    ${moc_srcs} 
 )
 
 SET(INCLUDE_DIRS

app/src/main.cpp

@@ -35,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UObjDeletionThread.h"
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UConversion.h"
-#include "ObjDeletionHandler.h"
 
 #ifdef RTABMAP_PYTHON
 #include "rtabmap/core/PythonInterface.h"

archive/2020-IlluminationInvariant/README.md

@@ -0,0 +1,22 @@
+
+## Multi-Session Visual SLAM for Illumination Invariant Localization in Indoor Environments
+
+* Paper: https://arxiv.org/abs/2103.03827
+
+* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
+![Overview](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_overview.jpg)]
+
+
+## Description
+
+This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using Multi-Session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg)]
+
+The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg)]
+
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_map_merged_999.jpg)]
+

archive/2020-IlluminationInvariant/results_common.m

@@ -0,0 +1,135 @@
+
+clear all
+close all
+
+pkg load signal
+
+# rtabmap-report --loc 32 Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms . Keypoint/Current_frame/words
+# Right-click on thr legend of the figure, copy all data to clipboard
+# Paste in correction#.txt, inliers#.txt and time#.txt where # is the 
+# number of the descriptor used
+
+skipFrameDir = '0';
+prefix = 'Stat';
+RAMaddOverhead = 1;
+% Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words'
+% Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1)
+statNames = {'Loop/Odom_correction_norm/m', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/'}; % 'Odometry_average'
+
+datasets = [ 0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
+sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
+
+allCumResults = {};
+allMaxResults = {};
+
+for s=1:length(statNames)
+
+avgResults = {};
+maxResults = {};
+totalResults = {};
+absResults = {};
+
+statName = strrep(statNames{s},'/','-');
+
+for d=1:length(datasets)
+
+if strcmp(statName,'Inliers_ratio_%')
+  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  dataWords = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data(:, 2:end) = data(:, 2:end) ./ dataWords(:, 2:end) * 100;
+elseif strcmp(statName, 'Odometry_average')
+  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+else
+  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
+endif
+sessions = size(data,2)-1;
+
+avgResultsTmp = zeros(sessions, length(sep)-1);
+maxResultsTmp = zeros(sessions, length(sep)-1);
+totalResultsTmp = zeros(sessions, length(sep)-1);
+absResultsTmp = zeros(sessions, length(sep)-1);
+
+for i = 1:sessions
+  for j = 1:length(sep)-1
+    x = data(:,1);
+    y = data(:,i+1);
+    y = y(x>=sep(j) & x<=sep(j+1), :);
+    x = x(x>=sep(j) & x<=sep(j+1), :);
+    if strcmp(statName, 'Odometry_average')
+      y(2:end) = y(2:end) - y(1:end-1);
+      y(y < 0.05) = 0;
+    elseif strcmp(statName, 'Loop-Map_id-')
+      y = y+1;
+      y(y>0) = 1;
+    end
+    if strcmp(statName, 'Memory-RAM_estimated-MB') && RAMaddOverhead == 1
+      % Valgrind estimated around 90 MB constant overhead
+      y = y + 90;
+      if datasets(d) == 7
+        %% 135 MB overhead for BRISK kernel
+        y = y + 135;
+      elseif datasets(d) == 11
+        %% 645 MB (library cuda) + 800 MB (network) for SuperPoint
+        y = y + 645+800;
+      elseif datasets(d) == 13 || datasets(d) == 14
+        %% 64 MB overhead for DAISY
+        y = y + 64;
+      endif
+    endif
+    nonzeros = y(y>0);
+    if strcmp(statName, 'Loop-Map_id-')
+      nonzeros = y;
+    end
+    if length(nonzeros) > 0
+      avgValue = sum(nonzeros)/length(nonzeros);
+      avgResultsTmp(i,j) = avgValue;
+      maxResultsTmp(i,j) = max(nonzeros);
+      totalResultsTmp(i,j) = length(nonzeros);
+      absResultsTmp(i,j) = sum(nonzeros);
+    endif
+  endfor
+endfor
+avgResults{1,d} = avgResultsTmp;
+maxResults{1,d} = maxResultsTmp;
+totalResults{1,d} = totalResultsTmp;
+absResults{1,d} = absResultsTmp;
+endfor
+
+% compute cumulative results
+cumResults = zeros(sessions+2, length(datasets)+1);
+for d=1:length(datasets)
+  cumResults(1,d+1) = datasets(d);
+  if sum(totalResults{1,d}, 2)
+    cumResults(2:end-1,d+1) = sum(absResults{1,d}, 2) ./ sum(totalResults{1,d}, 2);
+  endif
+  cumResults(end,d+1) = sum(sum(absResults{1,d}(1:6,1:6).*eye(6,6))) / sum(sum(totalResults{1,d}(1:6,1:6).*eye(6,6)));
+end
+cumResults(2:end-1,1) = 1:sessions;
+
+allCumResults{1,s} = statNames{s};
+if strcmp(statNames{s}, 'Loop/Odom_correction_norm/m')
+  cumResults(2:end,2:end) = cumResults(2:end,2:end) * 1000;
+  allCumResults{1,s} = 'Loop/Odom_correction_norm/mm';
+elseif strcmp(statNames{s}, 'Loop/Map_id/')
+  cumResults(2:end,2:end) = cumResults(2:end,2:end) * 100;
+endif
+allCumResults{2,s} = round(cumResults);
+
+% compute max results
+cumMaxResults = zeros(sessions+2, length(datasets)+1);
+for d=1:length(datasets)
+  cumMaxResults(1,d+1) = datasets(d);
+  if sum(totalResults{1,d}, 2)
+    cumMaxResults(2:end-1,d+1) = max(maxResults{1,d}, [], 2);
+  endif
+  cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:6,1:6).*eye(6,6)));
+end
+cumMaxResults(2:end-1,1) = 1:sessions;
+
+allMaxResults{1,s} = statNames{s};
+allMaxResults{2,s} = cumMaxResults;
+
+endfor % statNames
+
+

archive/2020-IlluminationInvariant/results_loc_over_time.m

@@ -0,0 +1,235 @@
+
+close all
+clear all
+
+pkg load signal
+
+# rtabmap-report --loc 32 Loop/Map_id/ loc
+# Right-click on thr legend of the figure, copy all data to clipboard
+# Paste in data#.txt where # is the number of the descriptor used
+
+resultsToShow = 1; % 1=single loc, 2=merged loc, 3=consecutive
+skipFrameDir = '0';
+
+datasetPrefix = 'Stat';
+datasets = [0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+datasetsName = {'SURF' 'SIFT' 'ORB' 'FAST/FREAK' 'FAST/BRIEF' 'GFTT/FREAK' 'GFTT/BRIEF' 'BRISK' 'GFTT/ORB' 'KAZE' 'ORB-OCTREE' 'SuperPoint' 'SURF/FREAK' 'GFTT/DAISY' 'SURF/DAISY'};
+sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
+sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
+
+if resultsToShow == 3
+  sep = [0, 1000, 3000, 5000, 7000, 9000];
+  sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
+  datasetPrefix = 'Consecutive'
+endif
+
+percentResults = {};
+totalResults = {};
+locResults = {};
+
+figure
+
+colors = get(gca, 'ColorOrder');
+tmp=colors(3,:);
+colors(3,:) = colors(5,:);
+colors(5,:) = tmp;
+
+globalSeparators = [];
+globalx = [];
+globaly = [];
+globalc = [];
+
+for d=1:length(datasets)
+
+data = dlmread([skipFrameDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);
+
+curvesBeg = 2;
+curvesEnd = size(data,2)-4;
+
+if resultsToShow == 2
+  curvesBeg = 8;
+  curvesEnd = size(data,2);
+elseif resultsToShow == 3
+  curvesEnd = size(data,2);
+endif
+curves = curvesEnd - curvesBeg + 1;
+
+percentResultsTmp = zeros(curves, length(sep)-1);
+totalResultsTmp = zeros(curves, length(sep)-1);
+locResultsTmp = zeros(curves, length(sep)-1);
+
+offset = 1;
+
+for i = 1:curves
+  index = i + curvesBeg - 1;
+  separators = [];
+  x_all = [];
+  y_all = [];
+  m_all = [];
+  previousMax = 0;
+  for j = 1:length(sep)-1
+    x = data(:,1);
+    y = data(:,index);
+    y = y(x>=sep(j) & x<=sep(j+1), :);
+    x = x(x>=sep(j) & x<=sep(j+1), :);
+    minimum = x(1,1);
+    separators = [separators previousMax];
+    x = x - (minimum-previousMax);
+    previousMax = x(end,1);
+    y = y + 1;
+    m = y;
+    y(y>0) = 1;
+    y(isnan(y)) = 0;
+    percent = sum(y)/length(y);
+    percentResultsTmp(i,j) = percent;
+    locResultsTmp(i,j) = sum(y);
+    totalResultsTmp(i,j) = length(y);
+    y(y>0) =  -(d-1)*curves -i - (d-1)*offset;
+    %x(y==0) = nan;
+    m(y==0) = nan;
+    y(y==0) = nan;
+    if resultsToShow == 2
+      if i==1 %% Merged 1, 6
+        m(m==1) = 1;
+        m(m==2) = 6;
+      elseif i==2 %% Merged 1,3(2 sessions),5
+        m(m==1) = 1;
+        m(m==2) = 3;
+        m(m==3) = 3;
+        m(m==4) = 5;
+      elseif i==3 %% Merged 2(2 sessions),4,6
+        m(m==1) = 2;
+        m(m==2) = 2;
+        m(m==4) = 6;
+        m(m==3) = 4;
+      elseif i>=4 %% Merged 1, 2(2 sessions), 3(2 sessions),4,5,6
+        m(m==1) = 1;
+        m(m==2) = 2;
+        m(m==3) = 2;
+        m(m==4) = 3;
+        m(m==5) = 3;
+        m(m==6) = 4;
+        m(m==7) = 5;
+        m(m==8) = 6;
+      endif
+    endif
+    x = upsample(x, 2);
+    y = upsample(y, 2);
+    m = upsample(m, 2);
+    x(2:2:end-1) = x(3:2:end);
+    y(2:2:end-1) = y(3:2:end);
+    m(2:2:end) = m(1:2:end);
+    x = x(1:end-1);
+    y = y(1:end-1);
+    m = m(1:end-1);
+  
+    x_all = [x_all nan x'];
+    y_all = [y_all nan y'];
+    m_all = [m_all nan m'];
+  endfor
+  if resultsToShow == 2
+    globalx = [globalx x_all];
+    globaly = [globaly y_all];
+    globalc = [globalc m_all];
+  else
+    plot(x_all,y_all, 'linewidth', 3, 'color', colors(i,:))
+    hold on
+  endif
+  separators = [separators previousMax];
+  globalSeparators = separators;
+endfor
+percentResults{1,d} = percentResultsTmp;
+totalResults{1,d} = totalResultsTmp;
+locResults{1,d} = locResultsTmp;
+endfor
+
+if resultsToShow == 2
+  indColors = ones(length(globalc), 3);
+  for j=1:length(globalc)
+    if ~isnan(globalc(j))
+      indColors(j,:) = colors(globalc(j),:);
+     endif
+  endfor
+  for i=1:6
+    tmpx = globalx;
+    tmpy = globaly;
+    tmpx(globalc~=i) = nan;
+    tmpy(globalc~=i) = nan;
+    plot(tmpx, tmpy, 'linewidth', 3, 'color', colors(i,:));
+    if i==1
+      hold on
+    endif
+  endfor
+endif
+
+for j=1:length(globalSeparators)
+  x = globalSeparators(j);
+  plot([x,x],[(-length(datasets)*(curves+1)) ,0], 'k','linewidth', 2);
+endfor
+
+for d=1:length(datasets)
+annotation ("textbox", [0, 0.96-((d-0.5)/length(datasets))*0.95, 0,0], 'string', datasetsName{datasets(d)+1})
+endfor
+for s=1:length(sep)-1
+annotation ("textbox", [0.1 + ((separators(s+1)-separators(s))/2+separators(s))/separators(end)*0.75, 0.98, 0,0], 'string', sepName{s})
+endfor
+axis('tight')
+
+
+
+set(gca, 'units', 'normalized');
+Tight = get(gca, 'Position');
+NewPos = [Tight(1) 0.01 0.77 0.95]; %New plot position [X Y W H]
+set(gca, 'Position', NewPos);
+if length(sep) == 7
+  legend('16:46', '17:27', '17:54', '18:27', '18:56', '19:35', "location", 'northeastoutside' )
+else
+  legend('16:46', '17:27', '17:54', '18:27', '18:56', "location", 'northeastoutside' )
+endif
+box off
+axis off
+
+#disp(percentResults);
+#disp(totalResults);
+
+figure; 
+for d=1:length(datasets)
+  subplot(4,2,d)
+  data=percentResults{1,d}*100;
+  data(isnan(data)) = 0;
+  hAxes = gca;
+  imagesc( hAxes, data, [0, 100])
+  %title({"",datasetsName{datasets(d)+1}})
+  colors = [ones(100,1) [1:100]'*0.01 [1:100]'*0];
+  colors(1,:) = 1;
+  colormap( hAxes ,  colors)
+  c = colorbar;
+  labels = {};
+  for v=get(c,'ytick'), labels{end+1} = sprintf('%d%%',v); end
+  set(c,'yticklabel',labels);
+  if mod(d,2) == 1
+    ylabel("Map")
+  endif
+  xlabel([datasetsName{datasets(d)+1} " Localization"])
+  set (gca, "xaxislocation", "top");
+  set(gca, 'XTickLabel', sepName, 'fontsize',7)
+  if resultsToShow == 3
+    set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56'}, 'fontsize',7)
+  elseif resultsToShow == 2
+    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+6', 'bundle', 'reduced'}, 'fontsize',7)
+  else
+    set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56', '19:35'}, 'fontsize',7)
+  endif
+endfor
+    
+% compute cumulative localizations
+cumResults = zeros(curves+2, length(datasets)+1);
+for d=1:length(datasets)
+  cumResults(1,d+1) = datasets(d);
+  cumResults(2:end-1,d+1) = round(sum(locResults{1,d}, 2) ./ sum(totalResults{1,d}, 2) * 100);
+  if resultsToShow == 1
+    cumResults(end,d+1) = round(sum(sum(locResults{1,d}.*eye(curves,curves))) / sum(totalResults{1,d},2)(1,1) * 100);
+  endif
+end
+cumResults(2:end-1,1) = 1:curves;
+cumResults

archive/2020-IlluminationInvariant/scripts/export_stats.sh

@@ -0,0 +1,19 @@
+#!/bin/bash
+
+SKIP=0
+if [ $# -eq 1 ]
+then
+  SKIP=$1
+fi
+
+DETECTOR=(0 1 6 7 9 11 12 14) #0 1 6 7 8 9 11 12 13 14
+
+PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
+REPORT_TOOL="${PREFIX}rtabmap-report"
+
+for d in "${DETECTOR[@]}"
+do
+  $REPORT_TOOL --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m "$SKIP/$d/loc"
+  $REPORT_TOOL --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ "$SKIP/$d/consecutive_loc"
+done
+

archive/2020-IlluminationInvariant/scripts/reprocess_maps.sh

@@ -0,0 +1,42 @@
+#!/bin/bash
+
+if [ $# -eq 0 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
+  exit
+fi
+TYPE=$1
+
+SKIP=0
+if [ $# -eq 2 ]
+then
+  SKIP=$2
+fi
+
+PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
+REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
+DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
+
+[ ! -d "$SKIP" ] && mkdir $SKIP
+[ ! -d "$SKIP/$TYPE" ] && mkdir $SKIP/$TYPE
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
+DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
+
+PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
+
+if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
+then
+  # binary descriptors
+  PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
+else
+  # float descriptors
+  PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
+if
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  $REPROCESS_TOOL --skip $SKIP --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 4 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --uerror $PARAMS $db $SKIP/$TYPE/$db
+  $DETECT_MORE_LOOP_CLOSURE_TOOL --uwarn $SKIP/$TYPE/$db
+done
+

archive/2020-IlluminationInvariant/scripts/reprocess_maps_all.sh

@@ -0,0 +1,16 @@
+#!/bin/bash
+
+SKIP=0
+if [ $# -eq 1 ]
+then
+  SKIP=$1
+fi
+
+DETECTOR=(0 1 6 7 9 11 12 14)
+
+for d in "${DETECTOR[@]}"
+do
+  ./reprocess_maps.sh $d $SKIP
+  ./run_merge.sh $d $SKIP
+done
+

archive/2020-IlluminationInvariant/scripts/run_all.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+
+SKIP=0
+if [ $# -eq 1 ]
+then
+  SKIP=$1
+fi
+
+./reprocess_maps_all.sh $SKIP
+./run_merge.sh $SKIP
+./run_localization_single_all.sh $SKIP
+./run_consecutive_localization_all.sh $SKIP
+

archive/2020-IlluminationInvariant/scripts/run_consecutive_localization.sh

@@ -0,0 +1,31 @@
+#!/bin/bash
+
+if [ $# -eq 0 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
+  exit
+fi
+TYPE=$1
+
+SKIP=0
+if [ $# -eq 2 ]
+then
+  SKIP=$2
+fi
+
+PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
+REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
+
+SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
+TARGETS=($SKIP/$TYPE'/map_190321-172717.db;'$SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-182709.db;' $SKIP/$TYPE'/map_190321-182709.db;'$SKIP/$TYPE'/map_190321-185608.db' $SKIP/$TYPE'/map_190321-185608.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-193556.db' )
+
+
+[ ! -d "$SKIP/$TYPE/consecutive_loc" ] && mkdir $SKIP/$TYPE/consecutive_loc
+
+for i in ${!SOURCE[@]}
+do
+    db=${SOURCE[$i]}
+    loc_dbs=${TARGETS[$i]}
+    $REPROCESS_TOOL --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --uwarn "$SKIP/$TYPE/$db;$loc_dbs" $SKIP/$TYPE/consecutive_loc/loc_$db
+done
+

archive/2020-IlluminationInvariant/scripts/run_consecutive_localization_all.sh

@@ -0,0 +1,15 @@
+#!/bin/bash
+
+SKIP=0
+if [ $# -eq 1 ]
+then
+  SKIP=$1
+fi
+
+DETECTOR=(0 1 6 7 9 11 12 14)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_consecutive_localization.sh $d $SKIP
+done
+

archive/2020-IlluminationInvariant/scripts/run_localization_accuracy.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+
+if [ $# -eq 0 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
+  exit
+fi
+TYPE=$1
+
+SKIP=0
+if [ $# -eq 2 ]
+then
+  SKIP=$2
+fi
+
+PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
+REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
+
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
+
+[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/accuracy
+
+db=merged_9999.db
+
+$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
+
+$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
+
+$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
+
+$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
+
+$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
+
+$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
+
+

archive/2020-IlluminationInvariant/scripts/run_localization_single.sh

@@ -0,0 +1,32 @@
+#!/bin/bash
+
+if [ $# -eq 0 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
+  exit
+fi
+TYPE=$1
+
+SKIP=0
+if [ $# -eq 2 ]
+then
+  SKIP=$2
+fi
+
+PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
+REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
+
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_9999_reduced.db'
+DATABASES=(  'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db'  )
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
+
+[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/loc
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  $REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/loc/loc_$db
+done
+
+

archive/2020-IlluminationInvariant/scripts/run_localization_single_all.sh

@@ -0,0 +1,15 @@
+#!/bin/bash
+
+SKIP=0
+if [ $# -eq 1 ]
+then
+  SKIP=$1
+fi
+
+DETECTOR=(0 1 6 7 9 11 12 14)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_localization_single.sh $d $SKIP
+done
+

archive/2020-IlluminationInvariant/scripts/run_merge.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+
+if [ $# -eq 0 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
+  exit
+fi
+TYPE=$1
+
+SKIP=0
+if [ $# -eq 2 ]
+then
+  SKIP=$2
+fi
+
+PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
+REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
+DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
+
+DATABASES="$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-185608.db;$SKIP/$TYPE/map_190321-193556.db"
+
+$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$DATABASES" $SKIP/$TYPE/merged_9999.db
+$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999.db
+
+#$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 --Mem/ReduceGraph true --Vis/MinInliers 60 "$DATABASES" $SKIP/$TYPE/merged_9999_reduced.db
+#$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999_reduced.db
+
+$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_16.db
+$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_16.db
+
+$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-185608.db" $SKIP/$TYPE/merged_135.db
+$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_135.db
+
+$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_246.db
+$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_246.db
+
+
+

archive/2020-IlluminationInvariant/scripts/valgrind.sh

@@ -0,0 +1,19 @@
+#!/bin/bash
+
+SKIP=0
+if [ $# -eq 1 ]
+then
+  SKIP=$1
+fi
+
+DETECTOR=(0 1 6 7 8 9 11 12 14)
+
+PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
+REPORT_TOOL="${PREFIX}rtabmap-report"
+
+for d in "${DETECTOR[@]}"
+do
+  valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 ${PREFIX}rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false  "${SKIP}/${d}/merged_9999.db;map_190321-164651.db" output.db
+  rm output.db
+done
+

cmake_modules/FindHIDAPI.cmake

@@ -0,0 +1,234 @@
+#.rst:
+# FindHIDAPI
+# ----------
+#
+# Try to find HIDAPI library, from http://www.signal11.us/oss/hidapi/
+#
+# Cache Variables: (probably not for direct use in your scripts)
+#  HIDAPI_INCLUDE_DIR
+#  HIDAPI_LIBRARY
+#
+# Non-cache variables you might use in your CMakeLists.txt:
+#  HIDAPI_FOUND
+#  HIDAPI_INCLUDE_DIRS
+#  HIDAPI_LIBRARIES
+#
+# COMPONENTS
+# ^^^^^^^^^^
+#
+# This module respects several COMPONENTS specifying the backend you prefer:
+# ``any`` (the default), ``libusb``, and ``hidraw``.
+# The availablility of the latter two depends on your platform.
+#
+#
+# IMPORTED Targets
+# ^^^^^^^^^^^^^^^^
+
+# This module defines :prop_tgt:`IMPORTED` target ``HIDAPI::hidapi`` (in all cases or
+# if no components specified), ``HIDAPI::hidapi-libusb`` (if you requested the libusb component),
+# and ``HIDAPI::hidapi-hidraw`` (if you requested the hidraw component),
+#
+# Result Variables
+# ^^^^^^^^^^^^^^^^
+#
+# ``HIDAPI_FOUND``
+#   True if HIDAPI or the requested components (if any) were found.
+#
+# We recommend using the imported targets instead of the following.
+#
+# ``HIDAPI_INCLUDE_DIRS``
+# ``HIDAPI_LIBRARIES``
+#
+# Original Author:
+# 2009-2010, 2019 Ryan Pavlik <[email protected]> <[email protected]>
+# http://academic.cleardefinition.com
+#
+# Copyright Iowa State University 2009-2010.
+# Copyright Collabora, Ltd. 2019.
+# Distributed under the Boost Software License, Version 1.0.
+# (See accompanying file LICENSE_1_0.txt or copy at
+# http://www.boost.org/LICENSE_1_0.txt)
+
+cmake_policy(SET CMP0045 NEW)
+cmake_policy(SET CMP0053 NEW)
+cmake_policy(SET CMP0054 NEW)
+
+set(HIDAPI_ROOT_DIR
+    "${HIDAPI_ROOT_DIR}"
+    CACHE PATH "Root to search for HIDAPI")
+
+# Clean up components
+if("${HIDAPI_FIND_COMPONENTS}")
+    if(WIN32 OR APPLE)
+        # This makes no sense on Windows or Mac, which have native APIs
+        list(REMOVE HIDAPI_FIND_COMPONENTS libusb)
+    endif()
+
+    if(NOT ${CMAKE_SYSTEM} MATCHES "Linux")
+        # hidraw is only on linux
+        list(REMOVE HIDAPI_FIND_COMPONENTS hidraw)
+    endif()
+endif()
+if(NOT "${HIDAPI_FIND_COMPONENTS}")
+    # Default to any
+    set(HIDAPI_FIND_COMPONENTS any)
+endif()
+
+# Ask pkg-config for hints
+find_package(PkgConfig QUIET)
+if(PKG_CONFIG_FOUND)
+    set(_old_prefix_path "${CMAKE_PREFIX_PATH}")
+    # So pkg-config uses HIDAPI_ROOT_DIR too.
+    if(HIDAPI_ROOT_DIR)
+        list(APPEND CMAKE_PREFIX_PATH ${HIDAPI_ROOT_DIR})
+    endif()
+    pkg_check_modules(PC_HIDAPI_LIBUSB QUIET hidapi-libusb)
+    pkg_check_modules(PC_HIDAPI_HIDRAW QUIET hidapi-hidraw)
+    # Restore
+    set(CMAKE_PREFIX_PATH "${_old_prefix_path}")
+endif()
+
+# Actually search
+find_library(
+    HIDAPI_UNDECORATED_LIBRARY
+    NAMES hidapi
+    PATHS "${HIDAPI_ROOT_DIR}"
+    PATH_SUFFIXES lib)
+
+find_library(
+    HIDAPI_LIBUSB_LIBRARY
+    NAMES hidapi hidapi-libusb
+    PATHS "${HIDAPI_ROOT_DIR}"
+    PATH_SUFFIXES lib
+    HINTS ${PC_HIDAPI_LIBUSB_LIBRARY_DIRS})
+
+if(CMAKE_SYSTEM MATCHES "Linux")
+    find_library(
+        HIDAPI_HIDRAW_LIBRARY
+        NAMES hidapi-hidraw
+        HINTS ${PC_HIDAPI_HIDRAW_LIBRARY_DIRS})
+endif()
+
+find_path(
+    HIDAPI_INCLUDE_DIR
+    NAMES hidapi.h
+    PATHS "${HIDAPI_ROOT_DIR}"
+    PATH_SUFFIXES hidapi include include/hidapi
+    HINTS ${PC_HIDAPI_HIDRAW_INCLUDE_DIRS} ${PC_HIDAPI_LIBUSB_INCLUDE_DIRS})
+
+find_package(Threads QUIET)
+
+###
+# Compute the "I don't care which backend" library
+###
+set(HIDAPI_LIBRARY)
+
+# First, try to use a preferred backend if supplied
+if("${HIDAPI_FIND_COMPONENTS}" MATCHES "libusb"
+   AND HIDAPI_LIBUSB_LIBRARY
+   AND NOT HIDAPI_LIBRARY)
+    set(HIDAPI_LIBRARY ${HIDAPI_LIBUSB_LIBRARY})
+endif()
+if("${HIDAPI_FIND_COMPONENTS}" MATCHES "hidraw"
+   AND HIDAPI_HIDRAW_LIBRARY
+   AND NOT HIDAPI_LIBRARY)
+    set(HIDAPI_LIBRARY ${HIDAPI_HIDRAW_LIBRARY})
+endif()
+
+# Then, if we don't have a preferred one, settle for anything.
+if(NOT HIDAPI_LIBRARY)
+    if(HIDAPI_LIBUSB_LIBRARY)
+        set(HIDAPI_LIBRARY ${HIDAPI_LIBUSB_LIBRARY})
+    elseif(HIDAPI_HIDRAW_LIBRARY)
+        set(HIDAPI_LIBRARY ${HIDAPI_HIDRAW_LIBRARY})
+    elseif(HIDAPI_UNDECORATED_LIBRARY)
+        set(HIDAPI_LIBRARY ${HIDAPI_UNDECORATED_LIBRARY})
+    endif()
+endif()
+
+###
+# Determine if the various requested components are found.
+###
+set(_hidapi_component_required_vars)
+
+foreach(_comp IN LISTS HIDAPI_FIND_COMPONENTS)
+    if("${_comp}" STREQUAL "any")
+        list(APPEND _hidapi_component_required_vars HIDAPI_INCLUDE_DIR
+                    HIDAPI_LIBRARY)
+        if(HIDAPI_INCLUDE_DIR AND EXISTS "${HIDAPI_LIBRARY}")
+            set(HIDAPI_any_FOUND TRUE)
+            mark_as_advanced(HIDAPI_INCLUDE_DIR)
+        else()
+            set(HIDAPI_any_FOUND FALSE)
+        endif()
+
+    elseif("${_comp}" STREQUAL "libusb")
+        list(APPEND _hidapi_component_required_vars HIDAPI_INCLUDE_DIR
+                    HIDAPI_LIBUSB_LIBRARY)
+        if(HIDAPI_INCLUDE_DIR AND EXISTS "${HIDAPI_LIBUSB_LIBRARY}")
+            set(HIDAPI_libusb_FOUND TRUE)
+            mark_as_advanced(HIDAPI_INCLUDE_DIR HIDAPI_LIBUSB_LIBRARY)
+        else()
+            set(HIDAPI_libusb_FOUND FALSE)
+        endif()
+
+    elseif("${_comp}" STREQUAL "hidraw")
+        list(APPEND _hidapi_component_required_vars HIDAPI_INCLUDE_DIR
+                    HIDAPI_HIDRAW_LIBRARY)
+        if(HIDAPI_INCLUDE_DIR AND EXISTS "${HIDAPI_HIDRAW_LIBRARY}")
+            set(HIDAPI_hidraw_FOUND TRUE)
+            mark_as_advanced(HIDAPI_INCLUDE_DIR HIDAPI_HIDRAW_LIBRARY)
+        else()
+            set(HIDAPI_hidraw_FOUND FALSE)
+        endif()
+
+    else()
+        message(WARNING "${_comp} is not a recognized HIDAPI component")
+        set(HIDAPI_${_comp}_FOUND FALSE)
+    endif()
+endforeach()
+unset(_comp)
+
+###
+# FPHSA call
+###
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(
+    HIDAPI REQUIRED_VARS ${_hidapi_component_required_vars} THREADS_FOUND
+    HANDLE_COMPONENTS)
+
+if(HIDAPI_FOUND)
+    set(HIDAPI_LIBRARIES "${HIDAPI_LIBRARY}")
+    set(HIDAPI_INCLUDE_DIRS "${HIDAPI_INCLUDE_DIR}")
+    if(NOT TARGET HIDAPI::hidapi)
+        add_library(HIDAPI::hidapi UNKNOWN IMPORTED)
+        set_target_properties(
+            HIDAPI::hidapi 
+	    PROPERTIES 
+            IMPORTED_LINK_INTERFACE_LANGUAGES "C"
+            IMPORTED_LOCATION ${HIDAPI_LIBRARY})
+        set_property(
+            TARGET HIDAPI::hidapi PROPERTY IMPORTED_LINK_INTERFACE_LIBRARIES
+                                           Threads::Threads)
+    endif()
+endif()
+
+if(HIDAPI_libusb_FOUND AND NOT TARGET HIDAPI::hidapi-libusb)
+    add_library(HIDAPI::hidapi-libusb UNKNOWN IMPORTED)
+    set_target_properties(
+        HIDAPI::hidapi-libusb
+        PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES "C" IMPORTED_LOCATION
+                   ${HIDAPI_LIBUSB_LIBRARY})
+    set_property(TARGET HIDAPI::hidapi-libusb
+                 PROPERTY IMPORTED_LINK_INTERFACE_LIBRARIES Threads::Threads)
+endif()
+
+if(HIDAPI_hidraw_FOUND AND NOT TARGET HIDAPI::hidapi-hidraw)
+    add_library(HIDAPI::hidapi-hidraw UNKNOWN IMPORTED)
+    set_target_properties(
+        HIDAPI::hidapi-hidraw
+        PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES "C" IMPORTED_LOCATION
+                   ${HIDAPI_HIDRAW_LIBRARY})
+    set_property(TARGET HIDAPI::hidapi-hidraw
+                 PROPERTY IMPORTED_LINK_INTERFACE_LIBRARIES Threads::Threads)
+endif()

cmake_modules/FindORB_SLAM.cmake

@@ -0,0 +1,53 @@
+# - Find ORB_SLAM2 OR ORB_SLAM3
+#
+# It sets the following variables:
+#  ORB_SLAM_FOUND         - Set to false, or undefined, if ORB_SLAM isn't found.
+#  ORB_SLAM_INCLUDE_DIRS  - The ORB_SLAM include directory.
+#  ORB_SLAM_LIBRARIES     - The ORB_SLAM library to link against.
+#  ORB_SLAM_VERSION       - The ORB_SLAM major version.
+#
+#  Set ORB_SLAM_ROOT_DIR environment variable as the path to ORB_SLAM2 or ORB_SLAM3 root folder.
+
+find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/include)
+find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
+find_library(ORB_SLAM3_LIBRARY NAMES ORB_SLAM3 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
+find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
+find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
+
+IF(ORB_SLAM2_LIBRARY)
+   SET(ORB_SLAM_VERSION 2)
+   SET(ORB_SLAM_LIBRARY ${ORB_SLAM2_LIBRARY})
+ELSEIF(ORB_SLAM3_LIBRARY)
+   SET(ORB_SLAM_VERSION 3)
+   SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY})
+ENDIF()
+
+IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+   SET(ORB_SLAM_FOUND TRUE)
+   SET(ORB_SLAM_INCLUDE_DIRS ${ORB_SLAM_INCLUDE_DIR} ${ORB_SLAM_INCLUDE_DIR}/CameraModels ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM_ROOT_DIR})
+   SET(ORB_SLAM_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM_LIBRARY} ${DBoW2_LIBRARY})
+ENDIF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+
+FIND_PACKAGE(Pangolin QUIET)
+IF(NOT Pangolin_FOUND)
+  SET(ORB_SLAM_FOUND FALSE)
+  MESSAGE(STATUS "Found ORB_SLAM but not Pangolin, disabling ORB_SLAM.")
+ELSE()
+  MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
+  SET(ORB_SLAM_INCLUDE_DIRS ${ORB_SLAM_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
+  SET(ORB_SLAM_LIBRARIES ${ORB_SLAM_LIBRARIES} ${Pangolin_LIBRARIES})
+ENDIF()
+
+IF (ORB_SLAM_FOUND)
+   # show which ORB_SLAM was found only if not quiet
+   IF (NOT ORB_SLAM_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_LIBRARIES}")
+   ENDIF (NOT ORB_SLAM_FIND_QUIETLY)
+ELSE (ORB_SLAM_FOUND)
+   # fatal error if ORB_SLAM is required but not found
+   IF (ORB_SLAM_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM")
+   ENDIF (ORB_SLAM_FIND_REQUIRED)
+ENDIF (ORB_SLAM_FOUND)
+

cmake_modules/FindORB_SLAM2.cmake

@@ -1,33 +0,0 @@
-# - Find ORB_SLAM2 
-#
-# It sets the following variables:
-#  ORB_SLAM2_FOUND         - Set to false, or undefined, if ORB_SLAM2 isn't found.
-#  ORB_SLAM2_INCLUDE_DIRS  - The ORB_SLAM2 include directory.
-#  ORB_SLAM2_LIBRARIES     - The ORB_SLAM2 library to link against.
-#
-#  Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder.
-
-find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
-find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
-find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
-find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
-find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
-
-IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
-   SET(ORB_SLAM2_FOUND TRUE)
-   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
-   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY})
-ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
-
-IF (ORB_SLAM2_FOUND)
-   # show which ORB_SLAM2 was found only if not quiet
-   IF (NOT ORB_SLAM2_FIND_QUIETLY)
-      MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}")
-   ENDIF (NOT ORB_SLAM2_FIND_QUIETLY)
-ELSE (ORB_SLAM2_FOUND)
-   # fatal error if ORB_SLAM2 is required but not found
-   IF (ORB_SLAM2_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2")
-   ENDIF (ORB_SLAM2_FIND_REQUIRED)
-ENDIF (ORB_SLAM2_FOUND)
-

cmake_modules/FindZEDOC.cmake

@@ -0,0 +1,28 @@
+# - Find ZED Open Capture 
+# This module finds zed open capture library
+#
+# It sets the following variables:
+#  ZEDOC_FOUND        - Set to false, or undefined, if ZEDOC isn't found.
+#  ZEDOC_INCLUDE_DIRS - The ZEDOC include directory.
+#  ZEDOC_LIBRARIES    - The ZEDOC library to link against.
+
+find_library(ZEDOC_LIBRARY NAMES zed_open_capture PATHS $ENV{ZEDOC_ROOT_DIR}/lib)
+find_path(ZEDOC_INCLUDE_DIR NAMES zed-open-capture/videocapture.hpp PATHS $ENV{ZEDOC_ROOT_DIR}/include)
+
+IF (ZEDOC_INCLUDE_DIR AND ZEDOC_LIBRARY)
+   SET(ZEDOC_FOUND TRUE)
+   SET(ZEDOC_INCLUDE_DIRS ${ZEDOC_INCLUDE_DIR})
+   SET(ZEDOC_LIBRARIES ${ZEDOC_LIBRARY})
+ENDIF (ZEDOC_INCLUDE_DIR AND ZEDOC_LIBRARY)
+
+IF (ZEDOC_FOUND)
+   # show which ZEDOC was found only if not quiet
+   IF (NOT _FIND_QUIETLY)
+      MESSAGE(STATUS "Found ZEDOC: ${ZEDOC_LIBRARIES}")
+   ENDIF (NOT ZEDOC_FIND_QUIETLY)
+ELSE (ZEDOC_FOUND)
+   # fatal error if ZEDOC is required but not found
+   IF (ZEDOC_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ZEDOC (Zed Open Capture)")
+   ENDIF (ZEDOC_FIND_REQUIRED)
+ENDIF (ZEDOC_FOUND)

corelib/include/rtabmap/core/CameraStereo.h

@@ -32,5 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraStereoImages.h>
 #include <rtabmap/core/camera/CameraStereoVideo.h>
 #include <rtabmap/core/camera/CameraStereoZed.h>
+#include <rtabmap/core/camera/CameraStereoZedOC.h>
 #include <rtabmap/core/camera/CameraStereoTara.h>
 #include <rtabmap/core/camera/CameraMyntEye.h>
+#include <rtabmap/core/camera/CameraDepthAI.h>

corelib/include/rtabmap/core/CameraThread.h

@@ -69,7 +69,7 @@ public:
 	void setDistortionModel(const std::string & path);
 	void enableBilateralFiltering(float sigmaS, float sigmaR);
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
-	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
 	void disableIMUFiltering();
 
 	RTABMAP_DEPRECATED(void setScanParameters(
@@ -125,6 +125,7 @@ private:
 	float _bilateralSigmaS;
 	float _bilateralSigmaR;
 	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Graph.h

@@ -327,6 +327,9 @@ std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);
 
+void RTABMAP_EXP computeMinMax(const std::map<int, Transform> & poses,
+		cv::Vec3f & min,
+		cv::Vec3f & max);
 
 } /* namespace graph */
 

corelib/include/rtabmap/core/IMU.h

@@ -9,7 +9,8 @@
 #define IMU_H_
 
 #include <opencv2/core/core.hpp>
-#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/core/Transform.h>
 
 namespace rtabmap {
 
@@ -60,6 +61,9 @@ public:
 
 	const Transform & localTransform() const {return localTransform_;}
 
+	// apply local transform rotation to data, and set Identity rotation for local transform
+	void convertToBaseFrame();
+
 	bool empty() const
 	{
 		return localTransform_.isNull();

corelib/include/rtabmap/core/LaserScan.h

@@ -71,11 +71,35 @@ public:
 
 public:
 	LaserScan();
+	LaserScan(const LaserScan & data,
+			int maxPoints,
+			float maxRange,
+			const Transform & localTransform = Transform::getIdentity());
+	RTABMAP_DEPRECATED(LaserScan(const LaserScan & data,
+			int maxPoints,
+			float maxRange,
+			Format format,
+			const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument.");
 	LaserScan(const cv::Mat & data,
 			int maxPoints,
 			float maxRange,
 			Format format,
 			const Transform & localTransform = Transform::getIdentity());
+	RTABMAP_DEPRECATED(LaserScan(const LaserScan & data,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity()), "Use version without \"format\" argument.");
+	LaserScan(const LaserScan & data,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity());
 	LaserScan(const cv::Mat & data,
 			Format format,
 			float minRange,
@@ -114,6 +138,17 @@ public:
 
 	void clear() {data_ = cv::Mat();}
 
+private:
+	void init(const cv::Mat & data,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			int maxPoints,
+			const Transform & localTransform = Transform::getIdentity());
+
 private:
 	cv::Mat data_;
 	Format format_;

corelib/include/rtabmap/core/Odometry.h

@@ -49,7 +49,7 @@ public:
 		kTypeFovis = 2,
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
-		kTypeORBSLAM2 = 5,
+		kTypeORBSLAM = 5,
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
@@ -67,7 +67,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() = 0;
 	virtual bool canProcessRawImages() const {return false;}
-	virtual bool canProcessIMU() const {return false;}
+	virtual bool canProcessAsyncIMU() const {return false;}
 
 	//getters
 	const Transform & getPose() const {return _pose;}

corelib/include/rtabmap/core/OdometryInfo.h

@@ -84,6 +84,7 @@ public:
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
 		output.guessVelocity = guessVelocity;
+		output.guess = guess;
 		output.distanceTravelled = distanceTravelled;
 		output.memoryUsage = memoryUsage;
 		output.gravityRollError = gravityRollError;
@@ -111,7 +112,8 @@ public:
 	Transform transform;
 	Transform transformFiltered;
 	Transform transformGroundTruth;
-	Transform guessVelocity;
+	Transform guessVelocity; // deprecated, will be removed. Use guess and interval instead.
+	Transform guess;
 	float distanceTravelled;
 	int memoryUsage; //MB
 	double gravityRollError;

corelib/include/rtabmap/core/PDALWriter.h

@@ -35,11 +35,11 @@ namespace rtabmap {
 
 std::string getPDALSupportedWriters();
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud,   const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
 
 }
 

corelib/include/rtabmap/core/Parameters.h

@@ -347,7 +347,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of RANSAC.");
 
     // RGB-D SLAM
-    RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
+    RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "Activate metric SLAM. If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information.");
     RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
     RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
     RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
@@ -356,7 +356,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
     RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
-    RTABMAP_PARAM(RGBD, SavedLocalizationIgnored, bool, false, "Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.");
+    RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
     RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
     RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
     RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
@@ -365,8 +365,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
     RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
     RTABMAP_PARAM(RGBD, LocalImmunizationRatio,   float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
-    RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,    "Save scan matching IDs in link's user data.");
+    RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,    "Save scan matching IDs from one-to-many proximity detection in link's user data.");
     RTABMAP_PARAM(RGBD, NeighborLinkRefining,         bool, false,  uFormat("When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using registration approach selected (%s).", kRegStrategy().c_str()));
+    RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess,     bool, false,  uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
     RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes.");
     RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
     RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
@@ -378,12 +379,12 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
     RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true,  "Detection over locations (in Working Memory) near in space.");
     RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,     "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
-    RTABMAP_PARAM(RGBD, ProximityMaxPaths,            int, 3,      "Maximum paths compared (from the most recent) for proximity detection by space. 0 means no limit.");
+    RTABMAP_PARAM(RGBD, ProximityMaxPaths,            int, 3,      "Maximum paths compared (from the most recent) for proximity detection. 0 means no limit.");
-    RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 1,    "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 1,    "Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.");
-    RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path. Set to 0 to disable merging the laser scans.");
+    RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path for one-to-many proximity detection. Set to 0 to disable one-to-many proximity detection (by merging the laser scans).");
-    RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
+    RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
-    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for visual proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
-    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for visual proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
 
     // Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -417,7 +418,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.0,      uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
 #endif
 
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
     RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
 #else
     RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
@@ -459,7 +460,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
     RTABMAP_PARAM(OdomF2M, ScanRange,           float, 0,     "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
     RTABMAP_PARAM(OdomF2M, ValidDepthRatio,     float, 0.75,  "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
     RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
     RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
@@ -520,12 +521,12 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
 
     // Odometry ORB_SLAM2
-    RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
-    RTABMAP_PARAM(OdomORBSLAM2, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
-    RTABMAP_PARAM(OdomORBSLAM2, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
-    RTABMAP_PARAM(OdomORBSLAM2, Fps,             float,  0.0,   "Camera FPS.");
+    RTABMAP_PARAM(OdomORBSLAM, Fps,             float,  0.0,   "Camera FPS.");
-    RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
+    RTABMAP_PARAM(OdomORBSLAM, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
-    RTABMAP_PARAM(OdomORBSLAM2, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");
+    RTABMAP_PARAM(OdomORBSLAM, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");
 
     // Odometry OKVIS
     RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
@@ -581,7 +582,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, PnPReprojError,           float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, PnPFlags,                 int, 0,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
     RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 0,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #else
     RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
@@ -618,7 +619,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,      uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
     RTABMAP_PARAM(Vis, BundleAdjustment,         int, 1,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #else
     RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
@@ -636,9 +637,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");
 
     // ICP registration parameters
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, Strategy,                  int, 1,       "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
+#else
+    RTABMAP_PARAM(Icp, Strategy,                  int, 0,       "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
+#endif
     RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
     RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
-    RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.0,   "Uniform sampling voxel size (0=disabled).");
+    RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.05,  "Uniform sampling voxel size (0=disabled).");
     RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1,       "Downsampling step size (1=no sampling). This is done before uniform sampling.");
     RTABMAP_PARAM(Icp, RangeMin,                  float, 0,     "Minimum range filtering (0=disabled).");
     RTABMAP_PARAM(Icp, RangeMax,                  float, 0,     "Maximum range filtering (0=disabled).");
@@ -650,6 +656,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
     RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
     RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
+    RTABMAP_PARAM(Icp, Force4DoF,                 bool, false,   uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
 #ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else
@@ -660,18 +667,17 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
     RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
     RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
+    RTABMAP_PARAM(Icp, OutlierRatio,                float, 0.85,   uFormat("Outlier ratio used with %s>0. For libpointmatcher, this parameter set TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the \"finalOverlapRatio\". The value should be between 0 and 1.", kIcpStrategy().c_str()));
 
     // libpointmatcher
-#ifdef RTABMAP_POINTMATCHER
-    RTABMAP_PARAM(Icp, PM,                       bool, true,    "Use libpointmatcher for ICP registration instead of PCL's implementation.");
-#else
-    RTABMAP_PARAM(Icp, PM,                       bool, false,   "Use libpointmatcher for ICP registration instead of PCL's implementation.");
-#endif
     RTABMAP_PARAM_STR(Icp, PMConfig,             "",            uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
     RTABMAP_PARAM(Icp, PMMatcherKnn,             int, 1,        "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
     RTABMAP_PARAM(Icp, PMMatcherEpsilon,         float, 0.0,    "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
     RTABMAP_PARAM(Icp, PMMatcherIntensity,       bool, false,   uFormat("KDTreeMatcher:  among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
-    RTABMAP_PARAM(Icp, PMOutlierRatio,           float, 0.95,   "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");
+
+    RTABMAP_PARAM(Icp, CCSamplingLimit,          unsigned int, 50000, "Maximum number of points per cloud (they are randomly resampled below this limit otherwise).");
+    RTABMAP_PARAM(Icp, CCFilterOutFarthestPoints, bool, false, "If true, the algorithm will automatically ignore farthest points from the reference, for better convergence.");
+    RTABMAP_PARAM(Icp, CCMaxFinalRMS,            float, 0.2,   "Maximum final RMS error.");
 
     // Stereo disparity
     RTABMAP_PARAM(Stereo, WinWidth,              int, 15,       "Window width.");
@@ -752,6 +758,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
     RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
     RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
+    RTABMAP_PARAM(GridGlobal, AltitudeDelta,        float,  0,       "Assemble only nodes that have the same altitude of +-delta meters of the current pose (0=disabled). This is used to generate 2D occupancy grid based on the current altitude (e.g., multi-floor building).");
     RTABMAP_PARAM(GridGlobal, OccupancyThr,         float,  0.5,     "Occupancy threshold (value between 0 and 1).");
     RTABMAP_PARAM(GridGlobal, ProbHit,              float,  0.7,     "Probability of a hit (value between 0.5 and 1).");
     RTABMAP_PARAM(GridGlobal, ProbMiss,             float,  0.4,     "Probability of a miss (value between 0 and 0.5).");

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -56,6 +56,7 @@ protected:
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
 
 private:
+	int _strategy;
 	float _maxTranslation;
 	float _maxRotation;
 	float _voxelSize;
@@ -66,19 +67,24 @@ private:
 	int _maxIterations;
 	float _epsilon;
 	float _correspondenceRatio;
+	bool _force4DoF;
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;
 	float _pointToPlaneGroundNormalsUp;
 	float _pointToPlaneMinComplexity;
 	int _pointToPlaneLowComplexityStrategy;
-	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
 	int _libpointmatcherKnn;
 	float _libpointmatcherEpsilon;
 	bool _libpointmatcherIntensity;
-	float _libpointmatcherOutlierRatio;
+	float _outlierRatio;
+	unsigned int _ccSamplingLimit;
+	bool _ccFilterOutFarthestPoints;
+	double _ccMaxFinalRMS;
+
 	void * _libpointmatcherICP;
+	void * _libpointmatcherICPFilters;
 };
 
 }

corelib/include/rtabmap/core/Rtabmap.h

@@ -199,12 +199,13 @@ public:
 	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
 	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
-			float clusterRadius = 0.5f,
+			float clusterRadiusMax = 0.5f,
 			float clusterAngle = M_PI/6.0f,
 			int iterations = 1,
 			bool intraSession = true,
 			bool interSession = true,
-			const ProgressState * state = 0);
+			const ProgressState * state = 0,
+			float clusterRadiusMin = 0.0f);
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
@@ -281,6 +282,7 @@ private:
 	bool _proximityByTime;
 	bool _proximityBySpace;
 	bool _scanMatchingIdsSavedInLinks;
+	bool _loopClosureIdentityGuess;
 	float _localRadius;
 	float _localImmunizationRatio;
 	int _proximityMaxGraphDepth;
@@ -300,7 +302,7 @@ private:
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
-	bool _savedLocalizationIgnored;
+	bool _restartAtOrigin;
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -140,6 +140,8 @@ private:
 	cv::Mat F_;
 };
 
+RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model);
+
 } // rtabmap
 
 #endif /* STEREOCAMERAMODEL_H_ */

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

@@ -0,0 +1,83 @@
+/*
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+#ifdef RTABMAP_DEPTHAI
+#ifndef DEPTHAI_OPENCV_SUPPORT
+#define DEPTHAI_OPENCV_SUPPORT
+#endif
+#include <depthai/depthai.hpp>
+#endif
+
+namespace rtabmap
+{
+
+class RTABMAP_EXP CameraDepthAI :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraDepthAI(
+			const std::string & deviceSerial = "",
+			int resolution = 1, // 0=720p, 1=800p, 2=400p
+			float imageRate=0.0f,
+			const Transform & localTransform = CameraModel::opticalRotation());
+	virtual ~CameraDepthAI();
+
+	void setOutputDepth(bool enabled, int confidence = 200);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_DEPTHAI
+	StereoCameraModel stereoModel_;
+	std::string deviceSerial_;
+	bool outputDepth_;
+	int depthConfidence_;
+	int resolution_;
+	std::shared_ptr<dai::Device> device_;
+	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
+	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
+#endif
+};
+
+
+} // namespace rtabmap

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

@@ -70,6 +70,8 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	void setResolution(int width, int height) {_width=width, _height=height;}
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
@@ -84,6 +86,8 @@ private:
 	CameraVideo::Source src_;
 	int usbDevice_;
 	int usbDevice2_;
+	int _width;
+	int _height;
 };
 
 } // namespace rtabmap

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

@@ -1,66 +1,83 @@
-/*
+/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
+All rights reserved.
-
+
-Redistribution and use in source and binary forms, with or without
+Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
+modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
+    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
+      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
+    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
+      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
+      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
+    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
+      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
+      derived from this software without specific prior written permission.
-
+
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+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
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+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
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
+*/
-
+
-#ifndef OBJDELETIONHANDLER_H_
+#pragma once
-#define OBJDELETIONHANDLER_H_
+
-
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-#include "rtabmap/utilite/UEventsHandler.h"
+
-#include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/core/StereoCameraModel.h"
-#include <QtCore/QObject>
+#include "rtabmap/core/Camera.h"
-
+#include "rtabmap/core/Version.h"
-class ObjDeletionHandler : public QObject, public UEventsHandler
+
-{
+namespace sl_oc {
-	Q_OBJECT
+namespace video {
-
+class VideoCapture;
-public:
+}
-	ObjDeletionHandler(int watchedId, QObject * receiver = 0, const char * member = 0) : _watchedId(watchedId)
+namespace sensors {
-	{
+class SensorCapture;
-		if(receiver && member)
+}
-		{
+}
-			connect(this, SIGNAL(objDeletionEventReceived(int)), receiver, member);
+
-		}
+namespace rtabmap
-	}
+{
-	virtual ~ObjDeletionHandler() {}
+class ZedOCThread;
-
+
-Q_SIGNALS:
+class RTABMAP_EXP CameraStereoZedOC :
-	void objDeletionEventReceived(int);
+	public Camera
-
+{
-protected:
+public:
-	virtual bool handleEvent(UEvent * event)
+	static bool available();
-	{
+
-		if(event->getClassName().compare("UObjDeletedEvent") == 0 &&
+public:
-		   event->getCode() == _watchedId)
+	CameraStereoZedOC(
-		{
+			int deviceId,
-			Q_EMIT objDeletionEventReceived(_watchedId);
+			int resolution = 3, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
-		}
+			float imageRate=0.0f,
-		return false;
+			const Transform & localTransform = CameraModel::opticalRotation());
-	}
+	virtual ~CameraStereoZedOC();
-private:
+
-	int _watchedId;
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-};
+	virtual bool isCalibrated() const;
-
+	virtual std::string getSerial() const;
-#endif /* OBJDELETIONHANDLER_H_ */
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_ZEDOC
+	sl_oc::video::VideoCapture * zed_;
+	sl_oc::sensors::SensorCapture * sensors_;
+	ZedOCThread * imuThread_;
+	StereoCameraModel stereoModel_;
+	int usbDevice_;
+	int resolution_;
+	uint64_t lastStamp_;
+#endif
+};
+
+
+} // namespace rtabmap

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

@@ -50,7 +50,6 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	const Signature & getMap() const {return *map_;}
 	const Signature & getLastFrame() const {return *lastFrame_;}
-	virtual bool canProcessIMU() const;
 
 	virtual Odometry::Type getType() {return Odometry::kTypeF2M;}
 
@@ -79,7 +78,6 @@ private:
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
 	std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
-	bool initGravity_;
 
 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
 	std::map<int, Transform> bundlePoses_;

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

@@ -44,7 +44,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeMSCKF;}
 	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessIMU() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

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

@@ -25,41 +25,48 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef ODOMETRYORBSLAM2_H_
+#ifndef ODOMETRYORBSLAM_H_
-#define ODOMETRYORBSLAM2_H_
+#define ODOMETRYORBSLAM_H_
 
 #include <rtabmap/core/Odometry.h>
 
+#if RTABMAP_ORB_SLAM == 3
+namespace ORB_SLAM3 {
+#else
 namespace ORB_SLAM2 {
+#endif
 class System;
 }
 
-class ORBSLAM2System;
+class ORBSLAMSystem;
 
 namespace rtabmap {
 
-class RTABMAP_EXP OdometryORBSLAM2 : public Odometry
+class RTABMAP_EXP OdometryORBSLAM : public Odometry
 {
 public:
-	OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryORBSLAM2();
+	virtual ~OdometryORBSLAM();
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM2;}
+	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;}
+	virtual bool canProcessAsyncIMU() const;
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
-#ifdef RTABMAP_ORB_SLAM2
+#ifdef RTABMAP_ORB_SLAM
-	ORBSLAM2System * orbslam2_;
+	ORBSLAMSystem * orbslam_;
 	bool firstFrame_;
 	Transform originLocalTransform_;
 	Transform previousPose_;
+	bool useIMU_;
+	Transform imuLocalTransform_;
 #endif
 
 };
 
 }
 
-#endif /* ODOMETRYORBSLAM2_H_ */
+#endif /* ODOMETRYORBSLAM_H_ */

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

@@ -46,7 +46,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeOkvis;}
 	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessIMU() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

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

@@ -43,7 +43,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
 	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessIMU() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

corelib/include/rtabmap/core/util3d.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
+#include <rtabmap/core/ProgressState.h>
 #include <map>
 #include <list>
 
@@ -198,35 +199,36 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
 
 LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs = true, bool is2D = false, const Transform & transform = Transform());
 // return CV_32FC3  (x,y,z)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC4 (x,y,z,rgb)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC4 (x,y,z,I)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC2  (x,y)
-cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC3  (x,y,I)
-cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC5  (x,y,normal_x, normal_y, normal_z)
-cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC6  (x,y,I,normal_x, normal_y, normal_z)
-cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
-cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 
 pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform());
 // For 2d laserScan, z is set to null.
@@ -299,6 +301,33 @@ void RTABMAP_EXP fillProjectedCloudHoles(
 		bool verticalDirection,
 		bool fillToBorder);
 
+/**
+ * For each point, return pixel of the best camera (NodeID->CameraIndex)
+ * looking at it based on the policy and parameters
+ */
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance = 0.0f,
+		float maxAngle = 0.0f,
+		const std::vector<float> & roiRatios = std::vector<float>(),
+		bool distanceToCamPolicy = false,
+		const ProgressState * state = 0);
+/**
+ * For each point, return pixel of the best camera (NodeID->CameraIndex)
+ * looking at it based on the policy and parameters
+ */
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance = 0.0f,
+		float maxAngle = 0.0f,
+		const std::vector<float> & roiRatios = std::vector<float>(),
+		bool distanceToCamPolicy = false,
+		const ProgressState * state = 0);
+
 bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(

corelib/include/rtabmap/core/util3d_filtering.h

@@ -279,6 +279,20 @@ pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Eigen::Vector4f & min,
@@ -297,6 +311,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const Eigen::Vector4f & min,
@@ -346,6 +366,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
 pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud);
+pcl::PCLPointCloud2::Ptr RTABMAP_EXP removeNaNFromPointCloud(
+		const pcl::PCLPointCloud2::Ptr & cloud);
 
 
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
@@ -408,6 +430,16 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
 		const pcl::IndicesPtr & indices,
 		float radiusSearch,
 		int minNeighborsInRadius);
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius);
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius);
 
 /**
  * For convenience.
@@ -590,6 +622,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		int normalKSearch,
+		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -604,6 +643,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		int normalKSearch,
+		const Eigen::Vector4f & viewpoint);
 
 /**
  * For convenience.
@@ -661,6 +707,20 @@ std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
 		int minClusterSize,
 		int maxClusterSize = std::numeric_limits<int>::max(),
 		int * biggestClusterIndex = 0);
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
 
 pcl::IndicesPtr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -678,6 +738,14 @@ pcl::IndicesPtr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		bool negative);
+pcl::IndicesPtr RTABMAP_EXP extractIndices(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative);
+pcl::IndicesPtr RTABMAP_EXP extractIndices(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -700,6 +768,16 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
 		const pcl::IndicesPtr & indices,
 		bool negative,
 		bool keepOrganized);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP extractIndices(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative,
+		bool keepOrganized);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP extractIndices(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative,
+		bool keepOrganized);
 
 pcl::IndicesPtr extractPlane(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/include/rtabmap/core/util3d_mapping.h

@@ -99,6 +99,21 @@ RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform
 		float minMapSize = 0.0f,
 		float scanMaxRange = 0.0f), "Use interface with cv::Mat scans.");
 
+/**
+ * Create 2d Occupancy grid (CV_8S)
+ * -1 = unknown
+ * 0 = empty space
+ * 100 = obstacle
+ * @param poses
+ * @param scans, should be CV_32FC2 type!
+ * @param viewpoints
+ * @param cellSize m
+ * @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
+ * @param xMin
+ * @param yMin
+ * @param minMapSize minimum map size in meters
+ * @param scanMaxRange laser scan maximum range, would be set if unknownSpaceFilled=true
+ */
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >, in /base_link frame
 		const std::map<int, cv::Point3f > & viewpoints, // /base_link -> /base_scan

corelib/src/CMakeLists.txt

@@ -33,9 +33,11 @@ SET(SRC_FILES
     camera/CameraStereoImages.cpp
     camera/CameraStereoVideo.cpp
     camera/CameraStereoZed.cpp
+    camera/CameraStereoZedOC.cpp
     camera/CameraStereoTara.cpp
     camera/CameraVideo.cpp
     camera/CameraMyntEye.cpp
+    camera/CameraDepthAI.cpp
     
     EpipolarGeometry.cpp
     VisualWord.cpp
@@ -85,11 +87,12 @@ SET(SRC_FILES
     odometry/OdometryViso2.cpp
     odometry/OdometryDVO.cpp
     odometry/OdometryOkvis.cpp
-    odometry/OdometryORBSLAM2.cpp
+    odometry/OdometryORBSLAM.cpp
     odometry/OdometryLOAM.cpp
     odometry/OdometryMSCKF.cpp
     odometry/OdometryVINS.cpp
     
+    IMU.cpp
     IMUThread.cpp
     IMUFilter.cpp
     imufilter/ComplementaryFilter.cpp
@@ -337,6 +340,14 @@ IF(mynteye_FOUND)
 	)
 ENDIF(mynteye_FOUND)
 
+IF(depthai_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		depthai::depthai-core
+		depthai::depthai-opencv
+	)
+ENDIF(depthai_FOUND)
+
 IF(WITH_TORO)
 	SET(SRC_FILES
     	${SRC_FILES}
@@ -350,14 +361,29 @@ IF(WITH_TORO)
 ENDIF(WITH_TORO)
 
 IF(G2O_FOUND)
-	SET(INCLUDE_DIRS 
+	IF(g2o_FOUND)
+	  SET(LIBRARIES
+             ${LIBRARIES}
+	     g2o::core
+	     g2o::solver_cholmod
+	     g2o::solver_eigen
+	     g2o::solver_pcg
+	     g2o::solver_csparse
+	     g2o::csparse_extension
+	     g2o::types_slam2d
+	     g2o::types_slam3d
+	     g2o::types_sba
+	  )
+	ELSE()
+	  SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
 		${G2O_INCLUDE_DIRS}
-	)
+	  )
-	SET(LIBRARIES
+	  SET(LIBRARIES
 		${LIBRARIES}
 		${G2O_LIBRARIES}
-	)
+	  )
+	ENDIF()
 	SET(SRC_FILES
     		${SRC_FILES}
 		optimizer/g2o/edge_se3_xyzprior.cpp
@@ -407,6 +433,13 @@ IF(libpointmatcher_FOUND)
 	)
 ENDIF(libpointmatcher_FOUND)
 
+IF(CCCoreLib_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		CCCoreLib::CCCoreLib
+	)
+ENDIF(CCCoreLib_FOUND)
+
 IF(FastCV_FOUND)
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
@@ -469,6 +502,19 @@ IF(ZED_FOUND)
 	ENDIF(CUDA_FOUND)
 ENDIF(ZED_FOUND)
 
+IF(ZEDOC_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${ZEDOC_INCLUDE_DIRS}
+		${HIDAPI_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${ZEDOC_LIBRARIES}
+		${HIDAPI_LIBRARIES}
+	)
+ENDIF(ZEDOC_FOUND)
+
 IF(octomap_FOUND)
     SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -563,16 +609,16 @@ IF(vins_FOUND)
 	)
 ENDIF(vins_FOUND)
 
-IF(ORB_SLAM2_FOUND)
+IF(ORB_SLAM_FOUND)
     SET(INCLUDE_DIRS
-        ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
+        ${ORB_SLAM_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM directory before the official g2o one
 		${INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
-	    ${ORB_SLAM2_LIBRARIES}
+	    ${ORB_SLAM_LIBRARIES}
 		${LIBRARIES}
 	)
-ENDIF(ORB_SLAM2_FOUND)
+ENDIF(ORB_SLAM_FOUND)
 
 IF(GTSAM_FOUND)
 	# Make sure GTSAM is built with system Eigen, not the included one in its package

corelib/src/CameraModel.cpp

@@ -37,8 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-CameraModel::CameraModel() :
+CameraModel::CameraModel()
-	localTransform_(0,0,1,0, -1,0,0,0, 0,-1,0,0)
 {
 
 }
@@ -234,7 +233,7 @@ bool CameraModel::load(const std::string & filePath)
 			n = fs["camera_name"];
 			if(n.type() != cv::FileNode::NONE)
 			{
-				name_ = (int)n;
+				name_ = (std::string)n;
 			}
 			else
 			{
@@ -767,7 +766,7 @@ bool CameraModel::inFrame(int u, int v) const
 
 std::ostream& operator<<(std::ostream& os, const CameraModel& model)
 {
-	os << "Name: " << model.name() << std::endl
+	os << "Name: " << model.name().c_str() << std::endl
 	   << "Size: " << model.imageWidth() << "x" << model.imageHeight() << std::endl
 	   << "K= " << model.K_raw() << std::endl
 	   << "D= " << model.D_raw() << std::endl

corelib/src/CameraThread.cpp

@@ -67,7 +67,8 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_bilateralFiltering(false),
 		_bilateralSigmaS(10),
 		_bilateralSigmaR(0.1),
-		_imuFilter(0)
+		_imuFilter(0),
+		_imuBaseFrameConversion(false)
 {
 	UASSERT(_camera != 0);
 }
@@ -117,10 +118,11 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 	_bilateralSigmaR = sigmaR;
 }
 
-void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters)
+void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
 {
 	delete _imuFilter;
 	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
+	_imuBaseFrameConversion = baseFrameConversion;
 }
 
 void CameraThread::disableIMUFiltering()
@@ -174,7 +176,7 @@ void CameraThread::mainLoop()
 	CameraInfo info;
 	SensorData data = _camera->takeImage(&info);
 
-	if(!data.imageRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
+	if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
 		postUpdate(&data, &info);
 
@@ -406,9 +408,8 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 					_scanRangeMin,
 					validIndices.get());
 			float maxPoints = (data.depthRaw().rows/_scanDownsampleStep)*(data.depthRaw().cols/_scanDownsampleStep);
-			cv::Mat scan;
+			LaserScan scan;
 			const Transform & baseToScan = data.cameraModels()[0].localTransform();
-			LaserScan::Format format = LaserScan::kXYZRGB;
 			if(validIndices->size())
 			{
 				if(_scanVoxelSize>0.0f)
@@ -433,7 +434,6 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
-						format = LaserScan::kXYZRGBNormal;
 					}
 					else
 					{
@@ -441,7 +441,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 					}
 				}
 			}
-			data.setLaserScan(LaserScan(scan, (int)maxPoints, _scanRangeMax, format, baseToScan));
+			data.setLaserScan(LaserScan(scan, (int)maxPoints, _scanRangeMax, baseToScan));
 			if(info) info->timeScanFromDepth = timer.ticks();
 		}
 		else
@@ -472,21 +472,31 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 		else
 		{
+			// Transform IMU data in base_link to correctly initialize yaw
+			IMU imu = data.imu();
+			if(_imuBaseFrameConversion)
+			{
+				UASSERT(!data.imu().localTransform().isNull());
+				imu.convertToBaseFrame();
+
+			}
 			_imuFilter->update(
-					data.imu().angularVelocity()[0],
+					imu.angularVelocity()[0],
-					data.imu().angularVelocity()[1],
+					imu.angularVelocity()[1],
-					data.imu().angularVelocity()[2],
+					imu.angularVelocity()[2],
-					data.imu().linearAcceleration()[0],
+					imu.linearAcceleration()[0],
-					data.imu().linearAcceleration()[1],
+					imu.linearAcceleration()[1],
-					data.imu().linearAcceleration()[2],
+					imu.linearAcceleration()[2],
 					data.stamp());
 			double qx,qy,qz,qw;
 			_imuFilter->getOrientation(qx,qy,qz,qw);
+
 			data.setIMU(IMU(
 					cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
-					data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
+					imu.angularVelocity(), imu.angularVelocityCovariance(),
-					data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
+					imu.linearAcceleration(), imu.linearAccelerationCovariance(),
-					data.imu().localTransform()));
+					imu.localTransform()));
+
 			UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
 						data.imu().orientation()[0],
 						data.imu().orientation()[1],

corelib/src/DBReader.cpp

@@ -200,6 +200,18 @@ bool DBReader::init(
 			{
 				_calibrated = true;
 			}
+			else
+			{
+				Signature * s = _dbDriver->loadSignature(*_ids.begin());
+				_dbDriver->loadNodeData(s);
+				if( s->sensorData().imageCompressed().empty() &&
+					s->getWords().empty() &&
+					!s->sensorData().laserScanCompressed().empty())
+				{
+					_calibrated = true; // only scans
+				}
+				delete s;
+			}
 		}
 	}
 	else

corelib/src/Graph.cpp

@@ -125,6 +125,12 @@ bool exportPoses(
 					// Format: stamp x y z qx qy qz qw
 					Eigen::Quaternionf q = pose.getQuaternionf();
 
+					if(iter == poses.begin())
+					{
+						// header
+						fprintf(fout, "# timestamp x y z qx qy qz qw\n");
+					}
+
 					UASSERT(uContains(stamps, iter->first));
 					fprintf(fout, "%f %f %f %f %f %f %f %f\n",
 							stamps.at(iter->first),
@@ -2328,6 +2334,33 @@ std::list<std::map<int, Transform> > getPaths(
 	return paths;
 }
 
+void computeMinMax(const std::map<int, Transform> & poses,
+		cv::Vec3f & min,
+		cv::Vec3f & max)
+{
+	if(!poses.empty())
+	{
+		min[0] = max[0] = poses.begin()->second.x();
+		min[1] = max[1] = poses.begin()->second.y();
+		min[2] = max[2] = poses.begin()->second.z();
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(min[0] > iter->second.x())
+				min[0] = iter->second.x();
+			if(max[0] < iter->second.x())
+				max[0] = iter->second.x();
+			if(min[1] > iter->second.y())
+				min[1] = iter->second.y();
+			if(max[1] < iter->second.y())
+				max[1] = iter->second.y();
+			if(min[2] > iter->second.z())
+				min[2] = iter->second.z();
+			if(max[2] < iter->second.z())
+				max[2] = iter->second.z();
+		}
+	}
+}
+
 } /* namespace graph */
 
 } /* namespace rtabmap */

corelib/src/IMU.cpp

@@ -0,0 +1,74 @@
+/*
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include <rtabmap/core/IMU.h>
+
+namespace rtabmap {
+
+void IMU::convertToBaseFrame()
+{
+	if(!localTransform_.isNull() && !localTransform_.rotation().isIdentity())
+	{
+		cv::Mat rotationMatrix, rotationMatrixT;
+		localTransform_.rotationMatrix().convertTo(rotationMatrix, CV_64FC1);
+		cv::transpose(rotationMatrix, rotationMatrixT);
+
+		cv::Mat_<double> v = rotationMatrix * cv::Mat(linearAcceleration_);
+		linearAcceleration_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
+		if(!linearAccelerationCovariance_.empty())
+		{
+			linearAccelerationCovariance_ = rotationMatrix * linearAccelerationCovariance_ * rotationMatrixT;
+		}
+
+		v = rotationMatrix * cv::Mat(angularVelocity_);
+		angularVelocity_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
+		if(!angularVelocityCovariance_.empty())
+		{
+			angularVelocityCovariance_ = rotationMatrix * angularVelocityCovariance_ * rotationMatrixT;
+		}
+
+		if(!(orientation_[0] == 0.0 && orientation_[1] == 0.0 && orientation_[2] == 0.0))
+		{
+			// orientation includes roll and pitch but not yaw in local transform
+			Eigen::Quaterniond qTheta =
+					Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()) *
+					Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
+					Eigen::AngleAxisd(localTransform_.theta(), Eigen::Vector3d::UnitZ());
+			Eigen::Quaterniond q = qTheta * Eigen::Quaterniond(orientation_[3], orientation_[0], orientation_[1], orientation_[2]) * localTransform_.getQuaterniond().inverse();
+			orientation_ = cv::Vec4d(q.x(),q.y(),q.z(),q.w());
+			if(!orientationCovariance_.empty())
+			{
+				orientationCovariance_ = rotationMatrix * orientationCovariance_ * rotationMatrixT;
+			}
+		}
+
+		localTransform_ = Transform(localTransform_.x(), localTransform_.y(), localTransform_.z(), 0,0,0);
+	}
+}
+
+} //namespace rtabmap

corelib/src/LaserScan.cpp

@@ -209,42 +209,60 @@ LaserScan::LaserScan() :
 {
 }
 
+LaserScan::LaserScan(
+		const LaserScan & scan,
+		int maxPoints,
+		float maxRange,
+		const Transform & localTransform)
+{
+	UASSERT(scan.empty() || scan.format() != kUnknown);
+	init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform);
+}
+
+LaserScan::LaserScan(
+		const LaserScan & scan,
+		int maxPoints,
+		float maxRange,
+		Format format,
+		const Transform & localTransform)
+{
+	init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
+}
+
 LaserScan::LaserScan(
 		const cv::Mat & data,
 		int maxPoints,
 		float maxRange,
 		Format format,
-		const Transform & localTransform) :
+		const Transform & localTransform)
-	data_(data),
-	format_(format),
-	maxPoints_(maxPoints),
-	rangeMin_(0),
-	rangeMax_(maxRange),
-	angleMin_(0),
-	angleMax_(0),
-	angleIncrement_(0),
-	localTransform_(localTransform)
 {
-	UASSERT(data.empty() || data.rows == 1);
+	init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
-	UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
+}
-	UASSERT(!localTransform.isNull());
 
-	if(!data.empty() && !isCompressed())
+LaserScan::LaserScan(
-	{
+		const LaserScan & scan,
-		if(format == kUnknown)
+		float minRange,
-		{
+		float maxRange,
-			*this = backwardCompatibility(data_, maxPoints_, rangeMax_, localTransform_);
+		float angleMin,
-		}
+		float angleMax,
-		else // verify that format corresponds to expected number of channels
+		float angleIncrement,
-		{
+		const Transform & localTransform)
-			UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+{
-			UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+	UASSERT(scan.empty() || scan.format() != kUnknown);
-			UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+	init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
-			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+}
-			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+
-			UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+LaserScan::LaserScan(
-		}
+		const LaserScan & scan,
-	}
+		Format format,
+		float minRange,
+		float maxRange,
+		float angleMin,
+		float angleMax,
+		float angleIncrement,
+		const Transform & localTransform)
+{
+	init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
 }
 
 LaserScan::LaserScan(
@@ -255,37 +273,77 @@ LaserScan::LaserScan(
 		float angleMin,
 		float angleMax,
 		float angleIncrement,
-		const Transform & localTransform) :
+		const Transform & localTransform)
-	data_(data),
-	format_(format),
-	rangeMin_(minRange),
-	rangeMax_(maxRange),
-	angleMin_(angleMin),
-	angleMax_(angleMax),
-	angleIncrement_(angleIncrement),
-	localTransform_(localTransform)
 {
-	UASSERT(maxRange>minRange);
+	init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
-	UASSERT(angleMax>angleMin);
+}
-	UASSERT(angleIncrement != 0.0f);
-	maxPoints_ = std::ceil((angleMax - angleMin) / angleIncrement)+1;
 
+void LaserScan::init(
+		const cv::Mat & data,
+		Format format,
+		float rangeMin,
+		float rangeMax,
+		float angleMin,
+		float angleMax,
+		float angleIncrement,
+		int maxPoints,
+		const Transform & localTransform)
+{
 	UASSERT(data.empty() || data.rows == 1);
 	UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
 	UASSERT(!localTransform.isNull());
 
+	bool is2D = false;
+	if(angleIncrement != 0.0f)
+	{
+		// 2D scan
+		is2D = true;
+		UASSERT(rangeMax>rangeMin);
+		UASSERT((angleIncrement>0 && angleMax>angleMin) || (angleIncrement<0 && angleMax<angleMin));
+		maxPoints_ = std::ceil((angleMax - angleMin) / angleIncrement)+1;
+	}
+	else
+	{
+		// 3D scan
+		UASSERT(rangeMax>=rangeMin);
+		maxPoints_ = maxPoints;
+	}
+
+	data_ = data;
+	format_ = format;
+	rangeMin_ = rangeMin;
+	rangeMax_ = rangeMax;
+	angleMin_ = angleMin;
+	angleMax_ = angleMax;
+	angleIncrement_ = angleIncrement;
+	localTransform_ = localTransform;
+
 	if(!data.empty() && !isCompressed())
 	{
-		if(data_.cols > maxPoints_)
+		if(is2D && data_.cols > maxPoints_)
 		{
 			UWARN("The number of points (%d) in the scan is over the maximum "
 				  "points (%d) defined by angle settings (min=%f max=%f inc=%f). "
 				  "The scan info may be wrong!",
 				  data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
 		}
+		else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_)
+		{
+			UDEBUG("The number of points (%d) in the scan is over the maximum "
+				  "points (%d) defined by max points setting.",
+				  data_.cols, maxPoints_);
+		}
+
 		if(format == kUnknown)
 		{
-			*this = backwardCompatibility(data_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_);
+			if(angleIncrement_ != 0)
+			{
+				*this = backwardCompatibility(data_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_);
+			}
+			else
+			{
+				*this = backwardCompatibility(data_, maxPoints_, rangeMax_, localTransform_);
+			}
 		}
 		else // verify that format corresponds to expected number of channels
 		{

corelib/src/Memory.cpp

@@ -2059,7 +2059,29 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 {
 	if(_dbDriver)
 	{
-		return _dbDriver->loadOptimizedPoses(lastlocalizationPose);
+		bool ok = true;
+		std::map<int, Transform> poses = _dbDriver->loadOptimizedPoses(lastlocalizationPose);
+		// Make sure optimized poses match the working directory! Otherwise return nothing.
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && ok; ++iter)
+		{
+			if(_workingMem.find(iter->first)==_workingMem.end())
+			{
+				ok = false;
+			}
+		}
+		if(!ok)
+		{
+			UWARN("Optimized poses (%d) and working memory "
+				  "size (%d) don't match. Returning empty optimized "
+				  "poses to force re-update. If you want to use the "
+				  "saved optimized poses, set %s to true",
+				  (int)poses.size(),
+				  (int)_workingMem.size(),
+				  Parameters::kMemInitWMWithAllNodes().c_str());
+			return std::map<int, Transform>();
+		}
+		return poses;
+
 	}
 	return std::map<int, Transform>();
 }
@@ -3053,7 +3075,7 @@ Transform Memory::computeIcpTransformMulti(
 	Transform t;
 	if(!fromScan.isEmpty() && !toScan.isEmpty())
 	{
-		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
+		Transform guess = poses.at(toId).inverse() * poses.at(fromId);
 		float guessNorm = guess.getNorm();
 		if(fromScan.rangeMax() > 0.0f && toScan.rangeMax() > 0.0f &&
 			guessNorm > fromScan.rangeMax() + toScan.rangeMax())
@@ -3144,7 +3166,7 @@ Transform Memory::computeIcpTransformMulti(
 			}
 		}
 
-		cv::Mat assembledScan;
+		LaserScan assembledScan;
 		if(assembledToNormalClouds->size())
 		{
 			assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
@@ -3183,17 +3205,20 @@ Transform Memory::computeIcpTransformMulti(
 				assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
 			}
 		}
-		UDEBUG("assembledScan=%d points", assembledScan.cols);
+		UDEBUG("assembledScan=%d points", assembledScan.size());
 
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
 		assembledData.setLaserScan(
 				LaserScan(assembledScan,
-					fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
+					maxPoints,
 					fromScan.rangeMax(),
-					toScan.format(),
 					fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
-		t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);
+		t = _registrationIcpMulti->computeTransformation(assembledData, fromS->sensorData(), guess, info);
+		if(!t.isNull())
+		{
+			t = t.inverse();
+		}
 	}
 
 	return t;
@@ -3532,11 +3557,11 @@ unsigned long Memory::getMemoryUsed() const
 	}
 	memoryUsage += _landmarksIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::map<int, std::set<int> >::iterator)) + sizeof(std::map<int, std::set<int> >);
 	memoryUsage += _landmarksInvertedIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::map<int, std::set<int> >::iterator)) + sizeof(std::map<int, std::set<int> >);
-	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
+	for(std::map<int, std::set<int> >::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
 	{
 		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
 	}
-	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksInvertedIndex.begin(); iter!=_landmarksInvertedIndex.end(); ++iter)
+	for(std::map<int, std::set<int> >::const_iterator iter=_landmarksInvertedIndex.begin(); iter!=_landmarksInvertedIndex.end(); ++iter)
 	{
 		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
 	}
@@ -4131,7 +4156,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	float t;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
-	bool isIntermediateNode = data.id() < 0 || (data.imageRaw().empty() && data.keypoints().empty());
+	bool isIntermediateNode = data.id() < 0 || (data.imageRaw().empty() && data.keypoints().empty() && data.laserScanRaw().empty());
 	int id = data.id();
 	if(_generateIds)
 	{
@@ -4205,6 +4230,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						"full calibration. If images are already rectified, set %s parameter back to true.",
 						(int)i,
 						Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					std::cout << data.cameraModels()[i] << std::endl;
 					return 0;
 				}
 			}
@@ -5246,7 +5272,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// Occupancy grid map stuff
 	if(_createOccupancyGrid && !isIntermediateNode)
 	{
-		if(!data.depthOrRightRaw().empty())
+		if( (_occupancy->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
+			(!_occupancy->isGridFromDepth() && !data.laserScanRaw().empty()))
 		{
 			cv::Mat ground, obstacles, empty;
 			float cellSize = 0.0f;

corelib/src/OccupancyGrid.cpp

@@ -286,8 +286,8 @@ void OccupancyGrid::createLocalMap(
 		cv::Mat & emptyCells,
 		cv::Point3f & viewPoint) const
 {
-	UDEBUG("scan format=%d, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
+	UDEBUG("scan format=%s, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
-			node.sensorData().laserScanRaw().isEmpty()?0:node.sensorData().laserScanRaw().format(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
+			node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
 
 	if((node.sensorData().laserScanRaw().is2d()) && !occupancyFromDepth_)
 	{
@@ -407,7 +407,7 @@ void OccupancyGrid::createLocalMap(
 				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 			}
-			createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f, LaserScan::kXYZRGB), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+			createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
 		}
 	}
 }
@@ -445,8 +445,8 @@ void OccupancyGrid::createLocalMap(
 			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 			if(grid3D_)
 			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
 			}
 			else
 			{
@@ -460,8 +460,8 @@ void OccupancyGrid::createLocalMap(
 			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 			if(grid3D_)
 			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
 			}
 			else
 			{
@@ -475,8 +475,8 @@ void OccupancyGrid::createLocalMap(
 			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 			if(grid3D_)
 			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
 			}
 			else
 			{
@@ -490,8 +490,8 @@ void OccupancyGrid::createLocalMap(
 			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 			if(grid3D_)
 			{
-				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
-				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
 			}
 			else
 			{
@@ -543,17 +543,17 @@ void OccupancyGrid::createLocalMap(
 					UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
 					if(scan.hasRGB())
 					{
-						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv).data();
-						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv).data();
-						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv).data();
 					}
 					else
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
 						pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
-						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv);
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv).data();
-						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv);
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv).data();
-						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv);
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv).data();
 					}
 				}
 			}
@@ -669,6 +669,10 @@ cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
 			}
 		}
 	}
+	else
+	{
+		UWARN("Map info is empty, cannot generate probabilistic occupancy grid");
+	}
 	return map;
 }
 
@@ -1244,6 +1248,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							ptBegin.y = 0;
 						if(ptEnd.y >= map.rows)
 							ptEnd.y = map.rows-1;
+
 						for(int i=ptBegin.x; i<ptEnd.x; ++i)
 						{
 							for(int j=ptBegin.y; j<ptEnd.y; ++j)
@@ -1282,6 +1287,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 									info[0] = (float)kter->first;
 									info[1] = float(i) * cellSize_ + xMin;
 									info[2] = float(j) * cellSize_ + yMin;
+									info[3] = probClampingMin_;
 									cter->second.first+=1;
 								}
 								value = -2; // free space (footprint)

corelib/src/Odometry.cpp

@@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryViso2.h"
 #include "rtabmap/core/odometry/OdometryDVO.h"
 #include "rtabmap/core/odometry/OdometryOkvis.h"
-#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
+#include "rtabmap/core/odometry/OdometryORBSLAM.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINS.h"
@@ -80,8 +80,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeDVO:
 		odometry = new OdometryDVO(parameters);
 		break;
-	case Odometry::kTypeORBSLAM2:
+	case Odometry::kTypeORBSLAM:
-		odometry = new OdometryORBSLAM2(parameters);
+		odometry = new OdometryORBSLAM(parameters);
 		break;
 	case Odometry::kTypeOkvis:
 		odometry = new OdometryOkvis(parameters);
@@ -284,6 +284,39 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 {
 	UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
 
+	// cache imu data
+	if(!data.imu().empty() && !this->canProcessAsyncIMU())
+	{
+		if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
+		{
+			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
+			// orientation includes roll and pitch but not yaw in local transform
+			Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) *
+					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)
+			{
+				Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
+				Transform previous = this->getPose();
+				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
+				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
+				this->reset(newFramePose);
+			}
+
+			imus_.insert(std::make_pair(data.stamp(), imuT));
+			if(imus_.size() > 1000)
+			{
+				imus_.erase(imus_.begin());
+			}
+		}
+	}
+
+
 	if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
 	{
 		if(data.stereoCameraModel().isValidForRectification())
@@ -386,21 +419,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
-	// cache imu data
-	if(!data.imu().empty())
-	{
-		if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
-		{
-			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
-			// orientation includes roll and pitch but not yaw in local transform
-			imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
-			if(imus_.size() > 1000)
-			{
-				imus_.erase(imus_.begin());
-			}
-		}
-	}
-
 	// KITTI datasets start with stamp=0
 	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
 	Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
@@ -447,7 +465,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	{
 		guess = guessIn;
 	}
-	else if(!data.imu().empty() && !imus_.empty())
+	else if(!imus_.empty())
 	{
 		// replace orientation guess with IMU (if available)
 		imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
@@ -458,6 +476,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
 					orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
 					orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
+			if(_force3DoF)
+			{
+				guess = guess.to3DoF();
+			}
+		}
+		else if(!imuLastTransform_.isNull())
+		{
+			UWARN("Could not find imu transform at %f", data.stamp());
 		}
 	}
 
@@ -503,6 +529,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			kpts[i].octave += log2value;
 		}
 		data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
+		data.setLaserScan(decimatedData.laserScanRaw());
 
 		if(info)
 		{
@@ -523,7 +550,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 	}
-	else
+	else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
 	{
 		t = this->computeTransform(data, guess, info);
 	}
@@ -540,6 +567,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		info->stamp = data.stamp();
 		info->interval = dt;
 		info->transform = t;
+		info->guess = guess;
 		if(_publishRAMUsage)
 		{
 			info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);

corelib/src/OdometryThread.cpp

@@ -119,7 +119,7 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		UDEBUG("Processing data...");
 		Transform pose = _odometry->process(data, &info);
-		if(!data.imageRaw().empty() || (pose.isNull() && data.imu().empty()))
+		if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty()))
 		{
 			UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
 			// a null pose notify that odometry could not be computed
@@ -134,9 +134,10 @@ void OdometryThread::addData(const SensorData & data)
 	{
 		if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 		{
-			if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
+			if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection())) &&
+					data.laserScanRaw().empty())
 			{
-				ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
+				ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
 				return;
 			}
 		}

corelib/src/PDALWriter.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <pdal/io/BufferReader.hpp>
 #include <pdal/StageFactory.hpp>
 #include <pdal/PluginManager.hpp>
@@ -72,14 +73,31 @@ std::string getPDALSupportedWriters()
 	return output;
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZ> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
+	if(!cameraIds.empty())
-		pdal::Dimension::Id::X,
+	{
-		pdal::Dimension::Id::Y,
+		table.layout()->registerDims({
-		pdal::Dimension::Id::Z});
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -88,15 +106,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
 		view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
 		view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -115,17 +140,37 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
+	if(!cameraIds.empty())
-		pdal::Dimension::Id::X,
+	{
-		pdal::Dimension::Id::Y,
+		table.layout()->registerDims({
-		pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Z,
-		pdal::Dimension::Id::Blue});
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -137,15 +182,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::Red, i, cloud.at(i).r);
 		view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g);
 		view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -164,20 +216,43 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
+	if(!cameraIds.empty())
-		pdal::Dimension::Id::X,
+	{
-		pdal::Dimension::Id::Y,
+		table.layout()->registerDims({
-		pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Z,
-		pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::Red,
-		pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::Green,
-		pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::Blue,
-		pdal::Dimension::Id::NormalZ});
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -192,15 +267,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
 		view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
 		view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -219,15 +301,33 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZI> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
+	if(!cameraIds.empty())
-		pdal::Dimension::Id::X,
+	{
-		pdal::Dimension::Id::Y,
+		table.layout()->registerDims({
-		pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Intensity});
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Intensity,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Intensity});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -237,15 +337,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
 		view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
 		view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -264,18 +371,39 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
+	if(!cameraIds.empty())
-		pdal::Dimension::Id::X,
+	{
-		pdal::Dimension::Id::Y,
+		table.layout()->registerDims({
-		pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Intensity,
+			pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::Z,
-		pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::Intensity,
-		pdal::Dimension::Id::NormalZ});
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Intensity,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -288,15 +416,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
 		view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
 		view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);

corelib/src/Parameters.cpp

@@ -234,6 +234,20 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 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())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/ThDepth",     std::make_pair(true, Parameters::kOdomORBSLAMThDepth())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/Fps",         std::make_pair(true, Parameters::kOdomORBSLAMFps())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/MaxFeatures", std::make_pair(true, Parameters::kOdomORBSLAMMaxFeatures())));
+		removedParameters_.insert(std::make_pair("OdomORBSLAM2/MapSize",     std::make_pair(true, Parameters::kOdomORBSLAMMapSize())));
+
+		removedParameters_.insert(std::make_pair("RGBD/SavedLocalizationIgnored", std::make_pair(true, Parameters::kRGBDStartAtOrigin())));
+
+		removedParameters_.insert(std::make_pair("Icp/PMForce4DoF", std::make_pair(true, Parameters::kIcpForce4DoF())));
+		removedParameters_.insert(std::make_pair("Icp/PM",          std::make_pair(true, Parameters::kIcpStrategy()))); // convert "true" to "1"
+		removedParameters_.insert(std::make_pair("Icp/PMOutlierRatio",   std::make_pair(true, Parameters::kIcpOutlierRatio())));
+
 		// 0.20.
 		removedParameters_.insert(std::make_pair("SuperGlue/Path",           std::make_pair(true, Parameters::kPyMatcherPath())));
 		removedParameters_.insert(std::make_pair("SuperGlue/Iterations",     std::make_pair(true, Parameters::kPyMatcherIterations())));
@@ -732,6 +746,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 ZED Open Capture:";
+#ifdef RTABMAP_ZEDOC
+				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 RealSense:";
 #ifdef RTABMAP_REALSENSE
@@ -756,12 +776,24 @@ 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 DepthAI:";
+#ifdef RTABMAP_DEPTHAI
+				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 libpointmatcher:";
 #ifdef RTABMAP_POINTMATCHER
 				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 CCCoreLib:";
+#ifdef RTABMAP_CCCORELIB
+				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 octomap:";
 #ifdef RTABMAP_OCTOMAP
@@ -811,8 +843,14 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
+#if RTABMAP_ORB_SLAM == 3
+				str = "With ORB_SLAM3:";
+#elif RTABMAP_ORB_SLAM == 2
 				str = "With ORB_SLAM2:";
-#ifdef RTABMAP_ORB_SLAM2
+#else
+				str = "With ORB_SLAM:";
+#endif
+#ifdef RTABMAP_ORB_SLAM
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;

corelib/src/Rtabmap.cpp

@@ -106,6 +106,7 @@ Rtabmap::Rtabmap() :
 	_proximityByTime(Parameters::defaultRGBDProximityByTime()),
 	_proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
 	_scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
+	_loopClosureIdentityGuess(Parameters::defaultRGBDLoopClosureIdentityGuess()),
 	_localRadius(Parameters::defaultRGBDLocalRadius()),
 	_localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()),
 	_proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
@@ -125,7 +126,7 @@ Rtabmap::Rtabmap() :
 	_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
 	_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
 	_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
-	_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
+	_restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()),
 	_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
@@ -336,36 +337,43 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	this->parseParameters(allParameters);
 
 	Transform lastPose;
-	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
+	_optimizedPoses.clear();
-	if(!_optimizedPoses.empty())
+	if(!_memory->isIncremental())
 	{
-		if(_savedLocalizationIgnored)
+		_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
+		if(!_optimizedPoses.empty())
 		{
-			UDEBUG("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDSavedLocalizationIgnored().c_str());
+			if(_restartAtOrigin)
-			lastPose.setIdentity();
+			{
+				UINFO("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
+				lastPose.setIdentity();
+			}
+			_lastLocalizationPose = lastPose;
+
+			UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
+
+			std::map<int, Transform> tmp;
+			// Get just the links
+			_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
+
+			// Initialize Bayes' prediction matrix
+			UTimer time;
+			std::map<int, float> likelihood;
+			likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
+			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+			{
+				if(_memory->getSignature(iter->first))
+				{
+					likelihood.insert(std::make_pair(iter->first, 0));
+				}
+			}
+			_bayesFilter->computePosterior(_memory, likelihood);
+			UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
 		}
-		_lastLocalizationPose = lastPose;
+		else
-
-		UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
-
-		std::map<int, Transform> tmp;
-		// Get just the links
-		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
-
-		// Initialize Bayes' prediction matrix
-		UTimer time;
-		std::map<int, float> likelihood;
-		likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
-		for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 		{
-			likelihood.insert(std::make_pair(iter->first, 0));
+			UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
 		}
-		_bayesFilter->computePosterior(_memory, likelihood);
-		UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
-	}
-	else
-	{
-		UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
 	}
 
 	if(_databasePath.empty())
@@ -508,6 +516,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDProximityByTime(), _proximityByTime);
 	Parameters::parse(parameters, Parameters::kRGBDProximityBySpace(), _proximityBySpace);
 	Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
+	Parameters::parse(parameters, Parameters::kRGBDLoopClosureIdentityGuess(), _loopClosureIdentityGuess);
 	Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio);
 	Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
@@ -541,7 +550,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
 	Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
 	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
-	Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
+	Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin);
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
 	Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
@@ -748,7 +757,7 @@ int Rtabmap::triggerNewMap()
 
 		if(!_memory->isIncremental())
 		{
-			if(_savedLocalizationIgnored)
+			if(_restartAtOrigin)
 			{
 				_mapCorrection.setIdentity();
 				_lastLocalizationPose.setIdentity();
@@ -1113,7 +1122,6 @@ bool Rtabmap::process(
 			_optimizedPoses.size() &&
 			_mapCorrection.isIdentity() &&
 			!_lastLocalizationPose.isNull() &&
-			!_lastLocalizationPose.isIdentity() &&
 			_lastLocalizationNodeId == 0)
 		{
 			// Localization mode
@@ -1235,6 +1243,7 @@ bool Rtabmap::process(
 	bool smallDisplacement = false;
 	bool tooFastMovement = false;
 	std::list<int> signaturesRemoved;
+	bool neighborLinkRefined = false;
 	if(_rgbdSlamMode)
 	{
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
@@ -1363,7 +1372,8 @@ bool Rtabmap::process(
 							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 						}
 					}
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
+					neighborLinkRefined = !t.isNull();
+					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(),neighborLinkRefined?1.0f:0);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
@@ -2265,7 +2275,15 @@ bool Rtabmap::process(
 	   _rgbdSlamMode &&
 	   signature->getWeight() >= 0) // not an intermediate node
 	{
-		if(_graphOptimizer->iterations() == 0)
+		if(_startNewMapOnLoopClosure &&
+			_memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place)
+			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session)
+		{
+			UINFO("Proximity detection by space disabled as if we force to have a global loop "
+					"closure with previous map before doing proximity detections (%s=true).",
+					Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
+		}
+		else if(_graphOptimizer->iterations() == 0)
 		{
 			UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
 		}
@@ -2596,7 +2614,12 @@ bool Rtabmap::process(
 		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
 		if(_rgbdSlamMode)
 		{
-			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
+			transform = _memory->computeTransform(
+					_loopClosureHypothesis.first,
+					signature->id(),
+					_loopClosureIdentityGuess?Transform::getIdentity():Transform(),
+					&info);
+
 			loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
 			loopClosureVisualInliersDistribution = info.inliersDistribution;
 
@@ -2686,7 +2709,7 @@ bool Rtabmap::process(
 	     lastProximitySpaceClosureId>0 || // can be different map of the current one
 	     statistics_.reducedIds().size() ||
 		 (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
-		 (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isOdomGravityUsed()) || // gravity edge
+		 (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && (!_memory->isOdomGravityUsed() || neighborLinkRefined)) || // gravity edge
 	     proximityDetectionsInTimeFound>0 ||
 		 landmarkDetected!=0 ||
 		 signaturesRetrieved.size()) // can be different map of the current one
@@ -4627,12 +4650,13 @@ std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
 }
 
 int Rtabmap::detectMoreLoopClosures(
-		float clusterRadius,
+		float clusterRadiusMax,
 		float clusterAngle,
 		int iterations,
 		bool intraSession,
 		bool interSession,
-		const ProgressState * processState)
+		const ProgressState * processState,
+		float clusterRadiusMin)
 {
 	UASSERT(iterations>0);
 
@@ -4675,11 +4699,11 @@ int Rtabmap::detectMoreLoopClosures(
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
-				n+1, iterations, clusterRadius, clusterAngle);
+				n+1, iterations, clusterRadiusMax, clusterAngle);
 
 		std::multimap<int, int> clusters = graph::radiusPosesClustering(
 				posesToCheckLoopClosures,
-				clusterRadius,
+				clusterRadiusMax,
 				clusterAngle);
 
 		UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
@@ -4727,138 +4751,148 @@ int Rtabmap::detectMoreLoopClosures(
 					   addedLinks.find(to) == addedLinks.end() &&
 					   rtabmap::graph::findLink(links, from, to) == links.end())
 					{
-						checkedLoopClosures.insert(std::make_pair(from, to));
+						// Reverify if in the bounds with the current optimized graph
-
+						Transform delta = poses.at(from).inverse() * poses.at(to);
-						UASSERT(signatures.find(from) != signatures.end());
+						if(delta.getNorm() < clusterRadiusMax &&
-						UASSERT(signatures.find(to) != signatures.end());
+						   delta.getNorm() >= clusterRadiusMin)
-
-						Transform guess;
-						if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
 						{
-							guess = poses.at(from).inverse() * poses.at(to);
+							checkedLoopClosures.insert(std::make_pair(from, to));
-						}
 
-						RegistrationInfo info;
+							UASSERT(signatures.find(from) != signatures.end());
-						// use signatures instead of IDs because some signatures may not be in WM
+							UASSERT(signatures.find(to) != signatures.end());
-						Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
 
-						if(!t.isNull())
+							Transform guess;
-						{
+							if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
-							bool updateConstraints = true;
-							if(_optimizationMaxError > 0.0f)
 							{
-								//optimize the graph to see if the new constraint is globally valid
+								guess = poses.at(from).inverse() * poses.at(to);
-
-								int fromId = from;
-								int mapId = signatures.at(from).mapId();
-								// use first node of the map containing from
-								for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
-								{
-									if(ster->second.mapId() == mapId)
-									{
-										fromId = ster->first;
-										break;
-									}
-								}
-								std::multimap<int, Link> linksIn = links;
-								linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
-								const Link * maxLinearLink = 0;
-								const Link * maxAngularLink = 0;
-								float maxLinearError = 0.0f;
-								float maxAngularError = 0.0f;
-								float maxLinearErrorRatio = 0.0f;
-								float maxAngularErrorRatio = 0.0f;
-								std::map<int, Transform> optimizedPoses;
-								std::multimap<int, Link> links;
-								UASSERT(poses.find(fromId) != poses.end());
-								UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
-								UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-								_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
-								UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
-								UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT(graph::findLink(links, from, to) != links.end());
-								optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
-								std::string msg;
-								if(optimizedPoses.size())
-								{
-									graph::computeMaxGraphErrors(
-											optimizedPoses,
-											links,
-											maxLinearErrorRatio,
-											maxAngularErrorRatio,
-											maxLinearError,
-											maxAngularError,
-											&maxLinearLink,
-											&maxAngularLink);
-									if(maxLinearLink)
-									{
-										UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-										if(maxLinearErrorRatio > _optimizationMaxError)
-										{
-											msg = uFormat("Rejecting edge %d->%d because "
-													  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
-													  "\"%s\" is %f.",
-													  from,
-													  to,
-													  maxLinearError,
-													  maxLinearLink->from(),
-													  maxLinearLink->to(),
-													  maxLinearErrorRatio,
-													  sqrt(maxLinearLink->transVariance()),
-													  Parameters::kRGBDOptimizeMaxError().c_str(),
-													  _optimizationMaxError);
-										}
-									}
-									else if(maxAngularLink)
-									{
-										UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-										if(maxAngularErrorRatio > _optimizationMaxError)
-										{
-											msg = uFormat("Rejecting edge %d->%d because "
-													  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
-													  "\"%s\" is %f m.",
-													  from,
-													  to,
-													  maxAngularError*180.0f/M_PI,
-													  maxAngularLink->from(),
-													  maxAngularLink->to(),
-													  maxAngularErrorRatio,
-													  sqrt(maxAngularLink->rotVariance()),
-													  Parameters::kRGBDOptimizeMaxError().c_str(),
-													  _optimizationMaxError);
-										}
-									}
-								}
-								else
-								{
-									msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
-											  from,
-											  to);
-								}
-								if(!msg.empty())
-								{
-									UWARN("%s", msg.c_str());
-									updateConstraints = false;
-								}
 							}
 
-							if(updateConstraints)
+							RegistrationInfo info;
-							{
+							// use signatures instead of IDs because some signatures may not be in WM
-								addedLinks.insert(from);
+							Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
-								addedLinks.insert(to);
-								cv::Mat inf = getInformation(info.covariance);
-								links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
-								loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
-								std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
-								UINFO(msg.c_str());
 
-								if(processState)
+							if(!t.isNull())
+							{
+								bool updateConstraints = true;
+								if(_optimizationMaxError > 0.0f)
 								{
-									UINFO(msg.c_str());
+									//optimize the graph to see if the new constraint is globally valid
-									if(!processState->callback(msg))
+
+									int fromId = from;
+									int mapId = signatures.at(from).mapId();
+									// use first node of the map containing from
+									for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
 									{
-										return -1;
+										if(ster->second.mapId() == mapId)
+										{
+											fromId = ster->first;
+											break;
+										}
+									}
+									std::multimap<int, Link> linksIn = links;
+									linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
+									const Link * maxLinearLink = 0;
+									const Link * maxAngularLink = 0;
+									float maxLinearError = 0.0f;
+									float maxAngularError = 0.0f;
+									float maxLinearErrorRatio = 0.0f;
+									float maxAngularErrorRatio = 0.0f;
+									std::map<int, Transform> optimizedPoses;
+									std::multimap<int, Link> links;
+									UASSERT(poses.find(fromId) != poses.end());
+									UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
+									UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
+									_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
+									UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
+									UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
+									UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
+									UASSERT(graph::findLink(links, from, to) != links.end());
+									optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
+									std::string msg;
+									if(optimizedPoses.size())
+									{
+										graph::computeMaxGraphErrors(
+												optimizedPoses,
+												links,
+												maxLinearErrorRatio,
+												maxAngularErrorRatio,
+												maxLinearError,
+												maxAngularError,
+												&maxLinearLink,
+												&maxAngularLink);
+										if(maxLinearLink)
+										{
+											UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+											if(maxLinearErrorRatio > _optimizationMaxError)
+											{
+												msg = uFormat("Rejecting edge %d->%d because "
+														  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+														  "\"%s\" is %f.",
+														  from,
+														  to,
+														  maxLinearError,
+														  maxLinearLink->from(),
+														  maxLinearLink->to(),
+														  maxLinearErrorRatio,
+														  sqrt(maxLinearLink->transVariance()),
+														  Parameters::kRGBDOptimizeMaxError().c_str(),
+														  _optimizationMaxError);
+											}
+										}
+										else if(maxAngularLink)
+										{
+											UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+											if(maxAngularErrorRatio > _optimizationMaxError)
+											{
+												msg = uFormat("Rejecting edge %d->%d because "
+														  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+														  "\"%s\" is %f m.",
+														  from,
+														  to,
+														  maxAngularError*180.0f/M_PI,
+														  maxAngularLink->from(),
+														  maxAngularLink->to(),
+														  maxAngularErrorRatio,
+														  sqrt(maxAngularLink->rotVariance()),
+														  Parameters::kRGBDOptimizeMaxError().c_str(),
+														  _optimizationMaxError);
+											}
+										}
+									}
+									else
+									{
+										msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+												  from,
+												  to);
+									}
+									if(!msg.empty())
+									{
+										UWARN("%s", msg.c_str());
+										updateConstraints = false;
+									}
+									else
+									{
+										poses = optimizedPoses;
+									}
+								}
+
+								if(updateConstraints)
+								{
+									addedLinks.insert(from);
+									addedLinks.insert(to);
+									cv::Mat inf = getInformation(info.covariance);
+									links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
+									loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
+									std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
+									UINFO(msg.c_str());
+
+									if(processState)
+									{
+										UINFO(msg.c_str());
+										if(!processState->callback(msg))
+										{
+											return -1;
+										}
 									}
 								}
 							}

corelib/src/StereoCameraModel.cpp

@@ -401,7 +401,11 @@ bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
 	}
 	else
 	{
-		UERROR("Failed saving stereo extrinsics (they are null).");
+		UERROR("Failed saving stereo extrinsics (they are null):");
+		std::cout << "R= " << R_ << std::endl;
+		std::cout << "T= " << T_ << std::endl;
+		std::cout << "E= " << T_ << std::endl;
+		std::cout << "F= " << F_ << std::endl;
 	}
 	return false;
 }
@@ -598,4 +602,17 @@ Transform StereoCameraModel::stereoTransform() const
 	return Transform();
 }
 
+std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model)
+{
+	os << "Left Camera " << model.left() << std::endl
+	   << "Right Camera " << model.right() << std::endl
+	   << "Stereo Extrinsics:" << std::endl
+	   << "R= " << model.R() << std::endl
+	   << "T= " << model.T() << std::endl
+	   << "E= " << model.E() << std::endl
+	   << "F= "<< model.F() << std::endl
+	   << "baseline= " << model.baseline() << std::endl;
+	return os;
+}
+
 } /* namespace rtabmap */

corelib/src/camera/CameraDepthAI.cpp

@@ -0,0 +1,419 @@
+/*
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraDepthAI.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
+
+
+namespace rtabmap {
+
+bool CameraDepthAI::available()
+{
+#ifdef RTABMAP_DEPTHAI
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraDepthAI::CameraDepthAI(
+		const std::string & deviceSerial,
+		int resolution,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_DEPTHAI
+	,
+	deviceSerial_(deviceSerial),
+	outputDepth_(false),
+	depthConfidence_(200),
+	resolution_(resolution)
+#endif
+{
+#ifdef RTABMAP_DEPTHAI
+	UASSERT(resolution_>=0 && resolution_<=2);
+#endif
+}
+
+CameraDepthAI::~CameraDepthAI()
+{
+}
+
+void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
+{
+#ifdef RTABMAP_DEPTHAI
+	outputDepth_ = enabled;
+	if(outputDepth_)
+	{
+		depthConfidence_ = confidence;
+	}
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+std::vector<unsigned char> convertCalibration(const StereoCameraModel & stereoModel)
+{
+	UDEBUG("");
+	// Calibration
+	// https://github.com/luxonis/depthai/blob/39852dcb9fe349476c30d0ed90d3750bb2a53e26/depthai_helpers/calibration_utils.py#L97-L109
+	std::vector<unsigned char> data;
+	cv::Mat tmp;
+	int ptr;
+	// R1_fp32
+	stereoModel.left().R().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// R2_fp32
+	stereoModel.right().R().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// M1_fp32
+	stereoModel.left().K_raw().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// M2_fp32
+	stereoModel.right().K_raw().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// R_fp32
+	stereoModel.R().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// T_fp32
+	stereoModel.T().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// M3_fp32
+	tmp = cv::Mat::zeros(3,3,CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// R_rgb_fp32
+	tmp = cv::Mat::eye(3,3,CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// T_rgb_fp32
+	tmp = cv::Mat::zeros(1,3,CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	// d1_coeff_fp32
+	stereoModel.left().D_raw().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+	data.resize(data.size() + (14-tmp.total())*sizeof(float), 0); // padding
+
+	// d2_coeff_fp32
+	stereoModel.right().D_raw().convertTo(tmp, CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+	data.resize(data.size() + (14-tmp.total())*sizeof(float), 0); // padding
+
+	// d3_coeff_fp32
+	tmp = cv::Mat::zeros(1,14,CV_32FC1);
+	ptr = data.size();
+	data.resize(data.size() + tmp.total()*tmp.elemSize());
+	memcpy(data.data()+ptr, tmp.data, tmp.total()*tmp.elemSize());
+
+	return data;
+}
+
+bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+#ifdef RTABMAP_DEPTHAI
+
+	std::vector<dai::DeviceInfo> devices = dai::Device::getAllAvailableDevices();
+	if(devices.empty())
+	{
+		return false;
+	}
+
+	dai::DeviceInfo deviceToUse;
+	if(deviceSerial_.empty())
+		deviceToUse = devices[0];
+	for(size_t i=0; i<devices.size(); ++i)
+	{
+		UINFO("DepthAI device found: %s", devices[i].getMxId().c_str());
+		if(!deviceSerial_.empty() && deviceSerial_.compare(devices[i].getMxId()) == 0)
+		{
+			deviceToUse = devices[i];
+		}
+	}
+
+	if(deviceToUse.getMxId().empty())
+	{
+		UERROR("Could not find device with serial \"%s\", found devices:", deviceSerial_.c_str());
+		for(size_t i=0; i<devices.size(); ++i)
+		{
+			UERROR("DepthAI device found: %s", devices[i].getMxId().c_str());
+		}
+		return false;
+	}
+	deviceSerial_ = deviceToUse.getMxId();
+
+	// look for calibration files
+	stereoModel_ = StereoCameraModel();
+	if(!calibrationFolder.empty())
+	{
+		std::string name = cameraName.empty()?deviceSerial_:cameraName;
+		if(!stereoModel_.load(calibrationFolder, name, false))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+					name.c_str(), calibrationFolder.c_str());
+			outputDepth_ = false;
+		}
+		else
+		{
+			UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+					stereoModel_.left().fx(),
+					stereoModel_.left().cx(),
+					stereoModel_.left().cy(),
+					stereoModel_.baseline());
+			stereoModel_.setLocalTransform(this->getLocalTransform());
+
+			cv::Size target(resolution_<2?1280:640, resolution_==0?720:resolution_==1?800:400);
+
+			if(stereoModel_.left().imageWidth() != target.width)
+			{
+				//adjust scale if resolution is not the same used than in calibration
+				UWARN("Loaded calibration has different resolution (%dx%d) than "
+					  "the selected device resolution (%dx%d). We will scale the calibration "
+					  "for convenience.",
+					  stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
+					  target.width, target.height);
+				stereoModel_.scale(double(target.width)/double(stereoModel_.left().imageWidth()));
+			}
+
+			if(stereoModel_.left().imageHeight() != target.height)
+			{
+				// Ratio not the same, adjust cy
+				cv::Rect roi(0, (stereoModel_.left().imageHeight()-target.height)/2, target.width, target.height);
+				UWARN("Loaded calibration has different height (%dx%d) than "
+					  "the selected device resolution (%dx%d). We will crop the calibration "
+					  "for convenience.",
+					  stereoModel_.left().imageWidth(), stereoModel_.left().imageHeight(),
+					  target.width, target.height);
+				stereoModel_.roi(roi);
+			}
+
+			if(ULogger::level() <= ULogger::kInfo)
+			{
+				UINFO("Calibration:");
+				std::cout << stereoModel_ << std::endl;
+			}
+		}
+	}
+
+	if(!stereoModel_.isValidForRectification())
+	{
+		UINFO("Disabling outputDepth as no valid calibration has been loaded.");
+		outputDepth_ = false;
+	}
+	else
+	{
+		stereoModel_.initRectificationMap();
+	}
+
+	dai::Pipeline p;
+	auto monoLeft  = p.create<dai::node::MonoCamera>();
+	auto monoRight = p.create<dai::node::MonoCamera>();
+	auto stereo    = p.create<dai::node::StereoDepth>();
+	auto xoutLeft = p.create<dai::node::XLinkOut>();
+	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
+
+	 // XLinkOut
+	xoutLeft->setStreamName(outputDepth_/*stereoModel_.isValidForRectification()*/?"rectified_left":"left");
+	xoutDepthOrRight->setStreamName(outputDepth_?"depth"/*:stereoModel_.isValidForRectification()?"rectified_right"*/:"right");
+
+	// MonoCamera
+	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
+	monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
+	monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
+	monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
+	if(this->getImageRate()>0)
+	{
+		monoLeft->setFps(this->getImageRate());
+		monoRight->setFps(this->getImageRate());
+	}
+
+	// StereoDepth
+	stereo->setOutputDepth(outputDepth_);
+	stereo->setOutputRectified(stereoModel_.isValidForRectification());
+	stereo->setConfidenceThreshold(depthConfidence_);
+	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
+	stereo->setRectifyMirrorFrame(false);
+	stereo->setLeftRightCheck(false);
+	stereo->setSubpixel(false);
+	stereo->setExtendedDisparity(false);
+
+	// Link plugins CAM -> STEREO -> XLINK
+	monoLeft->out.link(stereo->left);
+	monoRight->out.link(stereo->right);
+
+	if(outputDepth_)
+	{
+		stereo->rectifiedLeft.link(xoutLeft->input);
+		stereo->depth.link(xoutDepthOrRight->input);
+	}
+	/*else if(stereoModel_.isValidForRectification())
+	{
+		stereo->rectifiedLeft.link(xoutLeft->input);
+		stereo->rectifiedRight.link(xoutDepthOrRight->input);
+	}*/
+	else
+	{
+		stereo->syncedLeft.link(xoutLeft->input);
+		stereo->syncedRight.link(xoutDepthOrRight->input);
+	}
+
+
+	if(stereoModel_.isValidForRectification())
+	{
+		// FIXME: What is the exact format for the calibration stream?
+		//std::vector<unsigned char> data = convertCalibration(stereoModel_);
+		//stereo->loadCalibrationData(data);
+	}
+	device_.reset(new dai::Device(p, deviceToUse));
+
+	UDEBUG("");
+	if(outputDepth_)
+	{
+		leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
+		rightOrDepthQueue_ = device_->getOutputQueue("depth", 8, false);
+	}
+	else
+	{
+		UDEBUG("");
+		leftQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_left":*/"left", 8, false);
+		UDEBUG("");
+		rightOrDepthQueue_ = device_->getOutputQueue(/*stereoModel_.isValidForRectification()?"rectified_right":*/"right", 8, false);
+		UDEBUG("");
+	}
+
+	device_->startPipeline();
+
+	uSleep(2000); // avoid bad frames on start
+
+	return true;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+	return false;
+}
+
+bool CameraDepthAI::isCalibrated() const
+{
+#ifdef RTABMAP_DEPTHAI
+	return stereoModel_.isValidForProjection();
+#else
+	return false;
+#endif
+}
+
+std::string CameraDepthAI::getSerial() const
+{
+#ifdef RTABMAP_DEPTHAI
+	return deviceSerial_;
+#endif
+	return "";
+}
+
+SensorData CameraDepthAI::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_DEPTHAI
+
+	cv::Mat left, depthOrRight;
+	auto rectifL = leftQueue_->get<dai::ImgFrame>();
+	auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
+	if(rectifL.get() && rectifRightOrDepth.get())
+	{
+		auto stampLeft = rectifL->getTimestamp().time_since_epoch().count();
+		auto stampRight = rectifRightOrDepth->getTimestamp().time_since_epoch().count();
+		double stamp = double(stampLeft)/10e8;
+		left = rectifL->getCvFrame();
+		depthOrRight = rectifRightOrDepth->getCvFrame();
+
+		if(!left.empty() && !depthOrRight.empty())
+		{
+			if(depthOrRight.type() == CV_8UC1)
+			{
+				if(stereoModel_.isValidForRectification())
+				{
+					left = stereoModel_.left().rectifyImage(left);
+					depthOrRight = stereoModel_.right().rectifyImage(depthOrRight);
+				}
+				data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
+			}
+			else
+			{
+				cv::flip(depthOrRight, depthOrRight, 1);
+				data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
+			}
+
+			if(stampLeft != stampRight)
+			{
+				UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8);
+			}
+		}
+	}
+	else
+	{
+		UWARN("Null images received!?");
+	}
+
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraImages.cpp

@@ -121,27 +121,32 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	UDEBUG("");
 	if(_dir)
 	{
-		_dir->setPath(_path, "jpg ppm png bmp pnm tiff pgm");
+		delete _dir;
+		_dir = 0;
 	}
-	else
+	if(!_path.empty())
 	{
 		_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff pgm");
-	}
+		if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
-	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
+		{
-	{
+			_path.append("/");
-		_path.append("/");
+		}
-	}
+		if(!_dir->isValid())
-	if(!_dir->isValid())
+		{
-	{
+			ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
-		ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
+			delete _dir;
-	}
+			_dir = 0;
-	else if(_dir->getFileNames().size() == 0)
+		}
-	{
+		else if(_dir->getFileNames().size() == 0)
-		UWARN("Directory is empty \"%s\"", _path.c_str());
+		{
-	}
+			UWARN("Directory is empty \"%s\"", _path.c_str());
-	else
+			delete _dir;
-	{
+			_dir = 0;
-		UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
+		}
+		else
+		{
+			UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
+		}
 	}
 
 	// check for scan directory
@@ -170,7 +175,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			delete _scanDir;
 			_scanDir = 0;
 		}
-		else if(_scanDir->getFileNames().size() != _dir->getFileNames().size())
+		else if(_dir && _scanDir->getFileNames().size() != _dir->getFileNames().size())
 		{
 			UERROR("Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
 					_scanPath.c_str(),
@@ -186,40 +191,49 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		}
 	}
 
-	// look for calibration files
+	if(_dir==0 && _scanDir == 0)
-	UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
-	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
-		if(!_model.load(calibrationFolder, cameraName))
+		ULOGGER_ERROR("Images path or scans path should be set!");
-		{
-			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-					cameraName.c_str(), calibrationFolder.c_str());
-		}
-		else
-		{
-			UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
-					_model.fx(),
-					_model.fy(),
-					_model.cx(),
-					_model.cy());
-		}
-	}
-	_model.setName(cameraName);
-
-	_model.setLocalTransform(this->getLocalTransform());
-	if(_rectifyImages && !_model.isValidForRectification())
-	{
-		UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
 		return false;
 	}
 
-	bool success = _dir->isValid();
+	if(_dir)
+	{
+		// look for calibration files
+		UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
+		if(!calibrationFolder.empty() && !cameraName.empty())
+		{
+			if(!_model.load(calibrationFolder, cameraName))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+						cameraName.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+						_model.fx(),
+						_model.fy(),
+						_model.cx(),
+						_model.cy());
+			}
+		}
+		_model.setName(cameraName);
+
+		_model.setLocalTransform(this->getLocalTransform());
+		if(_rectifyImages && !_model.isValidForRectification())
+		{
+			UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
+			return false;
+		}
+	}
+
+	bool success = _dir|| _scanDir;
 	_stamps.clear();
 	odometry_.clear();
 	groundTruth_.clear();
 	if(success)
 	{
-		if(_hasConfigForEachFrame)
+		if(_dir && _hasConfigForEachFrame)
 		{
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2)
 			UDirectory dirJson(_path, "yaml xml");
@@ -322,7 +336,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		}
 		else if(_filenamesAreTimestamps)
 		{
-			const std::list<std::string> & filenames = _dir->getFileNames();
+			std::list<std::string> filenames = _dir?_dir->getFileNames():_scanDir->getFileNames();
 			for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
 			{
 				// format is text_1223445645.12334_text.png or text_122344564512334_text.png
@@ -461,7 +475,7 @@ bool CameraImages::readPoses(
 				(int)poses.size(), this->imagesCount(), filePath.c_str());
 		return false;
 	}
-	else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && inOutStamps.size() == 0)
+	else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
 	{
 		UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
 		return false;
@@ -478,6 +492,11 @@ bool CameraImages::readPoses(
 		}
 		std::vector<double> values = uValues(stamps);
 
+		if(inOutStamps.empty())
+		{
+			inOutStamps = uValuesList(stamps);
+		}
+
 		int validPoses = 0;
 		for(std::list<double>::iterator ster=inOutStamps.begin(); ster!=inOutStamps.end(); ++ster)
 		{
@@ -489,6 +508,7 @@ bool CameraImages::readPoses(
 				if(endIter->first == *ster)
 				{
 					pose = poses.at(endIter->second);
+					++validPoses;
 				}
 				else if(endIter != stampsToIds.begin())
 				{
@@ -560,7 +580,8 @@ bool CameraImages::readPoses(
 
 bool CameraImages::isCalibrated() const
 {
-	return _model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection());
+	return (_dir && (_model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection()))) ||
+			_scanDir;
 }
 
 std::string CameraImages::getSerial() const
@@ -574,6 +595,10 @@ unsigned int CameraImages::imagesCount() const
 	{
 		return (unsigned int)_dir->getFileNames().size();
 	}
+	else if(_scanDir)
+	{
+		return (unsigned int)_scanDir->getFileNames().size();
+	}
 	return 0;
 }
 
@@ -583,6 +608,10 @@ std::vector<std::string> CameraImages::filenames() const
 	{
 		return uListToVector(_dir->getFileNames());
 	}
+	else if(_scanDir)
+	{
+		return uListToVector(_scanDir->getFileNames());
+	}
 	return std::vector<std::string>();
 }
 
@@ -628,11 +657,14 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	cv::Mat depthFromScan;
 	CameraModel model = _model;
 	UDEBUG("");
-	if(_dir->isValid())
+	if(_dir || _scanDir)
 	{
 		if(_refreshDir)
 		{
-			_dir->update();
+			if(_dir)
+			{
+				_dir->update();
+			}
 			if(_scanDir)
 			{
 				_scanDir->update();
@@ -642,13 +674,16 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		std::string scanFilePath;
 		if(_startAt < 0)
 		{
-			const std::list<std::string> & fileNames = _dir->getFileNames();
+			if(_dir)
-			if(fileNames.size())
 			{
-				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
+				const std::list<std::string> & fileNames = _dir->getFileNames();
+				if(fileNames.size())
 				{
-					_lastFileName = *fileNames.rbegin();
+					if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
-					imageFilePath = _path + _lastFileName;
+					{
+						_lastFileName = *fileNames.rbegin();
+						imageFilePath = _path + _lastFileName;
+					}
 				}
 			}
 			if(_scanDir)
@@ -696,11 +731,12 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		}
 		else
 		{
-			std::string fileName;
+			std::string imageFileName = _dir?_dir->getNextFileName():"";
-			fileName = _dir->getNextFileName();
+			std::string scanFileName = _scanDir?_scanDir->getNextFileName():"";
-			if(!fileName.empty())
+			if((_dir && !imageFileName.empty()) || (!_dir && !scanFileName.empty()))
 			{
-				imageFilePath = _path + fileName;
+				imageFilePath = _path + imageFileName;
+				scanFilePath = _scanPath + scanFileName;
 				if(_stamps.size())
 				{
 					stamp = _stamps.front();
@@ -731,9 +767,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					_models.pop_front();
 				}
 
-				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
+				while(_count++ < _startAt)
 				{
-					imageFilePath = _path + fileName;
+					imageFileName = _dir?_dir->getNextFileName():"";
+					scanFileName = _scanDir?_scanDir->getNextFileName():"";
+
+					if((_dir && imageFileName.empty()) || (!_dir && scanFileName.empty()))
+					{
+						break;
+					}
+
+					imageFilePath = _path + imageFileName;
+					scanFilePath = _scanPath + scanFileName;
 					if(_stamps.size())
 					{
 						stamp = _stamps.front();
@@ -765,18 +810,6 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					}
 				}
 			}
-			if(_scanDir)
-			{
-				fileName = _scanDir->getNextFileName();
-				if(!fileName.empty())
-				{
-					scanFilePath = _scanPath + fileName;
-					while(_countScan++ < _startAt && (fileName = _scanDir->getNextFileName()).size())
-					{
-						scanFilePath = _scanPath + fileName;
-					}
-				}
-			}
 		}
 
 		if(_maxFrames <=0 || ++_framesPublished <= _maxFrames)
@@ -890,8 +923,12 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		model.setImageSize(img.size());
 	}
 
-	SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
+	SensorData data;
-	data.setGroundTruth(groundTruthPose);
+	if(!img.empty() || !scan.empty())
+	{
+		data = SensorData(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
+		data.setGroundTruth(groundTruthPose);
+	}
 
 	if(info && !odometryPose.isNull())
 	{

corelib/src/camera/CameraRealSense2.cpp

@@ -127,7 +127,7 @@ void CameraRealSense2::close()
 	{
 		UINFO("%s", error.what());
 	}
-	
+
 	closing_ = false;
 }
 
@@ -179,7 +179,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
 				pose.rotation.y,
 				pose.rotation.w);
 
-	UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
+	//UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
 
 	UScopeMutex sm(poseMutex_);
 	poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
@@ -191,7 +191,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)
@@ -486,7 +486,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	UINFO("setupDevice...");
 
 	close();
-	
+
 	clockSyncWarningShown_ = false;
 	imuGlobalSyncWarningShown_ = false;
 
@@ -498,32 +498,40 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	}
 
 	bool found=false;
-	for (rs2::device dev : list)
+	try
 	{
-		auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+		for (rs2::device dev : list)
-		auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
-		uint16_t pid;
-		std::stringstream ss;
-		ss << std::hex << pid_str;
-		ss >> pid;
-		UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
-		if(dualMode_ && pid == 0x0B37)
 		{
-			// Dual setup: device[0] = D400, device[1] = T265
+			auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
-			// T265
+			auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
-			dev_.resize(2);
+
-			dev_[1] = dev;
+			uint16_t pid;
-		}
+			std::stringstream ss;
-		else if (!found && (deviceId_.empty() || deviceId_ == sn))
+			ss << std::hex << pid_str;
-		{
+			ss >> pid;
-			if(dev_.empty())
+			UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
+			if(dualMode_ && pid == 0x0B37)
 			{
-				dev_.resize(1);
+				// Dual setup: device[0] = D400, device[1] = T265
+				// T265
+				dev_.resize(2);
+				dev_[1] = dev;
+			}
+			else if (!found && (deviceId_.empty() || deviceId_ == sn))
+			{
+				if(dev_.empty())
+				{
+					dev_.resize(1);
+				}
+				dev_[0] = dev;
+				found=true;
 			}
-			dev_[0] = dev;
-			found=true;
 		}
 	}
+	catch(const rs2::error & error)
+	{
+		UWARN("%s. Is the camera already used with another app?", error.what());
+	}
 
 	if (!found)
 	{
@@ -1405,27 +1413,37 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					{
 						++iterB;
 					}
-					if(iterA != iterB)
+					std::vector<double> stamps;
+					for(;iterA != iterB;++iterA)
 					{
-						int pub = 0;
+						stamps.push_back(iterA->first);
-						for(;iterA != iterB;++iterA)
-						{
-							Transform tmp;
-							IMU imuTmp;
-							getPoseAndIMU(iterA->first, tmp, confidence, imuTmp);
-							if(!imuTmp.empty())
-							{
-								UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
-								pub++;
-							}
-							else
-							{
-								break;
-							}
-						}
-						UDEBUG("inter imu published=%d, %f -> %f", pub, lastImuStamp_, imuStamp);
 					}
 					imuMutex_.unlock();
+
+					int pub = 0;
+					for(size_t i=0; i<stamps.size(); ++i)
+					{
+						Transform tmp;
+						IMU imuTmp;
+						getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
+						if(!imuTmp.empty())
+						{
+							UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
+							pub++;
+						}
+						else
+						{
+							break;
+						}
+					}
+					if(stamps.size())
+					{
+						UDEBUG("inter imu published=%d (rate=%fHz), %f -> %f", pub, double(pub)/((stamps.back()-stamps.front())/1000.0), stamps.front()/1000.0, stamps.back()/1000.0);
+					}
+					else
+					{
+						UWARN("No inter imu published!?");
+					}
 				}
 				lastImuStamp_ = imuStamp;
 			}

corelib/src/camera/CameraStereoVideo.cpp

@@ -31,6 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/imgproc/types_c.h>
 #if CV_MAJOR_VERSION > 3
 #include <opencv2/videoio/videoio_c.h>
+#if CV_MAJOR_VERSION > 4
+#include <opencv2/videoio/legacy/constants_c.h>
+#endif
 #endif
 
 namespace rtabmap
@@ -51,7 +54,9 @@ CameraStereoVideo::CameraStereoVideo(
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
 		usbDevice_(0),
-		usbDevice2_(-1)
+		usbDevice2_(-1),
+		_width(0),
+		_height(0)
 {
 }
 
@@ -67,7 +72,9 @@ CameraStereoVideo::CameraStereoVideo(
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
 		usbDevice_(0),
-		usbDevice2_(-1)
+		usbDevice2_(-1),
+		_width(0),
+		_height(0)
 {
 }
 
@@ -80,7 +87,9 @@ CameraStereoVideo::CameraStereoVideo(
 	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
 	usbDevice_(device),
-	usbDevice2_(-1)
+	usbDevice2_(-1),
+	_width(0),
+	_height(0)
 {
 }
 
@@ -94,7 +103,9 @@ CameraStereoVideo::CameraStereoVideo(
 	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
 	usbDevice_(deviceLeft),
-	usbDevice2_(deviceRight)
+	usbDevice2_(deviceRight),
+	_width(0),
+	_height(0)
 {
 }
 
@@ -183,6 +194,37 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	}
 
 	stereoModel_.setLocalTransform(this->getLocalTransform());
+
+	if(src_ == CameraVideo::kUsbDevice)
+	{
+		if(stereoModel_.isValidForProjection())
+		{
+			if(capture_.isOpened())
+			{
+				capture_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.left().imageWidth()*(capture2_.isOpened()?1:2));
+				capture_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.left().imageHeight());
+				if(capture2_.isOpened())
+				{
+					capture2_.set(CV_CAP_PROP_FRAME_WIDTH, stereoModel_.right().imageWidth());
+					capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, stereoModel_.right().imageHeight());
+				}
+			}
+		}
+		else if(_width > 0 && _height > 0)
+		{
+			if(capture_.isOpened())
+			{
+				capture_.set(CV_CAP_PROP_FRAME_WIDTH, _width*(capture2_.isOpened()?1:2));
+				capture_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
+				if(capture2_.isOpened())
+				{
+					capture2_.set(CV_CAP_PROP_FRAME_WIDTH, _width);
+					capture2_.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
+				}
+			}
+		}
+	}
+
 	if(rectifyImages_ && !stereoModel_.isValidForRectification())
 	{
 		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");

corelib/src/camera/CameraStereoZedOC.cpp

@@ -0,0 +1,795 @@
+/*
+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/camera/CameraStereoZedOC.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#ifdef RTABMAP_ZEDOC
+#define VIDEO_MOD_AVAILABLE
+#define SENSORS_MOD_AVAILABLE
+#include <zed-open-capture/videocapture.hpp>
+#include <zed-open-capture/sensorcapture.hpp>
+#include "SimpleIni.h"
+
+///////////////////////////////////////////////////////////////////////////
+//
+// Copyright (c) 2018, STEREOLABS.
+//
+// All rights reserved.
+//
+// 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
+// OWNER 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.
+//
+///////////////////////////////////////////////////////////////////////////
+
+inline std::vector<std::string> &split(const std::string &s, char delim, std::vector<std::string> &elems) {
+    std::stringstream ss(s);
+    std::string item;
+    while (getline(ss, item, delim)) {
+        elems.push_back(item);
+    }
+    return elems;
+}
+
+inline std::vector<std::string> split(const std::string &s, char delim) {
+    std::vector<std::string> elems;
+    split(s, delim, elems);
+    return elems;
+}
+
+class ConfManager {
+public:
+
+    ConfManager(std::string filename) {
+        filename_ = filename;
+        ini_.SetUnicode();
+        SI_Error rc = ini_.LoadFile(filename_.c_str());
+        is_opened_ = !(rc < 0);
+    }
+
+    ~ConfManager() {
+        //if (is_opened_) ini_.SaveFile(filename_.c_str());
+    }
+
+    float getValue(std::string key, float default_value = -1) {
+        if (is_opened_) {
+            std::vector<std::string> elems;
+            split(key, ':', elems);
+
+            return atof(ini_.GetValue(elems.front().c_str(), elems.back().c_str(), std::to_string(default_value).c_str()));
+        } else
+            return -1.f;
+    }
+
+    void setValue(std::string key, float value) {
+        if (is_opened_) {
+            std::vector<std::string> elems;
+            split(key, ':', elems);
+
+            /*SI_Error rc = */ini_.SetValue(elems.front().c_str(), elems.back().c_str(), std::to_string(value).c_str());
+        }
+    }
+
+    inline bool isOpened() {
+        return is_opened_;
+    }
+
+private:
+    std::string filename_;
+    bool is_opened_;
+    CSimpleIniA ini_;
+};
+
+bool checkFile(std::string path) {
+    std::ifstream f(path.c_str());
+    return f.good();
+}
+
+static inline std::string getRootHiddenDir() {
+#ifdef WIN32
+
+#ifdef UNICODE
+    wchar_t szPath[MAX_PATH];
+#else
+    TCHAR szPath[MAX_PATH];
+#endif
+
+    if (!SUCCEEDED(SHGetFolderPath(NULL, CSIDL_COMMON_APPDATA, NULL, 0, szPath)))
+        return "";
+
+    char snfile_path[MAX_PATH];
+
+#ifndef UNICODE
+
+    size_t newsize = strlen(szPath) + 1;
+    wchar_t * wcstring = new wchar_t[newsize];
+    // Convert char* string to a wchar_t* string.
+    size_t convertedChars = 0;
+    mbstowcs_s(&convertedChars, wcstring, newsize, szPath, _TRUNCATE);
+    wcstombs(snfile_path, wcstring, MAX_PATH);
+#else
+    wcstombs(snfile_path, szPath, MAX_PATH);
+#endif
+
+    std::string filename(snfile_path);
+    filename += "\\Stereolabs\\";
+
+#else //LINUX
+    std::string homepath = getenv("HOME");
+    std::string filename = homepath + "/zed/";
+#endif
+
+    return filename;
+}
+
+/*return the path to the Sl ZED hidden dir*/
+static inline std::string getHiddenDir() {
+    std::string filename = getRootHiddenDir();
+#ifdef WIN32
+    filename += "settings\\";
+#else //LINUX
+    filename += "settings/";
+#endif
+    return filename;
+}
+
+bool downloadCalibrationFile(unsigned int serial_number, std::string &calibration_file) {
+#ifndef _WIN32
+    std::string path = getHiddenDir();
+    char specific_name[128];
+    sprintf(specific_name, "SN%d.conf", serial_number);
+    calibration_file = path + specific_name;
+    if (!checkFile(calibration_file)) {
+        std::string cmd;
+        int res;
+
+        // Create download folder
+        cmd = "mkdir -p " + path;
+        res = system(cmd.c_str());
+
+        // Download the file
+        std::string url("'https://calib.stereolabs.com/?SN=");
+
+        cmd = "wget " + url + std::to_string(serial_number) + "' -O " + calibration_file;
+        std::cout << cmd << std::endl;
+        res = system(cmd.c_str());
+
+        if( res == EXIT_FAILURE )
+        {
+            std::cerr << "Error downloading the calibration file" << std::endl;
+            return false;
+        }
+
+        if (!checkFile(calibration_file)) {
+            std::cerr << "Invalid calibration file" << std::endl;
+            return false;
+        }
+    }
+#else
+    std::string path = getHiddenDir();
+    char specific_name[128];
+    sprintf(specific_name, "SN%d.conf", serial_number);
+    calibration_file = path + specific_name;
+    if (!checkFile(calibration_file)) {
+        TCHAR *settingFolder = new TCHAR[path.size() + 1];
+        settingFolder[path.size()] = 0;
+        std::copy(path.begin(), path.end(), settingFolder);
+        SHCreateDirectoryEx(NULL, settingFolder, NULL); //recursive creation
+
+        std::string url("https://calib.stereolabs.com/?SN=");
+        url += std::to_string(serial_number);
+        TCHAR *address = new TCHAR[url.size() + 1];
+        address[url.size()] = 0;
+        std::copy(url.begin(), url.end(), address);
+        TCHAR *calibPath = new TCHAR[calibration_file.size() + 1];
+        calibPath[calibration_file.size()] = 0;
+        std::copy(calibration_file.begin(), calibration_file.end(), calibPath);
+
+        HRESULT hr = URLDownloadToFile(NULL, address, calibPath, 0, NULL);
+        if (hr != 0) {
+            std::cout << "Fail to download calibration file" << std::endl;
+            return false;
+        }
+
+        if (!checkFile(calibration_file)) {
+            std::cout << "Invalid calibration file" << std::endl;
+            return false;
+        }
+    }
+#endif
+
+    return true;
+}
+
+bool initCalibration(std::string calibration_file, cv::Size image_size, rtabmap::StereoCameraModel & model, const rtabmap::Transform & localTransform) {
+
+    if (!checkFile(calibration_file)) {
+        std::cout << "Calibration file missing." << std::endl;
+        return false;
+    }
+
+    // Open camera configuration file
+    ConfManager camerareader(calibration_file.c_str());
+    if (!camerareader.isOpened())
+        return false;
+
+    std::string resolution_str;
+    switch ((int) image_size.width) {
+        case 2208:
+            resolution_str = "2k";
+            break;
+        case 1920:
+            resolution_str = "fhd";
+            break;
+        case 1280:
+            resolution_str = "hd";
+            break;
+        case 672:
+            resolution_str = "vga";
+            break;
+        default:
+            resolution_str = "hd";
+            break;
+    }
+
+    // Get translations
+    float T_[3];
+    T_[0] = camerareader.getValue("stereo:baseline", 0.0f);
+    T_[1] = camerareader.getValue("stereo:ty_" + resolution_str, 0.f);
+    if(T_[1] == 0.f)
+    {
+    	T_[1] = camerareader.getValue("stereo:ty", 0.f);
+    }
+    T_[2] = camerareader.getValue("stereo:tz_" + resolution_str, 0.f);
+    if(T_[2] == 0.f)
+    {
+	    T_[2] = camerareader.getValue("stereo:tz", 0.f);
+    }
+
+    // Get left parameters
+    float left_cam_cx = camerareader.getValue("left_cam_" + resolution_str + ":cx", 0.0f);
+    float left_cam_cy = camerareader.getValue("left_cam_" + resolution_str + ":cy", 0.0f);
+    float left_cam_fx = camerareader.getValue("left_cam_" + resolution_str + ":fx", 0.0f);
+    float left_cam_fy = camerareader.getValue("left_cam_" + resolution_str + ":fy", 0.0f);
+    float left_cam_k1 = camerareader.getValue("left_cam_" + resolution_str + ":k1", 0.0f);
+    float left_cam_k2 = camerareader.getValue("left_cam_" + resolution_str + ":k2", 0.0f);
+    float left_cam_p1 = camerareader.getValue("left_cam_" + resolution_str + ":p1", 0.0f);
+    float left_cam_p2 = camerareader.getValue("left_cam_" + resolution_str + ":p2", 0.0f);
+    float left_cam_k3 = camerareader.getValue("left_cam_" + resolution_str + ":k3", 0.0f);
+
+    // Get right parameters
+    float right_cam_cx = camerareader.getValue("right_cam_" + resolution_str + ":cx", 0.0f);
+    float right_cam_cy = camerareader.getValue("right_cam_" + resolution_str + ":cy", 0.0f);
+    float right_cam_fx = camerareader.getValue("right_cam_" + resolution_str + ":fx", 0.0f);
+    float right_cam_fy = camerareader.getValue("right_cam_" + resolution_str + ":fy", 0.0f);
+    float right_cam_k1 = camerareader.getValue("right_cam_" + resolution_str + ":k1", 0.0f);
+    float right_cam_k2 = camerareader.getValue("right_cam_" + resolution_str + ":k2", 0.0f);
+    float right_cam_p1 = camerareader.getValue("right_cam_" + resolution_str + ":p1", 0.0f);
+    float right_cam_p2 = camerareader.getValue("right_cam_" + resolution_str + ":p2", 0.0f);
+    float right_cam_k3 = camerareader.getValue("right_cam_" + resolution_str + ":k3", 0.0f);
+
+    // (Linux only) Safety check A: Wrong "." or "," reading in file conf.
+#ifndef _WIN32
+    if (right_cam_k1 == 0 && left_cam_k1 == 0 && left_cam_k2 == 0 && right_cam_k2 == 0) {
+        UERROR("ZED File invalid");
+
+        std::string cmd = "rm " + calibration_file;
+        int res = system(cmd.c_str());
+        if( res == EXIT_FAILURE )
+	   {
+		 	return false;
+	   }
+       	return false;
+    }
+#endif
+
+    // Get rotations
+    cv::Mat R_zed = (cv::Mat_<double>(1, 3) << camerareader.getValue("stereo:rx_" + resolution_str, 0.f), camerareader.getValue("stereo:cv_" + resolution_str, 0.f), camerareader.getValue("stereo:rz_" + resolution_str, 0.f));
+    //R_zed *= -1.f; // FIXME: we had to invert T below, do we need to do this with R? I don't see much difference looking at the disparity image
+    cv::Mat R;
+
+    cv::Rodrigues(R_zed /*in*/, R /*out*/);
+
+    cv::Mat distCoeffs_left, distCoeffs_right;
+
+    // Left
+    cv::Mat cameraMatrix_left = (cv::Mat_<double>(3, 3) << left_cam_fx, 0, left_cam_cx, 0, left_cam_fy, left_cam_cy, 0, 0, 1);
+    distCoeffs_left = (cv::Mat_<double>(1, 5) << left_cam_k1, left_cam_k2, left_cam_p1, left_cam_p2, left_cam_k3);
+
+    // Right
+    cv::Mat cameraMatrix_right = (cv::Mat_<double>(3, 3) << right_cam_fx, 0, right_cam_cx, 0, right_cam_fy, right_cam_cy, 0, 0, 1);
+    distCoeffs_right = (cv::Mat_<double>(1, 5) << right_cam_k1, right_cam_k2, right_cam_p1, right_cam_p2, right_cam_k3);
+
+    // Stereo
+    cv::Mat T = (cv::Mat_<double>(3, 1) << T_[0], T_[1], T_[2]);
+    T /= -1000.f; // convert in meters, inverted to get positive baseline
+    //std::cout << " Camera Matrix L: \n" << cameraMatrix_left << std::endl << std::endl;
+    //std::cout << " Camera Matrix R: \n" << cameraMatrix_right << std::endl << std::endl;
+    //std::cout << " Camera Rotation: \n" << R << std::endl << std::endl;
+    //std::cout << " Camera Translation: \n" << T << std::endl << std::endl;
+
+    cv::Mat R1, R2, P1, P2, Q;
+	cv::stereoRectify(cameraMatrix_left, distCoeffs_left, cameraMatrix_right, distCoeffs_right, image_size, R, T,
+			R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 0, image_size);
+
+	model = rtabmap::StereoCameraModel("zed",
+			image_size, cameraMatrix_left, distCoeffs_left, R1, P1,
+			image_size, cameraMatrix_right, distCoeffs_right, R2, P2,
+			R, T, cv::Mat(), cv::Mat(), localTransform);
+	return true;
+}
+
+#endif
+
+namespace rtabmap
+{
+
+#ifdef RTABMAP_ZEDOC
+
+class ZedOCThread: public UThread
+{
+public:
+	ZedOCThread(sl_oc::sensors::SensorCapture* sensCap, const Transform & imuLocalTransform)
+	{
+		sensCap_= sensCap;
+		imuLocalTransform_ = imuLocalTransform;
+	}
+
+	void getIMU(
+			const double & stamp,
+			IMU & imu,
+			int maxWaitTimeMs)
+	{
+		imu = IMU();
+		if(imuBuffer_.empty())
+		{
+			return;
+		}
+
+		// Interpolate imu
+		cv::Vec3d acc;
+		cv::Vec3d gyr;
+
+		int waitTry = 0;
+		imuMutex_.lock();
+		while(maxWaitTimeMs > 0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		{
+			imuMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			imuMutex_.lock();
+		}
+		bool set = false;
+		if(imuBuffer_.rbegin()->first < stamp)
+		{
+			if(maxWaitTimeMs>0)
+			{
+				UWARN("Could not find imu data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, imuBuffer_.rbegin()->first);
+			}
+		}
+		else
+		{
+			std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterB = imuBuffer_.lower_bound(stamp);
+			std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterA = iterB;
+			if(iterA != imuBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == imuBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				acc[0] = iterA->second.first[0];
+				acc[1] = iterA->second.first[1];
+				acc[2] = iterA->second.first[2];
+				gyr[0] = iterA->second.second[0];
+				gyr[1] = iterA->second.second[1];
+				gyr[2] = iterA->second.second[2];
+				set = true;
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+				acc[0] = iterA->second.first[0] + t*(iterB->second.first[0] - iterA->second.first[0]);
+				acc[1] = iterA->second.first[1] + t*(iterB->second.first[1] - iterA->second.first[1]);
+				acc[2] = iterA->second.first[2] + t*(iterB->second.first[2] - iterA->second.first[2]);
+				gyr[0] = iterA->second.second[0] + t*(iterB->second.second[0] - iterA->second.second[0]);
+				gyr[1] = iterA->second.second[1] + t*(iterB->second.second[1] - iterA->second.second[1]);
+				gyr[2] = iterA->second.second[2] + t*(iterB->second.second[2] - iterA->second.second[2]);
+				set = true;
+			}
+			else
+			{
+				if(stamp < iterA->first)
+				{
+					UDEBUG("Could not find imu data to interpolate at image time %f (earliest is %f). Are sensors synchronized? (may take some time to be synchronized...)", stamp, iterA->first);
+				}
+				else
+				{
+					UDEBUG("Could not find imu data to interpolate at image time %f (between %f and %f). Are sensors synchronized? (may take some time to be synchronized...)", stamp, iterA->first, iterB->first);
+				}
+			}
+		}
+		imuMutex_.unlock();
+
+		if(set)
+		{
+			imu = IMU(gyr, cv::Mat::eye(3, 3, CV_64FC1),
+					acc, cv::Mat::eye(3, 3, CV_64FC1),
+					imuLocalTransform_);
+		}
+
+		return;
+	}
+private:
+	virtual void mainLoop()
+	{
+		// ----> Get IMU data
+		const sl_oc::sensors::data::Imu imuData = sensCap_->getLastIMUData(2000);
+
+		// Process data only if valid
+		if(imuData.valid == sl_oc::sensors::data::Imu::NEW_VAL ) // Uncomment to use only data syncronized with the video frames
+		{
+			UScopeMutex sm(imuMutex_);
+			static double deg2rad = 0.017453293;
+			std::pair<cv::Vec3d, cv::Vec3d> imu(
+					cv::Vec3d(imuData.aX, imuData.aY, imuData.aZ),
+					cv::Vec3d(imuData.gX*deg2rad, imuData.gY*deg2rad, imuData.gZ*deg2rad));
+
+			double stamp = double(imuData.timestamp)/10e8;
+			if(!imuBuffer_.empty() && imuBuffer_.rbegin()->first > stamp)
+			{
+				UWARN("IMU data not received in order, reset buffer! (previous=%f new=%f)", imuBuffer_.rbegin()->first, stamp);
+				imuBuffer_.clear();
+			}
+			imuBuffer_.insert(imuBuffer_.end(), std::make_pair(stamp, imu));
+			if(imuBuffer_.size() > 1000)
+			{
+				imuBuffer_.erase(imuBuffer_.begin());
+			}
+		}
+
+	}
+	sl_oc::sensors::SensorCapture* sensCap_;
+	Transform imuLocalTransform_;
+	UMutex imuMutex_;
+	std::map<double, std::pair<cv::Vec3f, cv::Vec3f> > imuBuffer_;
+};
+
+#endif
+
+bool CameraStereoZedOC::available()
+{
+#ifdef RTABMAP_ZEDOC
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoZedOC::CameraStereoZedOC(
+		int deviceId,
+		int resolution,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_ZEDOC
+    ,
+	zed_(0),
+	sensors_(0),
+	imuThread_(0),
+	usbDevice_(deviceId),
+	resolution_(resolution),
+	lastStamp_(0)
+#endif
+{
+	UDEBUG("");
+#ifdef RTABMAP_ZEDOC
+
+	sl_oc::video::RESOLUTION res = static_cast<sl_oc::video::RESOLUTION>(resolution_);
+    UASSERT(res >= sl_oc::video::RESOLUTION::HD2K && res < sl_oc::video::RESOLUTION::LAST);
+#endif
+}
+
+CameraStereoZedOC::~CameraStereoZedOC()
+{
+#ifdef RTABMAP_ZEDOC
+	if(imuThread_)
+	{
+		imuThread_->join(true);
+		delete imuThread_;
+	}
+	delete zed_;
+	delete sensors_;
+#endif
+}
+
+bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+#ifdef RTABMAP_ZEDOC
+	if(imuThread_)
+	{
+		imuThread_->join(true);
+		delete imuThread_;
+		imuThread_=0;
+	}
+	if(zed_)
+	{
+		delete zed_;
+		zed_ = 0;
+	}
+	if(sensors_)
+	{
+		delete sensors_;
+		sensors_ = 0;
+	}
+	lastStamp_ = 0;
+
+	// ----> Set Video parameters
+	sl_oc::video::VideoParams params;
+	params.res = static_cast<sl_oc::video::RESOLUTION>(resolution_);
+
+	params.fps = sl_oc::video::FPS::FPS_15;
+	if(this->getImageRate() > 60)
+	{
+		params.fps = sl_oc::video::FPS::FPS_100;
+	}
+	else if(this->getImageRate() > 30)
+	{
+		params.fps = sl_oc::video::FPS::FPS_60;
+	}
+	else if(this->getImageRate() > 15)
+	{
+		params.fps = sl_oc::video::FPS::FPS_30;
+	}
+
+	if(ULogger::level() <= ULogger::kInfo)
+	{
+		params.verbose = sl_oc::VERBOSITY::INFO;
+	}
+	else if(ULogger::level() <= ULogger::kWarning)
+	{
+		params.verbose = sl_oc::VERBOSITY::WARNING;
+	}
+	// <---- Set Video parameters
+
+	// ----> Create Video Capture
+	zed_ = new sl_oc::video::VideoCapture(params);
+	if( !zed_->initializeVideo(usbDevice_) )
+	{
+		UERROR("Cannot open camera video capture. Set log level <= info for more details.");
+
+		delete zed_;
+		zed_ = 0;
+		return false;
+	}
+	int sn = zed_->getSerialNumber();
+	UINFO("Connected to camera sn: %d", sn);
+	// <---- Create Video Capture
+
+	// ----> Retrieve calibration file from Stereolabs server
+	std::string calibration_file;
+	// ZED Calibration
+	unsigned int serial_number = sn;
+	// Download camera calibration file
+	if( !downloadCalibrationFile(serial_number, calibration_file) )
+	{
+		UERROR("Could not load calibration file from Stereolabs servers");
+		delete zed_;
+		zed_ = 0;
+		return false;
+	}
+	UINFO("Calibration file found. Loading...");
+
+	// ----> Frame size
+	int w,h;
+	zed_->getFrameSize(w,h);
+	// <---- Frame size
+
+	 // ----> Initialize calibration
+	if(initCalibration(calibration_file, cv::Size(w/2,h), stereoModel_, this->getLocalTransform()))
+	{
+		if(ULogger::level() <= ULogger::kInfo)
+		{
+			std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
+			std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
+		}
+		stereoModel_.initRectificationMap();
+	}
+
+	// ----> Create a Sensors Capture object
+	sensors_ = new sl_oc::sensors::SensorCapture((sl_oc::VERBOSITY)params.verbose);
+	if( !sensors_->initializeSensors(serial_number) ) // Note: we use the serial number acquired by the VideoCapture object
+	{
+		UERROR("Cannot open sensors capture. Set log level <= info for more details.");
+		delete sensors_;
+		sensors_ = 0;
+	}
+	else
+	{
+		UINFO("Sensors Capture connected to camera sn: %d", sensors_->getSerialNumber());
+		UINFO("Wait max 5 sec to see if the camera has imu...");
+		// Check is IMU data is available
+		UTimer timer;
+		while(timer.elapsed() < 5 &&
+			  sensors_->getLastIMUData().valid != sl_oc::sensors::data::Imu::NEW_VAL)
+		{
+			// wait 5 sec to see if we can get an imu stream...
+			uSleep(100);
+		}
+		if(timer.elapsed() > 5)
+		{
+			UINFO("Camera doesn't have IMU sensor");
+		}
+		else
+		{
+			UINFO("Camera has IMU");
+
+			// Start the sensor capture thread. Note: since sensor data can be retrieved at 400Hz and video data frequency is
+			// minor (max 100Hz), we use a separated thread for sensors.
+			// Transform based on ZED2: x->down, y->right, z->backward
+			Transform imuLocalTransform_ = this->getLocalTransform() * Transform(0,1,0,0, 1,0,0,0, 0,0,-1,0);
+			//std::cout << imuLocalTransform_ << std::endl;
+			imuThread_ = new ZedOCThread(sensors_, imuLocalTransform_);
+			// <---- Create Sensors Capture
+
+			// ----> Enable video/sensors synchronization
+			if(!zed_->enableSensorSync(sensors_))
+			{
+				UWARN("Failed to enable image/imu synchronization");
+			}
+			// <---- Enable video/sensors synchronization
+
+			imuThread_->start();
+		}
+	}
+
+	return true;
+#else
+	UERROR("CameraStereoZEDOC: RTAB-Map is not built with ZED Open Capture support!");
+#endif
+	return false;
+}
+
+bool CameraStereoZedOC::isCalibrated() const
+{
+#ifdef RTABMAP_ZEDOC
+	return stereoModel_.isValidForProjection();
+#else
+	return false;
+#endif
+}
+
+std::string CameraStereoZedOC::getSerial() const
+{
+#ifdef RTABMAP_ZEDOC
+	if(zed_)
+	{
+		return uFormat("%x", zed_->getSerialNumber());
+	}
+#endif
+	return "";
+}
+
+SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_ZEDOC
+	// Get a new frame from camera
+	if(zed_)
+	{
+		UTimer timer;
+
+		bool imuReceived = imuThread_!=0;
+		bool warned = false;
+		do
+		{
+			const sl_oc::video::Frame frame = zed_->getLastFrame();
+
+			if(frame.data!=nullptr && frame.timestamp!=lastStamp_)
+			{
+				lastStamp_ = frame.timestamp;
+
+				double stamp = double(lastStamp_)/10e8;
+
+				// If the sensor supports IMU, wait IMU to be available before sending data.
+				IMU imu;
+				if(imuThread_)
+				{
+					imuThread_->getIMU(stamp, imu, 10);
+					imuReceived = !imu.empty();
+					if(!imuReceived && !warned && timer.elapsed() > 1.0)
+					{
+						UWARN("Waiting for synchronized imu (this can take several seconds when camera has been just started)...");
+						warned = true;
+					}
+				}
+
+				if(imuReceived)
+				{
+					cv::Mat frameBGR, left, right;
+
+					// ----> Conversion from YUV 4:2:2 to BGR for visualization
+					cv::Mat frameYUV = cv::Mat( frame.height, frame.width, CV_8UC2, frame.data );
+					cv::cvtColor(frameYUV,frameBGR,cv::COLOR_YUV2BGR_YUYV);
+					// <---- Conversion from YUV 4:2:2 to BGR for visualization
+
+					// ----> Extract left and right images from side-by-side
+					left = frameBGR(cv::Rect(0, 0, frameBGR.cols / 2, frameBGR.rows));
+					cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
+					// <---- Extract left and right images from side-by-side
+
+					if(stereoModel_.isValidForRectification())
+					{
+						left = stereoModel_.left().rectifyImage(left);
+						right = stereoModel_.right().rectifyImage(right);
+					}
+
+					data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+
+					if(!imu.empty())
+					{
+						data.setIMU(imu);
+					}
+				}
+			}
+			else
+			{
+				UERROR("CameraStereoZEDOC: Cannot get frame from the camera since 100 msec!");
+				imuReceived = true;
+			}
+		}
+		while(!imuReceived && timer.elapsed() < 15.0);
+
+		// ----> If the frame is valid we can convert, rectify and display it
+		if(!imuReceived)
+		{
+			UERROR("CameraStereoZEDOC: Cannot get synchronized IMU with camera for 15 sec!");
+		}
+	}
+#else
+	UERROR("CameraStereoZEDOC: RTAB-Map is not built with ZED Open Capture support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/icp/cccorelib.h

@@ -0,0 +1,180 @@
+/*
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CORELIB_SRC_ICP_CCCORELIB_H_
+#define CORELIB_SRC_ICP_CCCORELIB_H_
+
+#include <CCCoreLib/RegistrationTools.h>
+
+namespace rtabmap {
+
+rtabmap::Transform icpCC(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & fromCloud,
+		pcl::PointCloud<pcl::PointXYZI>::Ptr & toCloud,
+		int maxIterations = 150,
+		double minRMSDecrease = 0.00001,
+		bool force3DoF = false,
+		bool force4DoF = false,
+		int samplingLimit = 50000,
+		double finalOverlapRatio = 0.85,
+		bool filterOutFarthestPoints = false,
+		double maxFinalRMS = 0.2,
+		std::string * errorMsg = 0)
+{
+	UDEBUG("maxIterations=%d", maxIterations);
+	UDEBUG("minRMSDecrease=%f", minRMSDecrease);
+	UDEBUG("samplingLimit=%d", samplingLimit);
+	UDEBUG("finalOverlapRatio=%f", finalOverlapRatio);
+	UDEBUG("filterOutFarthestPoints=%s", filterOutFarthestPoints?"true":"false");
+	UDEBUG("force 3DoF=%s 4DoF=%s", force3DoF?"true":"false", force4DoF?"true":"false");
+	UDEBUG("maxFinalRMS=%f", maxFinalRMS);
+
+	rtabmap::Transform icpTransformation;
+
+	CCCoreLib::ICPRegistrationTools::RESULT_TYPE result;
+	CCCoreLib::PointProjectionTools::Transformation transform;
+	CCCoreLib::ICPRegistrationTools::Parameters params;
+	{
+		if(minRMSDecrease > 0.0)
+		{
+			params.convType = CCCoreLib::ICPRegistrationTools::MAX_ERROR_CONVERGENCE;
+			params.minRMSDecrease = minRMSDecrease; //! The minimum error (RMS) reduction between two consecutive steps to continue process (ignored if convType is not MAX_ERROR_CONVERGENCE)
+		}
+		else
+		{
+			params.convType = CCCoreLib::ICPRegistrationTools::MAX_ITER_CONVERGENCE;
+			params.nbMaxIterations = maxIterations; //! The maximum number of iteration (ignored if convType is not MAX_ITER_CONVERGENCE)
+		}
+		params.adjustScale = false;             //! Whether to release the scale parameter during the registration procedure or not
+		params.filterOutFarthestPoints = filterOutFarthestPoints; //! If true, the algorithm will automatically ignore farthest points from the reference, for better convergence
+		params.samplingLimit = samplingLimit; //! Maximum number of points per cloud (they are randomly resampled below this limit otherwise)
+		params.finalOverlapRatio = finalOverlapRatio;  //! Theoretical overlap ratio (at each iteration, only this percentage (between 0 and 1) will be used for registration
+		params.modelWeights = nullptr;          //! Weights for model points (i.e. only if the model entity is a cloud) (optional)
+		params.dataWeights = nullptr;           //! Weights for data points (optional)
+		params.transformationFilters = force3DoF?33:force4DoF?1:0;  //! Filters to be applied on the resulting transformation at each step (experimental) - see RegistrationTools::TRANSFORMATION_FILTERS flags
+		params.maxThreadCount = 0;              //! Maximum number of threads to use (0 = max)
+	}
+
+	double finalError = 0.0;
+	unsigned finalPointCount = 0;
+
+	CCCoreLib::PointCloud toPointCloud = CCCoreLib::PointCloud();
+	CCCoreLib::PointCloud fromPointCloud = CCCoreLib::PointCloud();
+
+	fromPointCloud.reserve(fromCloud->points.size());
+	for(uint nIndex=0; nIndex < fromCloud->points.size(); nIndex++)
+	{
+		CCVector3 P;
+		P.x = fromCloud->points[nIndex].x;
+		P.y = fromCloud->points[nIndex].y;
+		P.z = force3DoF?0:fromCloud->points[nIndex].z;
+		fromPointCloud.addPoint(P);
+	}
+	toPointCloud.reserve(toCloud->points.size());
+	for(uint nIndex=0; nIndex < toCloud->points.size(); nIndex++)
+	{
+		CCVector3 P;
+		P.x = toCloud->points[nIndex].x;
+		P.y = toCloud->points[nIndex].y;
+		P.z = force3DoF?0:toCloud->points[nIndex].z;
+		toPointCloud.addPoint(P);
+	}
+
+	UDEBUG("CCCoreLib: start ICP");
+	result = CCCoreLib::ICPRegistrationTools::Register(
+			&fromPointCloud,
+			nullptr,
+			&toPointCloud,
+			params,
+			transform,
+			finalError,
+			finalPointCount);
+	UDEBUG("CCCoreLib: ICP done!");
+
+	UDEBUG("CC ICP result: %d", result);
+	UDEBUG("CC Final error: %f . Finall Pointcount: %d", finalError, finalPointCount);
+	UDEBUG("CC ICP success Trans: %f %f %f", transform.T.x,transform.T.y,transform.T.z);
+
+	if(result != 1)
+	{
+		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as result %d !=1", result);
+		UDEBUG(msg.c_str());
+		if(errorMsg)
+		{
+			*errorMsg = msg;
+		}
+
+		icpTransformation.setNull();
+		return icpTransformation;
+	}
+	else if(finalPointCount <10)
+	{
+		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 10 ", finalPointCount);
+		UDEBUG(msg.c_str());
+		if(errorMsg)
+		{
+			*errorMsg = msg;
+		}
+
+		icpTransformation.setNull();
+		return icpTransformation;
+	}
+	//CC transform to EIgen4f
+	Eigen::Matrix4f matrix;
+	matrix.setIdentity();
+	for(int i=0;i<3;i++)
+	{
+		for(int j=0;j<3;j++)
+		{
+			matrix(i,j)=transform.R.getValue(i,j);
+		}
+	}
+	for(int i=0;i<3;i++)
+	{
+		matrix(i,3)=transform.T[i];
+	}
+
+	icpTransformation = rtabmap::Transform::fromEigen4f(matrix);
+	icpTransformation = icpTransformation.inverse();
+	UDEBUG("CC ICP result: %s", icpTransformation.prettyPrint().c_str());
+
+	if(finalError > maxFinalRMS)
+	{
+		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as RMS %f > %f (%s) ", finalError, maxFinalRMS, rtabmap::Parameters::kIcpCCMaxFinalRMS().c_str());
+		UDEBUG(msg.c_str());
+		if(errorMsg)
+		{
+			*errorMsg = msg;
+		}
+		icpTransformation.setNull();
+	}
+	return icpTransformation;
+}
+
+}
+
+#endif /* CORELIB_SRC_ICP_CCCORELIB_H_ */

corelib/src/icp/libpointmatcher.h

@@ -0,0 +1,534 @@
+/*
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef CORELIB_SRC_ICP_LIBPOINTMATCHER_H_
+#define CORELIB_SRC_ICP_LIBPOINTMATCHER_H_
+
+#include <fstream>
+#include "pointmatcher/PointMatcher.h"
+#include "nabo/nabo.h"
+typedef PointMatcher<float> PM;
+typedef PM::DataPoints DP;
+
+namespace rtabmap {
+
+DP pclToDP(const pcl::PointCloud<pcl::PointXYZI>::Ptr & pclCloud, bool is2D)
+{
+	UDEBUG("");
+	typedef DP::Label Label;
+	typedef DP::Labels Labels;
+	typedef DP::View View;
+
+	if (pclCloud->empty())
+		return DP();
+
+	// fill labels
+	// conversions of descriptor fields from pcl
+	// see http://www.ros.org/wiki/pcl/Overview
+	Labels featLabels;
+	Labels descLabels;
+	featLabels.push_back(Label("x", 1));
+	featLabels.push_back(Label("y", 1));
+	if(!is2D)
+	{
+		featLabels.push_back(Label("z", 1));
+	}
+	featLabels.push_back(Label("pad", 1));
+
+	descLabels.push_back(Label("intensity", 1));
+
+	// create cloud
+	DP cloud(featLabels, descLabels, pclCloud->size());
+	cloud.getFeatureViewByName("pad").setConstant(1);
+
+	// fill cloud
+	View view(cloud.getFeatureViewByName("x"));
+	View viewIntensity(cloud.getDescriptorRowViewByName("intensity",0));
+	for(unsigned int i=0; i<pclCloud->size(); ++i)
+	{
+		view(0, i) = pclCloud->at(i).x;
+		view(1, i) = pclCloud->at(i).y;
+		if(!is2D)
+		{
+			view(2, i) = pclCloud->at(i).z;
+		}
+		viewIntensity(0, i) = pclCloud->at(i).intensity;
+	}
+
+	return cloud;
+}
+
+DP pclToDP(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & pclCloud, bool is2D)
+{
+	UDEBUG("");
+	typedef DP::Label Label;
+	typedef DP::Labels Labels;
+	typedef DP::View View;
+
+	if (pclCloud->empty())
+		return DP();
+
+	// fill labels
+	// conversions of descriptor fields from pcl
+	// see http://www.ros.org/wiki/pcl/Overview
+	Labels featLabels;
+	Labels descLabels;
+	featLabels.push_back(Label("x", 1));
+	featLabels.push_back(Label("y", 1));
+	if(!is2D)
+	{
+		featLabels.push_back(Label("z", 1));
+	}
+	featLabels.push_back(Label("pad", 1));
+
+	descLabels.push_back(Label("normals", 3));
+	descLabels.push_back(Label("intensity", 1));
+
+	// create cloud
+	DP cloud(featLabels, descLabels, pclCloud->size());
+	cloud.getFeatureViewByName("pad").setConstant(1);
+
+	// fill cloud
+	View view(cloud.getFeatureViewByName("x"));
+	View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
+	View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
+	View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
+	View viewIntensity(cloud.getDescriptorRowViewByName("intensity",0));
+	for(unsigned int i=0; i<pclCloud->size(); ++i)
+	{
+		view(0, i) = pclCloud->at(i).x;
+		view(1, i) = pclCloud->at(i).y;
+		if(!is2D)
+		{
+			view(2, i) = pclCloud->at(i).z;
+		}
+		viewNormalX(0, i) = pclCloud->at(i).normal_x;
+		viewNormalY(0, i) = pclCloud->at(i).normal_y;
+		viewNormalZ(0, i) = pclCloud->at(i).normal_z;
+		viewIntensity(0, i) = pclCloud->at(i).intensity;
+	}
+
+	return cloud;
+}
+
+DP laserScanToDP(const rtabmap::LaserScan & scan, bool ignoreLocalTransform = false)
+{
+	UDEBUG("");
+	typedef DP::Label Label;
+	typedef DP::Labels Labels;
+	typedef DP::View View;
+
+	if (scan.isEmpty())
+		return DP();
+
+	// fill labels
+	// conversions of descriptor fields from pcl
+	// see http://www.ros.org/wiki/pcl/Overview
+	Labels featLabels;
+	Labels descLabels;
+	featLabels.push_back(Label("x", 1));
+	featLabels.push_back(Label("y", 1));
+	if(!scan.is2d())
+	{
+		featLabels.push_back(Label("z", 1));
+	}
+	featLabels.push_back(Label("pad", 1));
+
+	if(scan.hasNormals())
+	{
+		descLabels.push_back(Label("normals", 3));
+	}
+	if(scan.hasIntensity())
+	{
+		descLabels.push_back(Label("intensity", 1));
+	}
+
+
+	// create cloud
+	DP cloud(featLabels, descLabels, scan.size());
+	cloud.getFeatureViewByName("pad").setConstant(1);
+
+	// fill cloud
+	int nx = scan.getNormalsOffset();
+	int ny = nx+1;
+	int nz = ny+1;
+	int offsetI = scan.getIntensityOffset();
+	bool hasLocalTransform = !ignoreLocalTransform && !scan.localTransform().isNull() && !scan.localTransform().isIdentity();
+	View view(cloud.getFeatureViewByName("x"));
+	View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName("normals",0):view);
+	View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
+	View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
+	View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
+	int oi = 0;
+	for(int i=0; i<scan.size(); ++i)
+	{
+		const float * ptr = scan.data().ptr<float>(0, i);
+
+		if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (scan.is2d() || uIsFinite(ptr[2])))
+		{
+			if(hasLocalTransform)
+			{
+				if(nx == -1)
+				{
+					cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
+					pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
+					view(0, oi) = pt.x;
+					view(1, oi) = pt.y;
+					if(!scan.is2d())
+					{
+						view(2, oi) = pt.z;
+					}
+					if(offsetI!=-1)
+					{
+						viewIntensity(0, oi) = ptr[offsetI];
+					}
+					++oi;
+				}
+				else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
+				{
+					pcl::PointNormal pt;
+					pt.x=ptr[0];
+					pt.y=ptr[1];
+					pt.z=scan.is2d()?0:ptr[2];
+					pt.normal_x=ptr[nx];
+					pt.normal_y=ptr[ny];
+					pt.normal_z=ptr[nz];
+					pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
+					view(0, oi) = pt.x;
+					view(1, oi) = pt.y;
+					if(!scan.is2d())
+					{
+						view(2, oi) = pt.z;
+					}
+					viewNormalX(0, oi) = pt.normal_x;
+					viewNormalY(0, oi) = pt.normal_y;
+					viewNormalZ(0, oi) = pt.normal_z;
+
+					if(offsetI!=-1)
+					{
+						viewIntensity(0, oi) = ptr[offsetI];
+					}
+
+					++oi;
+				}
+				else
+				{
+					UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
+				}
+			}
+			else if(nx==-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
+			{
+				view(0, oi) = ptr[0];
+				view(1, oi) = ptr[1];
+				if(!scan.is2d())
+				{
+					view(2, oi) = ptr[2];
+				}
+				if(nx!=-1)
+				{
+					viewNormalX(0, oi) = ptr[nx];
+					viewNormalY(0, oi) = ptr[ny];
+					viewNormalZ(0, oi) = ptr[nz];
+				}
+				if(offsetI!=-1)
+				{
+					viewIntensity(0, oi) = ptr[offsetI];
+				}
+				++oi;
+			}
+			else
+			{
+				UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
+			}
+		}
+		else
+		{
+			UWARN("Ignoring point %d with invalid data: pos=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3]);
+		}
+
+	}
+	if(oi != scan.size())
+	{
+		cloud.conservativeResize(oi);
+	}
+
+	return cloud;
+}
+
+void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZI> & pclCloud)
+{
+	UDEBUG("");
+	typedef DP::ConstView ConstView;
+
+	if (cloud.features.cols() == 0)
+		return;
+
+	pclCloud.resize(cloud.features.cols());
+	pclCloud.is_dense = true;
+
+	bool hasIntensity = cloud.descriptorExists("intensity");
+
+		// fill cloud
+	ConstView view(cloud.getFeatureViewByName("x"));
+	ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
+	bool is3D = cloud.featureExists("z");
+	for(unsigned int i=0; i<pclCloud.size(); ++i)
+	{
+		pclCloud.at(i).x = view(0, i);
+		pclCloud.at(i).y = view(1, i);
+		pclCloud.at(i).z = is3D?view(2, i):0;
+		if(hasIntensity)
+			pclCloud.at(i).intensity = viewIntensity(0, i);
+	}
+}
+
+void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZINormal> & pclCloud)
+{
+	UDEBUG("");
+	typedef DP::ConstView ConstView;
+
+	if (cloud.features.cols() == 0)
+		return;
+
+	pclCloud.resize(cloud.features.cols());
+	pclCloud.is_dense = true;
+
+	bool hasIntensity = cloud.descriptorExists("intensity");
+
+	// fill cloud
+	ConstView view(cloud.getFeatureViewByName("x"));
+	bool is3D = cloud.featureExists("z");
+	ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
+	ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
+	ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
+	ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
+	for(unsigned int i=0; i<pclCloud.size(); ++i)
+	{
+		pclCloud.at(i).x = view(0, i);
+		pclCloud.at(i).y = view(1, i);
+		pclCloud.at(i).z = is3D?view(2, i):0;
+		pclCloud.at(i).normal_x = viewNormalX(0, i);
+		pclCloud.at(i).normal_y = viewNormalY(0, i);
+		pclCloud.at(i).normal_z = viewNormalZ(0, i);
+		if(hasIntensity)
+			pclCloud.at(i).intensity = viewIntensity(0, i);
+	}
+}
+
+rtabmap::LaserScan laserScanFromDP(const DP & cloud, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity())
+{
+	UDEBUG("");
+	typedef DP::ConstView ConstView;
+
+	rtabmap::LaserScan scan;
+
+	if (cloud.features.cols() == 0)
+		return rtabmap::LaserScan();
+
+	// fill cloud
+	bool transformValid = !localTransform.isNull() && !localTransform.isIdentity();
+	rtabmap::Transform localTransformInv;
+	if(transformValid)
+		localTransformInv = localTransform.inverse();
+	bool is3D = cloud.featureExists("z");
+	bool hasNormals = cloud.descriptorExists("normals");
+	bool hasIntensity = cloud.descriptorExists("intensity");
+	ConstView view(cloud.getFeatureViewByName("x"));
+	ConstView viewNormalX(hasNormals?cloud.getDescriptorRowViewByName("normals",0):view);
+	ConstView viewNormalY(hasNormals?cloud.getDescriptorRowViewByName("normals",1):view);
+	ConstView viewNormalZ(hasNormals?cloud.getDescriptorRowViewByName("normals",2):view);
+	ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
+	int channels = 2+(is3D?1:0) + (hasNormals?3:0) + (hasIntensity?1:0);
+	cv::Mat data(1, cloud.features.cols(), CV_32FC(channels));
+	for(unsigned int i=0; i<cloud.features.cols(); ++i)
+	{
+		pcl::PointXYZINormal pt;
+		pt.x = view(0, i);
+		pt.y = view(1, i);
+		if(is3D)
+			pt.z = view(2, i);
+		if(hasIntensity)
+			pt.intensity = viewIntensity(0, i);
+		if(hasNormals) {
+			pt.normal_x = viewNormalX(0, i);
+			pt.normal_y = viewNormalY(0, i);
+			pt.normal_z = viewNormalZ(0, i);
+		}
+		if(transformValid)
+			pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
+
+		float * value = data.ptr<float>(0, i);
+		int index = 0;
+		value[index++] = pt.x;
+		value[index++] = pt.y;
+		if(is3D)
+			value[index++] = pt.z;
+		if(hasIntensity)
+			value[index++] = pt.intensity;
+		if(hasNormals) {
+			value[index++] = pt.normal_x;
+			value[index++] = pt.normal_y;
+			value[index++] = pt.normal_z;
+		}
+	}
+
+	UASSERT(data.channels() >= 2 && data.channels() <=7);
+	return rtabmap::LaserScan(data, 0, 0,
+			data.channels()==2?rtabmap::LaserScan::kXY:
+			data.channels()==3?(hasIntensity?rtabmap::LaserScan::kXYI:rtabmap::LaserScan::kXYZ):
+			data.channels()==4?rtabmap::LaserScan::kXYZI:
+			data.channels()==5?rtabmap::LaserScan::kXYINormal:
+			data.channels()==6?rtabmap::LaserScan::kXYZNormal:
+			rtabmap::LaserScan::kXYZINormal,
+			localTransform);
+}
+
+template<typename T>
+typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
+{
+	typedef typename PointMatcher<T>::TransformationParameters M;
+	assert(matrix.rows() == matrix.cols());
+	assert((matrix.rows() == 3) || (matrix.rows() == 4));
+	assert((dimp1 == 3) || (dimp1 == 4));
+
+	if (matrix.rows() == dimp1)
+		return matrix;
+
+	M out(M::Identity(dimp1,dimp1));
+	out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2);
+	out.topRightCorner(2,1) = matrix.topRightCorner(2,1);
+	return out;
+}
+
+} // namespace rtabmap
+
+template<typename T>
+struct KDTreeMatcherIntensity : public PointMatcher<T>::Matcher
+{
+	typedef PointMatcherSupport::Parametrizable Parametrizable;
+	typedef PointMatcherSupport::Parametrizable P;
+	typedef Parametrizable::Parameters Parameters;
+	typedef Parametrizable::ParameterDoc ParameterDoc;
+	typedef Parametrizable::ParametersDoc ParametersDoc;
+
+	typedef typename Nabo::NearestNeighbourSearch<T> NNS;
+	typedef typename NNS::SearchType NNSearchType;
+
+	typedef typename PointMatcher<T>::DataPoints DataPoints;
+	typedef typename PointMatcher<T>::Matcher Matcher;
+	typedef typename PointMatcher<T>::Matches Matches;
+	typedef typename PointMatcher<T>::Matrix Matrix;
+
+	inline static const std::string description()
+	{
+		return "This matcher matches a point from the reading to its closest neighbors in the reference.";
+	}
+	inline static const ParametersDoc availableParameters()
+	{
+		return {
+			{"knn", "number of nearest neighbors to consider it the reference", "1", "1", "2147483647", &P::Comp<unsigned>},
+			{"epsilon", "approximation to use for the nearest-neighbor search", "0", "0", "inf", &P::Comp<T>},
+			{"searchType", "Nabo search type. 0: brute force, check distance to every point in the data (very slow), 1: kd-tree with linear heap, good for small knn (~up to 30) and 2: kd-tree with tree heap, good for large knn (~from 30)", "1", "0", "2", &P::Comp<unsigned>},
+			{"maxDist", "maximum distance to consider for neighbors", "inf", "0", "inf", &P::Comp<T>}
+		};
+	}
+
+	const int knn;
+	const T epsilon;
+	const NNSearchType searchType;
+	const T maxDist;
+
+protected:
+	std::shared_ptr<NNS> featureNNS;
+	Matrix filteredReferenceIntensity;
+
+public:
+	KDTreeMatcherIntensity(const Parameters& params = Parameters()) :
+		PointMatcher<T>::Matcher("KDTreeMatcherIntensity", KDTreeMatcherIntensity::availableParameters(), params),
+		knn(Parametrizable::get<int>("knn")),
+		epsilon(Parametrizable::get<T>("epsilon")),
+		searchType(NNSearchType(Parametrizable::get<int>("searchType"))),
+		maxDist(Parametrizable::get<T>("maxDist"))
+	{
+		UINFO("* KDTreeMatcherIntensity: initialized with knn=%d, epsilon=%f, searchType=%d and maxDist=%f", knn, epsilon, searchType, maxDist);
+	}
+	virtual ~KDTreeMatcherIntensity() {}
+	virtual void init(const DataPoints& filteredReference)
+	{
+		// build and populate NNS
+		if(knn>1)
+		{
+			filteredReferenceIntensity = filteredReference.getDescriptorCopyByName("intensity");
+		}
+		else
+		{
+			UWARN("KDTreeMatcherIntensity: knn is not over 1 (%d), intensity re-ordering will be ignored.", knn);
+		}
+		featureNNS.reset( NNS::create(filteredReference.features, filteredReference.features.rows() - 1, searchType, NNS::TOUCH_STATISTICS));
+	}
+	virtual PM::Matches findClosests(const DP& filteredReading)
+	{
+		const int pointsCount(filteredReading.features.cols());
+		Matches matches(
+			typename Matches::Dists(knn, pointsCount),
+			typename Matches::Ids(knn, pointsCount)
+		);
+
+		const BOOST_AUTO(filteredReadingIntensity, filteredReading.getDescriptorViewByName("intensity"));
+
+		static_assert(NNS::InvalidIndex == PM::Matches::InvalidId, "");
+		static_assert(NNS::InvalidValue == PM::Matches::InvalidDist, "");
+		this->visitCounter += featureNNS->knn(filteredReading.features, matches.ids, matches.dists, knn, epsilon, NNS::ALLOW_SELF_MATCH, maxDist);
+
+		if(knn > 1)
+		{
+			Matches matchesOrderedByIntensity(
+				typename Matches::Dists(1, pointsCount),
+				typename Matches::Ids(1, pointsCount)
+			);
+			#pragma omp parallel for
+			for (int i = 0; i < pointsCount; ++i)
+			{
+				float minDistance = std::numeric_limits<float>::max();
+				for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
+				{
+					float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
+					if(distIntensity < minDistance)
+					{
+						matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
+						matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
+						minDistance = distIntensity;
+					}
+				}
+			}
+			matches = matchesOrderedByIntensity;
+		}
+		return matches;
+	}
+};
+
+
+#endif /* CORELIB_SRC_ICP_LIBPOINTMATCHER_H_ */

corelib/src/imufilter/MadgwickFilter.cpp

@@ -217,14 +217,14 @@ static inline bool computeOrientation(
   // magnetic Field E must not be parallel to A,
   // choose an arbitrary orthogonal vector
   Eigen::Vector3f E;
-  if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
+  if (fabs(A[2]) > 0.1) {
+  	  E[0] = 0.0;
+  	  E[1] = A[2];
+  	  E[2] = -A[1];
+  } else if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
 	  E[0] = A[1];
 	  E[1] = A[0];
 	  E[2] = 0.0;
-  } else if (fabs(A[2]) > 0.1) {
-	  E[0] = 0.0;
-	  E[1] = A[2];
-	  E[2] = A[1];
   } else {
 	  // free fall
 	  return false;