commonFiltering(): warn when normals could not be computed (returned all NaNs)

* Fixed build with VTK9

* Fixed VTK9 build with OctoMap dependency (vtkRenderingVolumeOpenGL2 missing)

.appveyor.yml

@@ -33,13 +33,16 @@ install:
   - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
   - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
   # OpenCV
-  #- ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
-  #- cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
-  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe' -outfile opencv-2.4.13.6-vc14.exe
-  - cmd: opencv-2.4.13.6-vc14.exe -o"C:\Program Files" -y
+  #- appveyor-retry appveyor DownloadFile http://downloads.sourceforge.net/project/opencvlibrary/4.5.2/opencv-4.5.2-vc14_vc15.exe
+  #- cmd: opencv-4.5.2-vc14_vc15.exe -o"C:\Program Files" -y
+  #- ECHO "Installed OpenCV:"
+  #- ps: "ls \"C:/Program Files/opencv/build\""
+  #- set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
+  - ps: wget 'https://dl.dropboxusercontent.com/s/o6ofn491bc0jso1/opencv450_vc14.exe?dl=0' -outfile opencv.exe
+  - cmd: opencv.exe -o"C:\Program Files" -y
   - ECHO "Installed OpenCV:"
-  - ps: "ls \"C:/Program Files/opencv/build\""
-  - set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
+  - ps: "ls \"C:/Program Files/opencv\""
+  - set PATH=%PATH%;C:\Program Files\opencv\x64\vc14\bin
   # VTK (including QVTK)
   - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
   - cmd: VTK-6_3.exe -o"C:\Program Files" -y
@@ -105,22 +108,31 @@ install:
   - ECHO "Installed Open Chisel:"
   - ps: "ls \"C:/Program Files/open_chisel\""
   - set PATH=%PATH%;C:\Program Files\open_chisel\bin
-  # cvsba
-  - ps: wget 'https://dl.dropboxusercontent.com/s/4ey8ergerx46zvj/cvsba_x64_vc14.exe?dl=0' -outfile cvsba.exe
-  - cmd: cvsba.exe -o"C:\Program Files" -y
-  - ECHO "Installed cvsba:"
-  - ps: "ls \"C:/Program Files/cvsba\""
-  - set PATH=%PATH%;C:\Program Files\cvsba\bin
+  # yaml-cpp
   - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe
   - cmd: yaml-cpp.exe -o"C:\Program Files" -y
   - ECHO "Installed yaml-cpp:"
   - ps: "ls \"C:/Program Files/yaml-cpp\""
+  # RealSense2
+  - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe
+  - cmd: realsense2.exe /VERYSILENT
+  - ECHO "Installed RealSense2:"
+  - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\""
+  - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64
+  - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0
+  # Kinect 4 Azure
+  - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe
+  - cmd: azure.exe /quiet
+  - ECHO "Installed Kinect For Azure:"
+  - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\""
+  - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools
+  - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1
   
 before_build:
   - cd c:\projects\rtabmap\build
   - ECHO %PROGRAMFILES%
   - ECHO %PATH%
-  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dcvsba_DIR="C:\Program Files\cvsba\lib\cmake" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON ..
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON ..
   
 after_build :
   - cmake --build . --config Release --target package

.gitignore

@@ -8,3 +8,5 @@ app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml
 app/android/res/raw/
+compile_flags.txt
+tags

.travis.yml

@@ -1,29 +1,34 @@
-sudo: true
 language: cpp
-group: deprecated-2017Q3
-
-compiler:
-  - gcc
-
-matrix:
+   
+jobs:
   include:
+#    - name: osx
+#      compiler: clang
+#      os: osx
+#      install:
+#        - brew install sqlite
+#        - brew install pcl
+#        - brew install opencv@3
+    
+#    - name: linux-trusty
+#      compiler: gcc
+#      os: linux
+#      dist: trusty
+#      install:
+#        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+#        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+#        - sudo apt-get update
+#        - sudo apt-get update && sudo apt-get install dpkg
+#        - sudo apt-get -y install ros-indigo-rtabmap-ros
+#        - sudo apt-get -y remove ros-indigo-rtabmap
+#
+#      before_script: 
+#        - source /opt/ros/indigo/setup.bash
 
-    - dist: trusty
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get update && sudo apt-get install dpkg
-        - sudo apt-get -y install ros-indigo-rtabmap-ros
-        - sudo apt-get -y remove ros-indigo-rtabmap
-
-      script: 
-        - source /opt/ros/indigo/setup.bash
-        - mkdir -p build && cd build
-        - cmake .. 
-        - make
-
-    - dist: xenial
+    - name: linux-xenial
+      compiler: gcc
+      os: linux
+      dist: xenial
       install:
         - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
         - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
@@ -32,13 +37,13 @@ matrix:
         - sudo apt-get -y install ros-kinetic-rtabmap-ros
         - sudo apt-get -y remove ros-kinetic-rtabmap
 
-      script: 
+      before_script: 
         - source /opt/ros/kinetic/setup.bash
-        - mkdir -p build && cd build
-        - cmake .. 
-        - make
 
-    - dist: bionic
+    - name: linux-bionic
+      compiler: gcc
+      os: linux
+      dist: bionic
       install:
         - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
         - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
@@ -47,11 +52,28 @@ matrix:
         - sudo apt-get -y install ros-melodic-rtabmap-ros
         - sudo apt-get -y remove ros-melodic-rtabmap
 
-      script: 
+      before_script: 
         - source /opt/ros/melodic/setup.bash
-        - mkdir -p build && cd build
-        - cmake .. 
-        - make
+
+    - name: linux-focal
+      compiler: gcc
+      os: linux
+      dist: focal
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-noetic-rtabmap-ros
+        - sudo apt-get -y remove ros-noetic-rtabmap
+
+      before_script: 
+        - source /opt/ros/noetic/setup.bash
+
+script:
+  - mkdir -p build && cd build
+  - cmake .. 
+  - make
 
 notifications:
   email:

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 0)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -61,7 +61,7 @@ ELSE ()
 ENDIF()
 
 if(POLICY CMP0020)
-    cmake_policy(SET CMP0020 OLD)
+    cmake_policy(SET CMP0020 NEW)
 endif()
 if(POLICY CMP0043)
     cmake_policy(SET CMP0043 OLD)
@@ -163,8 +163,10 @@ ELSE()
 option(WITH_QT            "Include Qt support"                   ON)
 ENDIF()
 option(WITH_ORB_OCTREE    "Include ORB Octree feature support"   ON)
-option(WITH_SUPERPOINT_TORCH  "Include SuperPoint Torch feature support"   ON)
-option(WITH_PYMATCHER     "Include Python3 matchers support"     OFF)
+option(WITH_TORCH         "Include Torch support (SuperPoint)"   OFF)
+option(WITH_PYTHON        "Include Python3 support (PyMatcher, PyDetector)"  OFF)
+option(WITH_PYTHON_THREADING  "Use more than one Python interpreter."  OFF)
+option(WITH_PDAL          "Include PDAL support"                 ON)
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
@@ -178,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)
@@ -192,10 +197,11 @@ 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)
+option(WITH_OPENVINS      "Include OpenVINS support"             ON)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 IF(ANDROID)
@@ -295,11 +301,15 @@ IF(WITH_QT)
         IF("${VTK_MAJOR_VERSION}" EQUAL 5)
             FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
         ELSE()
-            list(FIND PCL_LIBRARIES vtkGUISupportQt value)
-            IF(value EQUAL -1)
-                SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
-                SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
-            ENDIF(value EQUAL -1)
+		    list(FIND PCL_LIBRARIES VTK::GUISupportQt value)
+			IF(value EQUAL -1)
+                list(FIND PCL_LIBRARIES vtkGUISupportQt value)
+                IF(value EQUAL -1)
+                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
+                ENDIF(value EQUAL -1)
+		    ENDIF(value EQUAL -1)
+		    
             MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}")
 
             IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
@@ -315,6 +325,14 @@ IF(WITH_QT)
                 IF(value EQUAL -1)
                     SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkRenderingVolumeOpenGL")
                 ENDIF(value EQUAL -1)
+            ELSEIF("${VTK_MAJOR_VERSION}" EQUAL 9)
+                list(FIND PCL_LIBRARIES VTK::RenderingOpenGL2 value)
+			    IF(NOT value EQUAL -1)
+			        list(FIND PCL_LIBRARIES VTK::RenderingVolumeOpenGL2 value)
+                    IF(value EQUAL -1)
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::RenderingVolumeOpenGL2")
+                    ENDIF(value EQUAL -1)
+			    ENDIF(NOT value EQUAL -1)
             ENDIF()
 
         ENDIF()
@@ -322,19 +340,26 @@ IF(WITH_QT)
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
-IF(WITH_SUPERPOINT_TORCH)
+IF(WITH_TORCH)
     FIND_PACKAGE(Torch QUIET)
     IF(TORCH_FOUND)
         MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
     ENDIF(TORCH_FOUND)
-ENDIF(WITH_SUPERPOINT_TORCH)
+ENDIF(WITH_TORCH)
 
-IF(WITH_PYMATCHER)
+IF(WITH_PYTHON)
     FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
     IF(Python3_FOUND)
         MESSAGE(STATUS "Found Python3")
     ENDIF(Python3_FOUND)
-ENDIF(WITH_PYMATCHER)
+ENDIF(WITH_PYTHON)
+
+IF(WITH_PDAL)
+    FIND_PACKAGE(PDAL QUIET)
+    IF(PDAL_FOUND)
+       MESSAGE(STATUS "Found PDAL ${PDAL_VERSION}: ${PDAL_INCLUDE_DIRS}")
+    ENDIF(PDAL_FOUND)
+ENDIF(WITH_PDAL)
 
 IF(WITH_FREENECT)
     FIND_PACKAGE(Freenect QUIET)
@@ -399,10 +424,17 @@ IF(WITH_DC1394)
 ENDIF(WITH_DC1394)
 
 IF(WITH_G2O)
-    FIND_PACKAGE(G2O QUIET)
-    IF(G2O_FOUND)
-       MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
-    ENDIF(G2O_FOUND)
+	FIND_PACKAGE(g2o QUIET NO_MODULE)
+    IF(g2o_FOUND)
+       MESSAGE(STATUS "Found g2o (targets)")
+	   SET(G2O_FOUND ${g2o_FOUND})
+	   SET(G2O_CPP11 1)
+    ELSE()
+       FIND_PACKAGE(G2O QUIET)
+       IF(G2O_FOUND)
+		 MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+       ENDIF(G2O_FOUND)
+    ENDIF()
 ENDIF(WITH_G2O)
 
 IF(WITH_GTSAM)
@@ -431,6 +463,24 @@ IF(WITH_POINTMATCHER)
     ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)
 
+IF(libpointmatcher_FOUND OR GTSAM_FOUND)
+    find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
+    IF(Boost_MINOR_VERSION GREATER 47)
+        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer REQUIRED)
+    ENDIF(Boost_MINOR_VERSION GREATER 47)
+    IF(WIN32)
+	    MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
+	    link_directories(${Boost_LIBRARY_DIRS})
+    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)
@@ -454,6 +504,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)
@@ -471,7 +535,7 @@ ENDIF(WITH_REALSENSE)
 IF(WITH_REALSENSE2)
     IF(WIN32)
         FIND_PACKAGE(RealSense2 QUIET)
-	ELSE()
+    ELSE()
         FIND_PACKAGE(realsense2 QUIET)
     ENDIF()
     IF(realsense2_FOUND)
@@ -486,6 +550,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)
@@ -513,11 +584,14 @@ ENDIF(WITH_OPENCHISEL)
 IF(WITH_ALICE_VISION)
     find_package(AliceVision CONFIG QUIET)
     IF(AliceVision_FOUND)
+        IF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
+           find_package(Boost COMPONENTS log log_setup container REQUIRED)
+        ENDIF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
         SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
         find_package(Geogram REQUIRED QUIET)
-        # Make sure the two following lines are also commented in AliceVision to avoid Eigen memory alignment error
-        #add_definitions("-DEIGEN_DONT_ALIGN_STATICALLY=1")
-        #add_definitions("-DEIGEN_DONT_VECTORIZE=1")
+        add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
+        add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
+        add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
     ENDIF(AliceVision_FOUND)
 ENDIF(WITH_ALICE_VISION)
 
@@ -577,6 +651,13 @@ IF(WITH_VINS)
     ENDIF(vins_FOUND)
 ENDIF(WITH_VINS)
 
+IF(WITH_OPENVINS)
+    FIND_PACKAGE(ov_msckf QUIET)
+    IF(ov_msckf_FOUND)
+       MESSAGE(STATUS "Found ov_msckf: ${ov_msckf_INCLUDE_DIRS}")
+    ENDIF(ov_msckf_FOUND)
+ENDIF(WITH_OPENVINS)
+
 IF(WITH_FASTCV)
     FIND_PACKAGE(FastCV QUIET)
     IF(FastCV_FOUND)
@@ -584,58 +665,54 @@ IF(WITH_FASTCV)
     ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)
 
-IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
-    FIND_PACKAGE(ORB_SLAM2 QUIET)
-    IF(ORB_SLAM2_FOUND)
-       MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
-       FIND_PACKAGE(Pangolin QUIET)
-       IF(NOT Pangolin_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(WITH_ORB_SLAM AND NOT G2O_FOUND)
+    FIND_PACKAGE(ORB_SLAM QUIET)
+    IF(ORB_SLAM_FOUND)
+       MESSAGE(STATUS "Found ORB_SLAM${ORB_SLAM_VERSION}: ${ORB_SLAM_INCLUDE_DIRS}")
+    ENDIF(ORB_SLAM_FOUND)
+ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
-IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND)
-   #LOAM and PCL>=1.10 require c++14
-   IF(NOT MSVC)
-     include(CheckCXXCompilerFlag)
-      CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-      IF(COMPILER_SUPPORTS_CXX14)
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-      ELSE()
-        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM (set \"-DWITH_LOAM=OFF\" to build without LOAM).")
-      ENDIF()
-   ENDIF()
-ELSEIF(G2O_FOUND OR 
-       GTSAM_FOUND OR 
-       CERES_FOUND OR
-       ZED_FOUND OR 
-       ANDROID OR 
-       RealSense_FOUND OR 
-       realsense2_FOUND OR 
-       ORB_SLAM2_FOUND OR 
-       okvis_FOUND OR 
-       open_chisel_FOUND OR 
-       msckf_vio_FOUND OR 
-       vins_FOUND)
-   #Newest versions require std11
-   IF(NOT MSVC)
-     include(CheckCXXCompilerFlag)
-      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-      IF(COMPILER_SUPPORTS_CXX11)
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-      ELSEIF(COMPILER_SUPPORTS_CXX0X)
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-      ELSE()
-        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-      ENDIF()
-   ENDIF()
+IF(NOT MSVC)
+	IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
+	 include(CheckCXXCompilerFlag)
+	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+	  IF(COMPILER_SUPPORTS_CXX14)
+	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+	    set(CMAKE_CXX_STANDARD 14)
+	  ELSE()
+	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
+	  ENDIF()
+	ENDIF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+
+	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
+	       G2O_FOUND OR
+	       GTSAM_FOUND OR
+	       CERES_FOUND OR
+	       ZED_FOUND OR
+	       ZEDOC_FOUND OR
+	       ANDROID OR
+	       RealSense_FOUND OR
+	       realsense2_FOUND OR
+	       ORB_SLAM_FOUND OR
+	       okvis_FOUND OR
+	       open_chisel_FOUND OR
+	       msckf_vio_FOUND OR
+	       vins_FOUND OR
+	       ov_msckf_FOUND OR
+	       libpointmatcher_FOUND))
+	 #Newest versions require std11
+	 include(CheckCXXCompilerFlag)
+	  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+	  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+	  IF(COMPILER_SUPPORTS_CXX11)
+	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	  ELSEIF(COMPILER_SUPPORTS_CXX0X)
+	    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+	  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 #######
@@ -689,16 +766,6 @@ ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2")
   ENDIF(${matchres} EQUAL -1)
 ENDIF()
 
-# check if version status is "-dev" (SIFT compatibility issue between 4.3.0 vs 4.3.0-dev)
-FIND_FILE(OpenCV_VERSION_HPP opencv2/core/version.hpp
-  PATHS ${OpenCV_INCLUDE_DIRS}
-  NO_DEFAULT_PATH)
-FILE(READ ${OpenCV_VERSION_HPP} TMPTXT)
-STRING(FIND "${TMPTXT}" "-dev" matchres)
-IF(${matchres} EQUAL -1)
-  SET(OPENCV_DEV "//")
-ENDIF(${matchres} EQUAL -1)
-
 IF(NOT G2O_FOUND)
    SET(G2O "//")
 ELSE()
@@ -729,9 +796,15 @@ 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)
+IF(NOT PDAL_FOUND)
+   SET(PDAL "//")
+ENDIF(NOT PDAL_FOUND)
 IF(NOT loam_velodyne_FOUND)
    SET(LOAM "//")
 ENDIF(NOT loam_velodyne_FOUND)
@@ -775,6 +848,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()
@@ -791,6 +869,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()
@@ -839,19 +920,24 @@ IF(NOT vins_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
 ENDIF()
-IF(NOT ORB_SLAM2_FOUND)
-   SET(ORB_SLAM2 "//")
+IF(NOT ov_msckf_FOUND)
+   SET(OPENVINS "//")
 ELSE()
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) 
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ov_msckf_LIBRARIES}) 
+ENDIF()
+IF(NOT ORB_SLAM_FOUND)
+   SET(ORB_SLAM "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM_LIBRARIES})
 ENDIF()
 IF(NOT WITH_ORB_OCTREE)
    SET(ORB_OCTREE "//")
 ENDIF()
 IF(NOT TORCH_FOUND)
-   SET(SUPERPOINT_TORCH "//")
+   SET(TORCH "//")
 ENDIF()
 IF(NOT Python3_FOUND)
-   SET(PYMATCHER "//")
+   SET(PYTHON "//")
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
@@ -1035,7 +1121,8 @@ INCLUDE(CPack)
 
 MESSAGE(STATUS "--------------------------------------------")
 MESSAGE(STATUS "Info :")
-MESSAGE(STATUS "  Version : ${RTABMAP_VERSION}")
+MESSAGE(STATUS "  RTAB-Map Version =     ${RTABMAP_VERSION}")
+MESSAGE(STATUS "  CMAKE_VERSION =        ${CMAKE_VERSION}")
 MESSAGE(STATUS "  CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}")
 MESSAGE(STATUS "  CMAKE_BUILD_TYPE =     ${CMAKE_BUILD_TYPE}")
 MESSAGE(STATUS "  CMAKE_INSTALL_LIBDIR = ${CMAKE_INSTALL_LIBDIR}")
@@ -1069,12 +1156,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)
@@ -1106,18 +1201,18 @@ ENDIF()
 
 IF(TORCH_FOUND)
 MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
-ELSEIF(NOT WITH_SUPERPOINT_TORCH)
-MESSAGE(STATUS "  With SupertPoint          = NO (WITH_SUPERPOINT_TORCH=OFF)")
+ELSEIF(NOT WITH_TORCH)
+MESSAGE(STATUS "  With SupertPoint          = NO (WITH_TORCH=OFF)")
 ELSE()
 MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
 ENDIF()
 
 IF(Python3_FOUND)
-MESSAGE(STATUS "  With Python3              = YES (License: PSF)")
-ELSEIF(NOT WITH_PYMATCHER)
-MESSAGE(STATUS "  With Python3              = NO (WITH_PYMATCHER=OFF)")
+MESSAGE(STATUS "  With Python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}            = YES (License: PSF)")
+ELSEIF(NOT WITH_PYTHON)
+MESSAGE(STATUS "  With Python3              = NO (WITH_PYTHON=OFF)")
 ELSE()
-MESSAGE(STATUS "  With Python3              = NO (python3 not found)")
+MESSAGE(STATUS "  With Python3              = NO (python not found)")
 ENDIF()
 
 IF(WITH_MADGWICK)
@@ -1134,6 +1229,14 @@ ELSE()
 MESSAGE(STATUS "  With FastCV               = NO (FastCV not found)")
 ENDIF()
 
+IF(PDAL_FOUND)
+MESSAGE(STATUS "  With PDAL                 = YES (License: BSD)")
+ELSEIF(NOT WITH_PDAL)
+MESSAGE(STATUS "  With PDAL                 = NO (WITH_PDAL=OFF)")
+ELSE()
+MESSAGE(STATUS "  With PDAL                 = NO (PDAL not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Solvers:")
 IF(WITH_TORO)
@@ -1192,6 +1295,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)
@@ -1219,7 +1330,7 @@ MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
 ENDIF()
 
 IF(AliceVision_FOUND)
-MESSAGE(STATUS "  With AliceVision          = YES (License: MPLv2)")
+MESSAGE(STATUS "  With AliceVision ${AliceVision_VERSION}    = YES (License: MPLv2)")
 ELSEIF(NOT WITH_ALICE_VISION)
 MESSAGE(STATUS "  With AliceVision          = NO (WITH_ALICE_VISION=OFF)")
 ELSE()
@@ -1292,6 +1403,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)
@@ -1323,6 +1442,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)
@@ -1381,14 +1508,22 @@ ELSE()
 MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
 ENDIF()
 
-IF(ORB_SLAM2_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
-ELSEIF(NOT WITH_ORB_SLAM2)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_ORB_SLAM2=OFF)")
-ELSEIF(G2O_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
+IF(ov_msckf_FOUND)
+MESSAGE(STATUS "  With OpenVINS             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_OPENVINS)
+MESSAGE(STATUS "  With OpenVINS             = NO (WITH_OPENVINS=OFF)")
 ELSE()
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
+MESSAGE(STATUS "  With OpenVINS             = NO (ov_msckf not found)")
+ENDIF()
+
+IF(ORB_SLAM_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM${ORB_SLAM_VERSION}            = YES (License: GPLv3)")
+ELSEIF(NOT WITH_ORB_SLAM)
+MESSAGE(STATUS "  With ORB_SLAM             = NO (WITH_ORB_SLAM=OFF)")
+ELSEIF(G2O_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM             = NO (WITH_G2O should be OFF as ORB_SLAM uses its own g2o version)")
+ELSE()
+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_")

LICENSE

@@ -1,5 +1,6 @@
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
+RTAB-Map - https://github.com/introlab/rtabmap
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) XXX, contributors, all rights reserved.
 
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
@@ -11,7 +12,7 @@ modification, are permitted provided that the following conditions are met:
   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
+* Neither the name of the copyright holders nor the names of the 
   contributors may be used to endorse or promote products derived from
   this software without specific prior written permission.
 
@@ -24,4 +25,4 @@ 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.
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -1,4 +1,4 @@
-rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
 =======
 
 [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
@@ -7,7 +7,7 @@ rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pix
 [![License][license-image]][license]
 Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
 
-[release-image]: https://img.shields.io/badge/release-0.18.0-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.20.7-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat

Version.h.in

@@ -38,7 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
 
 @NONFREE@#define RTABMAP_NONFREE
-@OPENCV_DEV@#define RTABMAP_OPENCV_DEV
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
 @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
@@ -52,15 +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,12 +74,24 @@ 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
+@OPENVINS@#define RTABMAP_OPENVINS
+@ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE
-@SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH
-@PYMATCHER@#define RTABMAP_PYMATCHER
+@TORCH@#define RTABMAP_TORCH
+@PYTHON@#define RTABMAP_PYTHON
 @MADGWICK@#define RTABMAP_MADGWICK
 
+#include <pcl/pcl_config.h>
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+#include <pcl/types.h>
+#define RTABMAP_PCL_INDEX pcl::index_t
+#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
+#define RTABMAP_PCL_INDEX std::uint32_t
+#else
+#include <pcl/pcl_macros.h>
+#define RTABMAP_PCL_INDEX pcl::uint32_t
+#endif
 
 #endif /* VERSION_H_ */
 

app/android/jni/CMakeLists.txt

@@ -22,6 +22,7 @@ set(sources
   scene.cpp
   point_cloud_drawable.cpp
   graph_drawable.cpp
+  background_renderer.cc
   tango-gl/axis.cpp 
   tango-gl/camera.cpp 
   tango-gl/conversions.cpp 

app/android/jni/CameraARCore.cpp

@@ -34,47 +34,31 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-#ifdef DEPTH_TEST
-// Camera Callbacks
-static void CameraDeviceOnDisconnected(void* context, ACameraDevice* device) {
-  LOGE("Camera(id: %s) is disconnected.\n", ACameraDevice_getId(device));
-}
-static void CameraDeviceOnError(void* context, ACameraDevice* device,
-                                int error) {
-  LOGE("Error(code: %d) on Camera(id: %s).\n", error,
-       ACameraDevice_getId(device));
-}
-// Capture Callbacks
-bool g_captureSessionReady = false;
-static void CaptureSessionOnReady(void* context,
-                                  ACameraCaptureSession* session) {
-  LOGI("Session is ready.\n");
-  g_captureSessionReady = true;
-}
-static void CaptureSessionOnActive(void* context,
-                                   ACameraCaptureSession* session) {
-  LOGI("Session is activated.\n");
-}
-#endif // DEPTH_TEST
-
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smoothing):
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing):
 	CameraMobile(smoothing),
 	env_(env),
 	context_(context),
 	activity_(activity),
-	arInstallRequested_(false)
+	arInstallRequested_(false),
+	textureId_(9999),
+	uvs_initialized_(false),
+	updateOcclusionImage_(false),
+	depthFromMotion_(depthFromMotion)
 {
-	glGenTextures(1, &textureId_);
 }
 
 CameraARCore::~CameraARCore() {
 	// Disconnect ARCore service
 	close();
 
-	glDeleteTextures(1, &textureId_);
+	if(textureId_ != 9999)
+	{
+		glDeleteTextures(1, &textureId_);
+		textureId_ = 9999;
+	}
 }
 
 
@@ -146,132 +130,10 @@ std::string CameraARCore::getSerial() const
 	return "ARCore";
 }
 
-#ifdef DEPTH_TEST
-void OnImageCallback(void *ctx, AImageReader *reader) {
-  reinterpret_cast<CameraARCore *>(ctx)->imageCallback(reader);
-}
-void CameraARCore::imageCallback(AImageReader *reader) {
-	int32_t format;
-	media_status_t status = AImageReader_getFormat(reader, &format);
-	UWARN("format=%d", format);
-	UASSERT_MSG(status == AMEDIA_OK, "Failed to get the media format");
-
-	if (format == AIMAGE_FORMAT_DEPTH16) {
-		// Create a thread and write out the jpeg files
-		AImage *image = nullptr;
-		media_status_t status = AImageReader_acquireNextImage(reader, &image);
-		UASSERT_MSG(status == AMEDIA_OK && image, "Image is not available");
-
-		int planeCount;
-		status = AImage_getNumberOfPlanes(image, &planeCount);
-		UASSERT_MSG(status == AMEDIA_OK && planeCount == 1,
-				uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
-		uint8_t *data = nullptr;
-		int len = 0;
-		int stride;
-		int width;
-		int height;
-		AImage_getWidth(image, &width);
-		AImage_getHeight(image, &height);
-		AImage_getPlaneRowStride(image, 0, &stride);
-		AImage_getPlaneData(image, 0, &data, &len);
-
-		cv::Mat output(height, width, CV_16UC1);
-		uint16_t *dataShort = (uint16_t *)data;
-		uint16_t max=0x0;
-		for (int y = 0; y < output.rows; ++y)
-		{
-			for (int x = 0; x < output.cols; ++x)
-			{
-				uint16_t depthSample = dataShort[y*output.cols + x];
-				uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
-				output.at<uint16_t>(y,x) = depthRange;
-				if(depthRange > max)
-				{
-					max = depthRange;
-				}
-			}
-		}
-		UWARN("width=%d, height=%d, bytes=%d stride=%d max=%dmm",
-				width, height, len, stride, (int)max);
-
-		std::string path = "/storage/emulated/0/RTAB-Map/depth.png";
-		cv::imwrite(path, output);
-		UWARN("depth image saved to %s", path.c_str());
-
-		AImage_delete(image);
-	}
-}
-#endif // DEPTH_TEST
-
 bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	close();
 
-#ifdef DEPTH_TEST
-	///////////////////////////
-	// Depth image using camera2 API
-	/////////////////////////////
-	camera_status_t cameraStatus = ACAMERA_OK;
-
-	cameraManager_ = ACameraManager_create();
-
-	deviceStateCallbacks_.onDisconnected = CameraDeviceOnDisconnected;
-	deviceStateCallbacks_.onError = CameraDeviceOnError;
-
-	const char * cameraId = "0";
-	cameraStatus = ACameraManager_openCamera(cameraManager_, cameraId, &deviceStateCallbacks_, &cameraDevice_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK, uFormat("Failed to open camera device (id: %s)",
-			cameraId).c_str());
-
-	// Currently only working resolution on Huawei P30 Pro
-	cv::Size size(240, 180);
-	int format = AIMAGE_FORMAT_DEPTH16;
-
-	media_status_t mediaStatus = AImageReader_new(size.width, size.height, format, 2, &imageReader_);
-	UASSERT_MSG(imageReader_ && mediaStatus == AMEDIA_OK, uFormat("Failed to create AImageReader %dx%d format=%d",
-			size.width, size.height, format).c_str());
-
-	AImageReader_ImageListener listener{
-	  .context = this,
-	  .onImageAvailable = OnImageCallback,
-	};
-	AImageReader_setImageListener(imageReader_, &listener);
-
-	//
-	ANativeWindow *nativeWindow;
-	mediaStatus = AImageReader_getWindow(imageReader_, &nativeWindow);
-	UASSERT_MSG(mediaStatus == AMEDIA_OK, "Could not get ANativeWindow");
-
-	outputNativeWindow_ = nativeWindow;
-	ACaptureSessionOutputContainer_create(&captureSessionOutputContainer_);
-	ANativeWindow_acquire(outputNativeWindow_);
-	ACaptureSessionOutput_create(outputNativeWindow_, &sessionOutput_);
-	ACaptureSessionOutputContainer_add(captureSessionOutputContainer_, sessionOutput_);
-	ACameraOutputTarget_create(outputNativeWindow_, &cameraOutputTarget_);
-
-	cameraStatus = ACameraDevice_createCaptureRequest(cameraDevice_, TEMPLATE_RECORD, &captureRequest_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK,
-			uFormat("Failed to create preview capture request (id: %s, status=%d)",
-					cameraId, cameraStatus).c_str());
-
-	ACaptureRequest_addTarget(captureRequest_, cameraOutputTarget_);
-
-	captureSessionStateCallbacks_.onReady = CaptureSessionOnReady;
-	captureSessionStateCallbacks_.onActive = CaptureSessionOnActive;
-	ACameraDevice_createCaptureSession(
-			cameraDevice_,
-			captureSessionOutputContainer_, // outputs
-			&captureSessionStateCallbacks_, // callbacks
-			&captureSession_);
-
-	ACameraCaptureSession_setRepeatingRequest(captureSession_, nullptr, 1,
-			&captureRequest_, nullptr);
-
-	// Don't start ARCore as we cannot use both at the same time
-	return true;
-#endif // DEPTH_TEST
-
 	UScopeMutex lock(arSessionMutex_);
 
 	ArInstallStatus install_status;
@@ -302,10 +164,19 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
 	UASSERT(arSession_);
 
+	int32_t is_depth_supported = 0;
+	ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
 	ArConfig_create(arSession_, &arConfig_);
 	UASSERT(arConfig_);
 
-	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
+	if (is_depth_supported!=0) {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
+	} else {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
+	}
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
 	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
 
 	ArFrame_create(arSession_, &arFrame_);
@@ -361,9 +232,6 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 
 	deviceTColorCamera_ = opticalRotation;
 
-	// Required as ArSession_update does some off-screen OpenGL stuff...
-	ArSession_setCameraTextureName(arSession_, textureId_);
-
 	if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
 	{
 		UERROR("Cannot resume camera!");
@@ -410,45 +278,8 @@ void CameraARCore::close()
 	}
 	arPose_ = nullptr;
 
-#ifdef DEPTH_TEST
-
-	if(captureSession_!=nullptr)
-	{
-		g_captureSessionReady = false;
-		ACameraCaptureSession_stopRepeating(captureSession_);
-		double start = UTimer::now();
-		while(g_captureSessionReady != true && UTimer::now()-start < 2.0){
-			uSleep(100);
-			UWARN("Waiting session to close.... max 2 seconds");
-		}
-		//ACameraCaptureSession_close(captureSession_); // FIXME: this crashes?!
-		captureSession_ = nullptr;
-
-		ACaptureRequest_removeTarget(captureRequest_, cameraOutputTarget_);
-		ACaptureRequest_free(captureRequest_);
-		ACameraOutputTarget_free(cameraOutputTarget_);
-		captureRequest_ = nullptr;
-		cameraOutputTarget_ = nullptr;
-
-		ACaptureSessionOutputContainer_remove(captureSessionOutputContainer_, sessionOutput_);
-		ANativeWindow_release(outputNativeWindow_);
-		ACaptureSessionOutputContainer_free(captureSessionOutputContainer_);
-		ACaptureSessionOutput_free(sessionOutput_);
-		captureSessionOutputContainer_ = nullptr;
-		sessionOutput_ = nullptr;
-
-		ACameraDevice_close(cameraDevice_);
-		cameraDevice_ = nullptr;
-
-		ACameraManager_delete(cameraManager_);
-		cameraManager_ = nullptr;
-
-		AImageReader_delete(imageReader_);
-		imageReader_ = nullptr;
-	}
-#endif
-
 	CameraMobile::close();
+	occlusionImage_ = cv::Mat();
 }
 
 LaserScan CameraARCore::scanFromPointCloudData(
@@ -499,6 +330,20 @@ LaserScan CameraARCore::scanFromPointCloudData(
 	return LaserScan();
 }
 
+void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		ArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
 SensorData CameraARCore::captureImage(CameraInfo * info)
 {
 	UScopeMutex lock(arSessionMutex_);
@@ -510,15 +355,44 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		return data;
 	}
 
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
 		LOGE("CameraARCore::captureImage() ArSession_update error");
 		return data;
 	}
 
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
 
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -551,17 +425,55 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 			ArPointCloud * pointCloud = nullptr;
 			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
 
+			int32_t is_depth_supported = 0;
+			ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
 			ArImage * image = nullptr;
 			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
 			if(status == AR_SUCCESS)
 			{
+				if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
+				{
+					LOGD("Acquire depth image!");
+					ArImage * depthImage = nullptr;
+					ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+					ArImageFormat format;
+					ArImage_getFormat(arSession_, depthImage, &format);
+					if(format == AR_IMAGE_FORMAT_DEPTH16)
+					{
+						LOGD("Depth format detected!");
+						int planeCount;
+						ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+						LOGD("planeCount=%d", planeCount);
+						UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+						const uint8_t *data = nullptr;
+						int len = 0;
+						int stride;
+						int depth_width;
+						int depth_height;
+						ArImage_getWidth(arSession_, depthImage, &depth_width);
+						ArImage_getHeight(arSession_, depthImage, &depth_height);
+						ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+						ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
+
+						occlusionImage_ = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+
+						float scaleX = (float)depth_width / (float)width;
+						float scaleY = (float)depth_height / (float)height;
+						occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+					}
+					ArImage_release(depthImage);
+				}
+
 				int64_t timestamp_ns;
 				ArImageFormat format;
 				ArImage_getTimestamp(arSession_, image, &timestamp_ns);
 				ArImage_getFormat(arSession_, image, &format);
 				if(format == AR_IMAGE_FORMAT_YUV_420_888)
 				{
-
 #ifndef DISABLE_LOG
 					int32_t num_planes;
 					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
@@ -623,7 +535,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}
 
-						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -662,21 +574,49 @@ void CameraARCore::capturePoseOnly()
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");
 
-	SensorData data;
 	if(!arSession_)
 	{
 		return;
 	}
 
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
-		LOGE("CameraARCore::captureImage() ArSession_update error");
+		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
 		return;
 	}
 
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
 
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -694,6 +634,52 @@ void CameraARCore::capturePoseOnly()
 			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 			this->poseReceived(pose);
 		}
+
+		int32_t is_depth_supported = 0;
+		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+		if(is_depth_supported && updateOcclusionImage_)
+		{
+			LOGD("Acquire depth image!");
+			ArImage * depthImage = nullptr;
+			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+			ArImageFormat format;
+			ArImage_getFormat(arSession_, depthImage, &format);
+			if(format == AR_IMAGE_FORMAT_DEPTH16)
+			{
+				LOGD("Depth format detected!");
+				int planeCount;
+				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+				LOGD("planeCount=%d", planeCount);
+				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+				const uint8_t *data = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				ArImage_getWidth(arSession_, depthImage, &width);
+				ArImage_getHeight(arSession_, depthImage, &height);
+				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
+
+				occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+
+				float fx,fy, cx, cy;
+				int32_t rgb_width, rgb_height;
+				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
+
+				float scaleX = (float)width / (float)rgb_width;
+				float scaleY = (float)height / (float)rgb_height;
+				occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+			}
+			ArImage_release(depthImage);
+		}
 	}
 
 	ArCamera_release(ar_camera);

app/android/jni/CameraARCore.h

@@ -38,14 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <background_renderer.h>
 
 #include <arcore_c_api.h>
-#ifdef DEPTH_TEST
 #include <camera/NdkCameraDevice.h>
 #include <camera/NdkCameraManager.h>
 #include <media/NdkImageReader.h>
 #include <android/native_window.h>
-#endif
 
 namespace rtabmap {
 
@@ -61,19 +60,28 @@ public:
 			std::vector<cv::Point3f> * kpts3D = 0);
 
 public:
-	CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
 	virtual ~CameraARCore();
 
+	bool uvsInitialized() const {return uvs_initialized_;}
+	const float* uvsTransformed() const {return transformed_uvs_;}
+	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+
+	void updateOcclusionImage(bool enabled) {updateOcclusionImage_ = enabled;}
+	const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	void setupGL();
 	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
+	GLuint getTextureId() const {return textureId_;}
 
-#ifdef DEPTH_TEST
 	void imageCallback(AImageReader *reader);
-#endif // DEPTH_TEST
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
 	virtual void capturePoseOnly();
 
 private:
@@ -92,22 +100,15 @@ private:
 	GLuint textureId_;
 	UMutex arSessionMutex_;
 
-#ifdef DEPTH_TEST
-	// Camera variables
-	ACameraDevice* cameraDevice_ = nullptr;
-	ACaptureRequest* captureRequest_ = nullptr;
-	ACameraOutputTarget* cameraOutputTarget_ = nullptr;
-	ACaptureSessionOutput* sessionOutput_ = nullptr;
-	ACaptureSessionOutputContainer* captureSessionOutputContainer_ = nullptr;
-	ACameraCaptureSession* captureSession_ = nullptr;
-	ANativeWindow *outputNativeWindow_ = nullptr;
+	float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
+	bool uvs_initialized_ = false;
+	glm::mat4 viewMatrix_;
+	glm::mat4 projectionMatrix_;
 
-	ACameraDevice_StateCallbacks deviceStateCallbacks_;
-	ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
-
-	ACameraManager* cameraManager_ = nullptr;
-	AImageReader* imageReader_ = nullptr;
-#endif // DEPTH_TEST
+	bool updateOcclusionImage_;
+	cv::Mat occlusionImage_;
+	CameraModel occlusionModel_;
+	bool depthFromMotion_;
 };
 
 } /* namespace rtabmap */

app/android/jni/CameraMobile.cpp

@@ -207,6 +207,7 @@ void CameraMobile::mainLoop()
 		// Rotate image depending on the camera orientation
 		if(colorCameraToDisplayRotation_ == ROTATION_90)
 		{
+			UDEBUG("ROTATION_90");
 			cv::Mat rgb, depth;
 			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
 			cv::flip(data.imageRaw(),rgb,1);
@@ -226,9 +227,18 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_180)
 		{
+			UDEBUG("ROTATION_180");
 			cv::Mat rgb, depth;
 			cv::flip(data.imageRaw(),rgb,1);
 			cv::flip(rgb,rgb,0);
@@ -244,9 +254,18 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_270)
 		{
+			UDEBUG("ROTATION_270");
 			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
 			cv::transpose(data.imageRaw(),rgb);
 			cv::flip(rgb,rgb,1);
@@ -263,6 +282,14 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 
 		rtabmap::Transform pose = info.odomPose;

app/android/jni/CameraMobile.h

@@ -94,7 +94,7 @@ public:
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
-	void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
 	void setData(const SensorData & data, const Transform & pose);

app/android/jni/RTABMapApp.cpp

@@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/poisson.h>
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 
+
 #define LOW_RES_PIX 2
 //#define DEBUG_RENDERING_PERFORMANCE
 
@@ -263,7 +264,7 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(camera_)
 	{
-		camera_->setScreenRotation(rotation);
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
 	}
 }
 
@@ -307,7 +308,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 		optRefPose_ = 0;
 	}
 	cv::Mat cloudMat;
-	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -657,6 +658,10 @@ bool RTABMapApp::isBuiltWith(int cameraDriver) const
 
 bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver)
 {
+	//ccapp = new computer_vision::ComputerVisionApplication();
+	//ccapp->OnResume(env, context, activity);
+	//return true;
+
 	cameraDriver_ = driver;
 	LOGW("startCamera() camera driver=%d", cameraDriver_);
 	boost::mutex::scoped_lock  lock(cameraMutex_);
@@ -685,8 +690,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, smoothing_);
-
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -712,7 +716,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 
 	if(camera_->init())
 	{
-		camera_->setScreenRotation(main_scene_.getScreenRotation());
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
 
 		//update mesh decimation based on camera calibration
 		LOGI("Cloud density level %d", cloudDensityLevel_);
@@ -937,6 +941,11 @@ void RTABMapApp::SetViewPort(int width, int height)
 {
 	UINFO("");
 	main_scene_.SetupViewPort(width, height);
+	boost::mutex::scoped_lock  lock(cameraMutex_);
+	if(camera_)
+	{
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
+	}
 }
 
 class PostRenderEvent : public UEvent
@@ -1101,12 +1110,59 @@ int RTABMapApp::Render()
 		}
 
 		// ARCore and AREngine capture should be done in opengl thread!
+		const float* uvsTransformed = 0;
+		glm::mat4 arProjectionMatrix(0);
+		glm::mat4 arViewMatrix(0);
+		rtabmap::Mesh occlusionMesh;
 		if((cameraDriver_ == 1 || cameraDriver_ == 2) && camera_!=0)
 		{
 			boost::mutex::scoped_lock  lock(cameraMutex_);
 			if(camera_!=0)
 			{
+#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ == 1)
+				{
+					((rtabmap::CameraARCore*)camera_)->updateOcclusionImage(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson);
+				}
+#endif
+
 				camera_->spinOnce();
+
+#ifdef RTABMAP_ARCORE
+				if(cameraDriver_ == 1)
+				{
+					if(main_scene_.background_renderer_ == 0)
+					{
+						main_scene_.background_renderer_ = new BackgroundRenderer();
+						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId());
+					}
+					if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
+					{
+						uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
+						((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+					}
+					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+					{
+						rtabmap::CameraModel occlusionModel;
+						cv::Mat occlusionImage = ((rtabmap::CameraARCore*)camera_)->getOcclusionImage(&occlusionModel);
+
+						if(occlusionModel.isValidForProjection())
+						{
+							pcl::IndicesPtr indices(new std::vector<int>);
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, 1, 0, 0, indices.get());
+							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*occlusionModel.localTransform());
+							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
+							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
+							occlusionMesh.indices = indices;
+							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
+						}
+						else
+						{
+							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
+						}
+					}
+				}
+#endif
 			}
 		}
 
@@ -1792,7 +1848,7 @@ int RTABMapApp::Render()
 
 			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
-			lastDrawnCloudsCount_ = main_scene_.Render();
+			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh);
 			if(renderingTime_ < fpsTime.elapsed())
 			{
 				renderingTime_ = fpsTime.elapsed();
@@ -2078,6 +2134,14 @@ void RTABMapApp::setSmoothing(bool enabled)
 	}
 }
 
+void RTABMapApp::setDepthFromMotion(bool enabled)
+{
+	if(depthFromMotion_ != enabled)
+	{
+		depthFromMotion_ = enabled;
+	}
+}
+
 void RTABMapApp::setAppendMode(bool enabled)
 {
 	if(appendMode_ != enabled)
@@ -2837,8 +2901,8 @@ 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
-						std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
+						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)
 						{
@@ -2854,10 +2918,10 @@ 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<unsigned int> > > polygons(textureMesh->tex_polygons.size());
+					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
 					for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
 					{
 						polygons[t].resize(textureMesh->tex_polygons[t].size());
@@ -2990,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;
@@ -3050,7 +3114,7 @@ bool RTABMapApp::postExportation(bool visualize)
 	{
 		visualizingMesh_ = false;
 		cv::Mat cloudMat;
-		std::vector<std::vector<std::vector<unsigned int> > > polygons;
+		std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -3107,7 +3171,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
 	cv::Mat cloudMat;
-	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
 #else
@@ -3122,7 +3186,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 			LOGI("writeExportedMesh: Found optimized mesh!");
 			if(textures.empty())
 			{
-				polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<unsigned int> >());
+				polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<RTABMAP_PCL_INDEX> >());
 			}
 			else
 			{

app/android/jni/RTABMapApp.h

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_base.h>
 #include <pcl/TextureMesh.h>
 
+
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
  public:
@@ -109,6 +110,7 @@ class RTABMapApp : public UEventsHandler {
   void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
   void setSmoothing(bool enabled);
+  void setDepthFromMotion(bool enabled);
   void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
@@ -182,6 +184,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
+  bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;

app/android/jni/background_renderer.cc

@@ -0,0 +1,92 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This modules handles drawing the passthrough camera image into the OpenGL
+// scene.
+
+#include "background_renderer.h"
+
+#include <type_traits>
+
+namespace {
+
+const std::string kVertexShader =
+		"attribute vec4 a_Position;\n"
+		"attribute vec2 a_TexCoord;\n"
+
+		"varying vec2 v_TexCoord;\n"
+
+		"void main() {\n"
+		"   gl_Position = a_Position;\n"
+		"   v_TexCoord = a_TexCoord;\n"
+		"}\n";
+
+const std::string kFragmentShader =
+	"#extension GL_OES_EGL_image_external : require\n"
+
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+	"uniform samplerExternalOES sTexture;\n"
+
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+	"    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+	"    gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+	"}\n";
+
+}  // namespace
+
+void BackgroundRenderer::InitializeGlContent(GLuint textureId)
+{
+  texture_id_ = textureId;
+
+  shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  glUseProgram(shader_program_);
+  attribute_vertices_ = glGetAttribLocation(shader_program_, "a_Position");
+  attribute_uvs_ = glGetAttribLocation(shader_program_, "a_TexCoord");
+  glUseProgram(0);
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs) {
+  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+
+  glUseProgram(shader_program_);
+  glDepthMask(GL_FALSE);
+  glEnable (GL_BLEND);
+
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+
+  glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
+
+  glEnableVertexAttribArray(attribute_vertices_);
+  glEnableVertexAttribArray(attribute_uvs_);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+
+  glDisableVertexAttribArray(attribute_vertices_);
+  glDisableVertexAttribArray(attribute_uvs_);
+
+  glUseProgram(0);
+  glDepthMask(GL_TRUE);
+  glDisable (GL_BLEND);
+  tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
+}
+

app/android/jni/background_renderer.h

@@ -0,0 +1,58 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+
+#include <GLES2/gl2.h>
+#include <GLES2/gl2ext.h>
+#include <cstdlib>
+
+#include "util.h"
+
+static const GLfloat BackgroundRenderer_kVertices[] = {
+	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+};
+
+// This class renders the passthrough camera image into the OpenGL frame.
+class BackgroundRenderer {
+public:
+	// Positions of the quad vertices in clip space (X, Y).
+
+	static constexpr int kNumVertices = 4;
+
+ public:
+  BackgroundRenderer() = default;
+  ~BackgroundRenderer() = default;
+
+  // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
+  // other methods below.
+  void InitializeGlContent(GLuint textureId);
+
+  // Draws the background image.  This methods must be called for every ArFrame
+  // returned by ArSession_update() to catch display geometry change events.
+  void Draw(const float * transformed_uvs);
+
+ private:
+
+  GLuint shader_program_;
+  GLuint texture_id_;
+
+  GLuint attribute_vertices_;
+  GLuint attribute_uvs_;
+};
+
+#endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_

app/android/jni/jni_interface.cpp

@@ -512,6 +512,19 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
 	}
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthFromMotion(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setCameraColor(
 		JNIEnv*, jclass, jlong native_application, bool enabled)
 {

app/android/jni/point_cloud_drawable.cpp

@@ -57,7 +57,7 @@ enum PointCloudShaders
 
 // PointCloud shaders
 const std::string kPointCloudVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
     "attribute vec3 aColor;\n"
@@ -75,7 +75,7 @@ const std::string kPointCloudVertexShader =
     "  vColor = aColor;\n"
     "}\n";
 const std::string kPointCloudLightingVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
 	"attribute vec3 aNormal;\n"
@@ -100,7 +100,7 @@ const std::string kPointCloudLightingVertexShader =
     "}\n";
 
 const std::string kPointCloudFragmentShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
 	"uniform float uGainR;\n"
 	"uniform float uGainG;\n"
@@ -127,7 +127,8 @@ const std::string kPointCloudBlendingFragmentShader =
     "  vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
 	"  float alpha = 1.0;\n"
 	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
-	"  float depth = texture2D(uDepthTexture, coord).r;\n"
+	"  vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+	"  float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
 	"  float num =  (2.0 * uNearZ * uFarZ);\n"
 	"  float diff = (uFarZ - uNearZ);\n"
 	"  float add = (uFarZ + uNearZ);\n"
@@ -141,7 +142,7 @@ const std::string kPointCloudBlendingFragmentShader =
     "}\n";
 
 const std::string kPointCloudDepthPackingVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
     "uniform mat4 uMVP;\n"
@@ -154,15 +155,15 @@ const std::string kPointCloudDepthPackingFragmentShader =
     "precision highp float;\n"
     "precision mediump int;\n"
     "void main() {\n"
-	"  float toFixed = 255.0/256.0;\n"
-	"  vec4 enc = vec4(1.0, 255.0, 65025.0, 160581375.0) * toFixed * gl_FragCoord.z;\n"
+	"  vec4 enc = vec4(1.,255.,65025.,16581375.) * gl_FragCoord.z;\n"
 	"  enc = fract(enc);\n"
+	"  enc -= enc.yzww * vec2(1./255., 0.).xxxy;\n"
 	"  gl_FragColor = enc;\n"
     "}\n";
 
 // Texture shaders
 const std::string kTextureMeshVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
     "attribute vec2 aTexCoord;\n"
@@ -185,7 +186,7 @@ const std::string kTextureMeshVertexShader =
     "  vLightWeighting = 1.0;\n"
     "}\n";
 const std::string kTextureMeshLightingVertexShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
 	"attribute vec3 aNormal;\n"
@@ -214,7 +215,7 @@ const std::string kTextureMeshLightingVertexShader =
     "    vLightWeighting=0.5;\n"
     "}\n";
 const std::string kTextureMeshFragmentShader =
-    "precision mediump float;\n"
+    "precision highp float;\n"
     "precision mediump int;\n"
 	"uniform sampler2D uTexture;\n"
 	"uniform float uGainR;\n"
@@ -245,7 +246,8 @@ const std::string kTextureMeshBlendingFragmentShader =
     "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
     "  float alpha = 1.0;\n"
 	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
-	"  float depth = texture2D(uDepthTexture, coord).r;\n"
+	"  vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+	"  float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
 	"  float num =  (2.0 * uNearZ * uFarZ);\n"
 	"  float diff = (uFarZ - uNearZ);\n"
 	"  float add = (uFarZ + uNearZ);\n"

app/android/jni/scene.cpp

@@ -69,6 +69,7 @@ const std::string kGraphFragmentShader =
 
 
 Scene::Scene() :
+		background_renderer_(0),
 		gesture_camera_(0),
 		axis_(0),
 		frustum_(0),
@@ -96,6 +97,7 @@ Scene::Scene() :
 		g_(0.0f),
 		b_(0.0f),
 		fboId_(0),
+		rboId_(0),
 		depthTexture_(0),
 		screenWidth_(0),
 		screenHeight_(0),
@@ -160,6 +162,8 @@ void Scene::DeleteResources() {
 		delete trace_;
 		delete grid_;
 		delete box_;
+		delete background_renderer_;
+		background_renderer_ = 0;
 	}
 
 	PointCloudDrawable::releaseShaderPrograms();
@@ -173,6 +177,8 @@ void Scene::DeleteResources() {
 	{
 		glDeleteFramebuffers(1, &fboId_);
 		fboId_ = 0;
+		glDeleteRenderbuffers(1, &rboId_);
+		rboId_ = 0;
 		glDeleteTextures(1, &depthTexture_);
 		depthTexture_ = 0;
 	}
@@ -217,16 +223,23 @@ void Scene::SetupViewPort(int w, int h) {
 	UASSERT(gesture_camera_ != 0);
 	gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
 	glViewport(0, 0, w, h);
-	if(screenWidth_ != w || fboId_ == 0)
+	if(screenWidth_ != w || screenHeight_ != h || fboId_ == 0)
 	{
 		if(fboId_>0)
 		{
 			glDeleteFramebuffers(1, &fboId_);
 			fboId_ = 0;
+			glDeleteRenderbuffers(1, &rboId_);
+			rboId_ = 0;
 			glDeleteTextures(1, &depthTexture_);
 			depthTexture_ = 0;
 		}
 
+		// regenerate fbo texture
+		// create a framebuffer object, you need to delete them when program exits.
+		glGenFramebuffers(1, &fboId_);
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
 		// Create depth texture
 		glGenTextures(1, &depthTexture_);
 		glBindTexture(GL_TEXTURE_2D, depthTexture_);
@@ -234,16 +247,17 @@ void Scene::SetupViewPort(int w, int h) {
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-		glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, w, h, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_INT, NULL);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
 		glBindTexture(GL_TEXTURE_2D, 0);
 
-		// regenerate fbo texture
-		// create a framebuffer object, you need to delete them when program exits.
-		glGenFramebuffers(1, &fboId_);
-		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+		glGenRenderbuffers(1, &rboId_);
+		glBindRenderbuffer(GL_RENDERBUFFER, rboId_);
+		glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, w, h);
+		glBindRenderbuffer(GL_RENDERBUFFER, 0);
 
-		// Set the texture to be at the depth attachment point of the FBO
-		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depthTexture_, 0);
+		// Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
+		glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
 
 		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
 		if ( status != GL_FRAMEBUFFER_COMPLETE)
@@ -364,7 +378,7 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render() {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh) {
 	UASSERT(gesture_camera_ != 0);
 
 	if(currentPose_ == 0)
@@ -395,6 +409,17 @@ int Scene::Render() {
 	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
 	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
 
+	bool renderBackgroundCamera =
+			background_renderer_ &&
+			gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
+			!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
+			uvsTransformed;
+	if(renderBackgroundCamera)
+	{
+		projectionMatrix = arProjectionMatrix;
+		viewMatrix = arViewMatrix;
+	}
+
 	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
@@ -444,26 +469,32 @@ int Scene::Render() {
 
 	UTimer timer;
 
-	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
 	if(onlineBlending && fboId_)
 	{
 		// set the rendering destination to FBO
 		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
 
-		glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
-		glClearColor(1, 1, 1, 1);
+		glClearColor(0, 0, 0, 0);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
-		// Draw scene
-		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+		if(renderBackgroundCamera)
 		{
-			// set large distance to cam to use low res polygons for fast processing
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			PointCloudDrawable drawable(occlusionMesh);
+			drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+		}
+		else
+		{
+			// Draw scene
+			for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+			{
+				// set large distance to cam to use low res polygons for fast processing
+				(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+			}
 		}
 
 		// back to normal window-system-provided framebuffer
 		glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
-		glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
 	}
 
 	if(doubleTapOn_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -479,8 +510,7 @@ int Scene::Render() {
 
 		GLubyte zValue[4];
 		glReadPixels(doubleTapPos_.x*screenWidth_, screenHeight_-doubleTapPos_.y*screenHeight_, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, zValue);
-		float fromFixed = 256.0f/255.0f;
-		float zValueF = float(zValue[0]/255.0f)*fromFixed + float(zValue[1]/255.0f)*fromFixed/255.0f + float(zValue[2]/255.0f)*fromFixed/65025.0f + float(zValue[3]/255.0f)*fromFixed/160581375.0f;
+		float zValueF = float(zValue[0]/255.0f) + float(zValue[1]/255.0f)/255.0f + float(zValue[2]/255.0f)/65025.0f + float(zValue[3]/255.0f)/160581375.0f;
 
 		if(zValueF != 0.0f)
 		{
@@ -495,6 +525,15 @@ int Scene::Render() {
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
+	if(renderBackgroundCamera)
+	{
+		background_renderer_->Draw(uvsTransformed);
+
+		//To debug occlusion image:
+		//PointCloudDrawable drawable(occlusionMesh);
+		//drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+	}
+
 	if(!currentPose_->isNull())
 	{
 		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -523,7 +562,7 @@ int Scene::Render() {
 		}
 	}
 
-	if(gridVisible_)
+	if(gridVisible_ && !renderBackgroundCamera)
 	{
 		grid_->Render(projectionMatrix, viewMatrix);
 	}

app/android/jni/scene.h

@@ -38,6 +38,7 @@
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
 #include <bounding_box_drawable.h>
+#include <background_renderer.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -71,13 +72,14 @@ class Scene {
   //         frame's timestamp.
   // @param: point_cloud_vertices, point cloud's vertices of the current point
   //         frame.
-  int Render();
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh());
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
   // @param: camera_type, camera type includes first person, third person and
   //         top down
   void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+  tango_gl::GestureCamera::CameraType GetCameraType() const {return gesture_camera_->GetCameraType();}
 
   void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
   rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
@@ -152,6 +154,8 @@ class Scene {
   bool isLighting() const {return lighting_;}
   bool isBackfaceCulling() const {return backfaceCulling_;}
 
+  BackgroundRenderer * background_renderer_;
+
  private:
   // Camera object that allows user to use touch input to interact with.
   tango_gl::GestureCamera* gesture_camera_;
@@ -200,6 +204,7 @@ class Scene {
   float g_;
   float b_;
   GLuint fboId_;
+  GLuint rboId_;
   GLuint depthTexture_;
   GLsizei screenWidth_;
   GLsizei screenHeight_;

app/android/jni/tango-gl/gesture_camera.cpp

@@ -199,8 +199,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 6.0f;
-      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
+      cam_cur_angle_.x = -M_PI / 12.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;

app/android/jni/util.h

@@ -252,7 +252,7 @@ inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
 // @param display: integer value of display orientation, values available
 // are 0, 1, 2 ,3. Followed by Android display orientation standard:
 // https://developer.android.com/reference/android/view/Display.html#getRotation()
-// @param color_camera: integer value of color camera oreintation, values
+// @param color_camera: integer value of color camera orientation, values
 // available are 0, 90, 180, 270. Followed by Android camera orientation
 // standard:
 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation

app/android/res/layout/activity_rtabmap.xml

@@ -163,6 +163,7 @@
         android:layout_height="100dp"
         android:layout_alignLeft="@+id/button_library"
         android:layout_below="@+id/button_library"
+        android:layout_marginTop="20dp"
         android:text="@string/new_scan" />
 
 </RelativeLayout>        

app/android/res/layout/activity_settings.xml

@@ -7,7 +7,7 @@
    			android:entries="@array/pref_camera_driver_keys"
    			android:entryValues="@array/pref_camera_driver_values"
    			android:defaultValue="@string/pref_default_camera_driver"/>
-    
+        
     <PreferenceCategory
         android:title="@string/pref_title_rendering">
         <ListPreference 
@@ -97,6 +97,11 @@
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
+		     <com.introlab.rtabmap.CustomSwitchPreference
+                android:key="@string/pref_key_depth_from_motion"
+                android:title="@string/pref_title_depth_from_motion"
+                android:summary="@string/pref_summary_depth_from_motion"
+                android:defaultValue="@string/pref_default_depth_from_motion"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"

app/android/res/values/strings.xml

@@ -81,6 +81,8 @@
     
     <string name="pref_key_camera_driver">pref_key_camera_driver</string>
     <string name="pref_default_camera_driver">0</string>
+    <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
+    <string name="pref_default_depth_from_motion">false</string>
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
     <string name="pref_key_max_speed">pref_key_max_speed</string>
@@ -323,6 +325,8 @@
     <string name="pref_title_mapping_database">Database</string>
     <string name="pref_title_camera_driver">Camera Driver</string>
     <string name="pref_summary_camera_driver">AR sdk use for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
+    <string name="pref_title_depth_from_motion">Depth From Motion</string>
+    <string name="pref_summary_depth_from_motion">Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver.</string>
     <string name="pref_title_append">Append Mode</string>
     <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
     <string name="pref_title_resolution">HD Mode</string>

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -992,6 +992,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			RTABMapLib.setRawScanSaved(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
 			RTABMapLib.setFullResolution(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution))));
 			RTABMapLib.setSmoothing(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_smoothing), Boolean.parseBoolean(getString(R.string.pref_default_smoothing))));
+			RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion))));
 			RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
 			RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));
 			RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/DetectionRate", mUpdateRate);
@@ -1162,6 +1163,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
+		final boolean depthFromMotion = sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion)));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 
 		if(!DISABLE_LOG) Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
@@ -1218,7 +1220,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			}
 			Thread bindThread = new Thread(new Runnable() {
 				public void run() {
-					if(mCameraDriver==1)
+					
+					if(mCameraDriver==1 && !depthFromMotion)
 					{
 						RTABMapLib.setMeshRendering(
 								nativeApplication, 
@@ -1263,9 +1266,9 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							}
 							else
 							{
-								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1)
+								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1 && !depthFromMotion)
 								{
-									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses and RGB images can be recorded.", mToast.LENGTH_LONG).show();
+									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses, RGB images and 3d features can be recorded.", mToast.LENGTH_LONG).show();
 								}
 								updateState(mState==State.STATE_VISUALIZING?State.STATE_VISUALIZING_CAMERA:State.STATE_CAMERA);	
 								if(mState==State.STATE_VISUALIZING_CAMERA && mItemLocalizationMode.isChecked())
@@ -2311,6 +2314,15 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		updateState(State.STATE_IDLE);
 
+		if(mArCoreCamera != null)
+		{
+			synchronized (this) {
+				mRenderer.setCamera(null);
+				mArCoreCamera.close();
+				mArCoreCamera = null;
+			}
+		}
+		
 		Thread stopThread = new Thread(new Runnable() {
 			public void run() {
 				if(!DISABLE_LOG) Log.i(TAG, String.format("setPausedMapping()"));

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -72,6 +72,7 @@ public class RTABMapLib
     public static native void setRawScanSaved(long nativeApplication, boolean enabled);
     public static native void setFullResolution(long nativeApplication, boolean enabled);
     public static native void setSmoothing(long nativeApplication, boolean enabled);
+    public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
     public static native void setCameraColor(long nativeApplication, boolean enabled);
     public static native void setAppendMode(long nativeApplication, boolean enabled);
     public static native void setDataRecorderMode(long nativeApplication, boolean enabled);

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
@@ -137,6 +124,16 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
           DESTINATION ${openni2_dest_dir} 
           COMPONENT runtime)
   ENDIF(OpenNI2_FOUND)
+  
+  IF(k4a_FOUND)
+	# Install needed depthengine_2_0.dll
+    IF(WIN32)
+		file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR)
+		INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" 
+          DESTINATION ${plugin_dest_dir} 
+          COMPONENT runtime)
+	ENDIF(WIN32)
+  ENDIF(k4a_FOUND)
 
   # Install needed Qt plugins by copying directories from the qt installation
   # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
@@ -156,26 +153,28 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 		list(GET loc_list 1 plugin_type)
 		IF(NOT plugin_root)
 			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
-		ENDIF(NOT plugin_root)
-	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		ENDIF(NOT plugin_root)
+		#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)
-	endforeach()
-	IF(WIN32)
-		IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
-			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
-			IF(EXISTS ${plugin_loc})
-				get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
-				string(REPLACE "plugins" ";" loc_list ${plugin_dir})
-				list(GET loc_list 1 plugin_type)
-				INSTALL(FILES ${plugin_loc}
-					DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
-					COMPONENT runtime)
-				#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
-			ENDIF(EXISTS ${plugin_loc})
-		ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
-	ENDIF(WIN32)
+	endforeach()
+	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+		IF(WIN32)
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+		ELSEIF(APPLE)
+			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+		ENDIF()
+		IF(EXISTS ${plugin_loc})
+			get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+			string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+			list(GET loc_list 1 plugin_type)
+			INSTALL(FILES ${plugin_loc}
+				DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+				COMPONENT runtime)
+			#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		ENDIF(EXISTS ${plugin_loc})
+	ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
   ENDIF()
   
   # install a qt.conf file

app/src/main.cpp

@@ -35,7 +35,10 @@ 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"
+#endif
 
 using namespace rtabmap;
 
@@ -45,6 +48,10 @@ int main(int argc, char* argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
 
+#ifdef RTABMAP_PYTHON
+	PythonInterface python; // Make sure we initialize python in main thread
+#endif
+
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box

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 <ryan.pavlik@collabora.com> <abiryan@ryand.net>
+# 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/BayesFilter.h

@@ -61,6 +61,8 @@ public:
 
 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
+	unsigned long getMemoryUsed() const;
+
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,

corelib/include/rtabmap/core/Camera.h

@@ -53,6 +53,7 @@ public:
 	virtual ~Camera();
 	SensorData takeImage(CameraInfo * info = 0);
 
+	bool initFromFile(const std::string & calibrationPath);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
@@ -73,7 +74,7 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
 
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded

corelib/include/rtabmap/core/CameraModel.h

@@ -37,6 +37,13 @@ namespace rtabmap {
 
 class RTABMAP_EXP CameraModel
 {
+public:
+	/**
+	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
+	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 */
+	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
+
 public:
 	CameraModel();
 	// K is the camera intrinsic 3x3 CV_64FC1
@@ -50,7 +57,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+			const Transform & localTransform = opticalRotation());
 
 	// minimal
 	CameraModel(
@@ -58,7 +65,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,7 +75,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
@@ -113,9 +120,13 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
-	double fovX() const {return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;}
-	double fovY() const {return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;}
+	double fovX() const; // in radians
+	double fovY() const; // in radians
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
+	bool isFisheye() const {return D_.cols == 6;}
 
+	bool load(const std::string & filePath);
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
 	std::vector<unsigned char> serialize() const;
@@ -125,9 +136,6 @@ public:
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;
 
-	double horizontalFOV() const; // in degrees
-	double verticalFOV() const;   // in degrees
-
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;
@@ -151,5 +159,7 @@ private:
 	Transform localTransform_;
 };
 
+RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const CameraModel& model);
+
 } /* namespace rtabmap */
 #endif /* CAMERAMODEL_H_ */

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,9 +69,18 @@ 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(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			int normalsRadius,
+			bool forceGroundNormalsUp) , "Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.");
 	void setScanParameters(
 			bool fromDepth,
 			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
@@ -80,17 +89,7 @@ public:
 			float voxelSize = 0.0f,
 			int normalsK = 0,
 			int normalsRadius = 0.0f,
-			bool forceGroundNormalsUp = false)
-	{
-		_scanFromDepth = fromDepth;
-		_scanDownsampleStep=downsampleStep;
-		_scanRangeMin = rangeMin;
-		_scanRangeMax = rangeMax;
-		_scanVoxelSize = voxelSize;
-		_scanNormalsK = normalsK;
-		_scanNormalsRadius = normalsRadius;
-		_scanForceGroundNormalsUp = forceGroundNormalsUp;
-	}
+			float groundNormalsUp = 0.0f);
 
 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
 
@@ -119,13 +118,14 @@ private:
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	float _scanNormalsRadius;
-	bool _scanForceGroundNormalsUp;
+	float _scanForceGroundNormalsUp;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;
 	float _bilateralSigmaS;
 	float _bilateralSigmaR;
 	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/DBDriver.h

@@ -70,6 +70,7 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 	virtual bool isInMemory() const {return _url.empty();}
 	const std::string & getUrl() const {return _url;}
+	const std::string & getTargetVersion() const {return _targetVersion;}
 
 	void beginTransaction() const;
 	void commit() const;
@@ -109,7 +110,7 @@ public:
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
 #else
@@ -117,7 +118,7 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
 	cv::Mat loadOptimizedMesh(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
 #else
@@ -131,7 +132,7 @@ public:
 	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
-	long getMemoryUsed() const; // In bytes
+	unsigned long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
 	long getNodesMemoryUsed() const;
 	long getLinksMemoryUsed() const;
@@ -188,7 +189,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
-	virtual long getMemoryUsedQuery() const = 0; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
 	virtual long getNodesMemoryUsedQuery() const = 0;
 	virtual long getLinksMemoryUsedQuery() const = 0;
@@ -247,7 +248,7 @@ protected:
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(
 				const cv::Mat & cloud,
-				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+				const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -255,7 +256,7 @@ protected:
 #endif
 				const cv::Mat & textures) const = 0;
 	virtual cv::Mat loadOptimizedMeshQuery(
-				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+				std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -300,6 +301,7 @@ private:
 	USemaphore _addSem;
 	double _emptyTrashesTime;
 	std::string _url;
+	std::string _targetVersion;
 	bool _timestampUpdate;
 };
 

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -54,7 +54,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
-	virtual long getMemoryUsedQuery() const; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
 	virtual long getNodesMemoryUsedQuery() const;
 	virtual long getLinksMemoryUsedQuery() const;
@@ -113,7 +113,7 @@ protected:
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
 	virtual void saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
 #else
@@ -121,7 +121,7 @@ protected:
 #endif
 			const cv::Mat & textures) const;
 	virtual cv::Mat loadOptimizedMeshQuery(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
@@ -189,7 +189,7 @@ protected:
 	std::string _version;
 
 private:
-	long _memoryUsedEstimate;
+	unsigned long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <pcl/point_cloud.h>
@@ -91,41 +92,133 @@ public:
 	 * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
 	 * realPairsCount = 4
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::map<int, cv::KeyPoint> & wordsA,
-			const std::map<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::map<int, T> & wordsA,
+			const std::map<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		int realPairsCount = 0;
+		pairs.clear();
+		for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
+			{
+				std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+				if(ptB != wordsB.end())
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		typename std::multimap<int, T>::const_iterator iterA;
+		typename std::multimap<int, T>::const_iterator iterB;
+		pairs.clear();
+		int realPairsCount = 0;
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
+			{
+				iterA = wordsA.find(*i);
+				iterB = wordsB.find(*i);
+				while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
+					++iterA;
+					++iterB;
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsUnique(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		int realPairsCount = 0;
+		pairs.clear();
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *i);
+				std::list<T> ptsB = uValues(wordsB, *i);
+				if(ptsA.size() == 1 && ptsB.size() == 1)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
+					++realPairsCount;
+				}
+				else if(ptsA.size()>1 && ptsB.size()>1)
+				{
+					// just update the count
+					realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsAll(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-			bool ignoreNegativeIds = true);
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		pairs.clear();
+		int realPairsCount = 0;;
+		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *iter);
+				std::list<T> ptsB = uValues(wordsB, *iter);
+
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+				for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+				{
+					for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+					{
+						pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
+					}
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	static cv::Mat linearLSTriangulation(
 			cv::Point3d u,    //homogenous image point (u,v,1)

corelib/include/rtabmap/core/Features2d.h

@@ -61,8 +61,9 @@ typedef cv::gpu::FAST_GPU CV_FAST_GPU;
 namespace cv{
 namespace xfeatures2d {
 class FREAK;
+class DAISY;
 class BriefDescriptorExtractor;
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 class SIFT;
 #endif
 class SURF;
@@ -73,14 +74,15 @@ class ORB;
 class SURF_CUDA;
 }
 }
-#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
 #else
-typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.3.0-dev
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
 #endif
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;
 typedef cv::xfeatures2d::FREAK CV_FREAK;
+typedef cv::xfeatures2d::DAISY CV_DAISY;
 typedef cv::GFTTDetector CV_GFTT;
 typedef cv::xfeatures2d::BriefDescriptorExtractor CV_BRIEF;
 typedef cv::BRISK CV_BRISK;
@@ -115,7 +117,12 @@ public:
 		kFeatureGfttOrb=8,  //new 0.10.11
 		kFeatureKaze=9,     //new 0.13.2
 		kFeatureOrbOctree=10, //new 0.19.2
-		kFeatureSuperPointTorch=11}; //new 0.19.7
+		kFeatureSuperPointTorch=11, //new 0.19.7
+		kFeatureSurfFreak=12, //new 0.20.4
+		kFeatureGfttDaisy=13, //new 0.20.6
+		kFeatureSurfDaisy=14,  //new 0.20.6
+		kFeaturePyDetector=15}; //new 0.20.8
+
 	static std::string typeName(Type type)
 	{
 		switch(type){
@@ -143,6 +150,12 @@ public:
 			return "ORB-OCTREE";
 		case kFeatureSuperPointTorch:
 			return "SUPERPOINT";
+		case kFeatureSurfFreak:
+			return "SURF+Freak";
+		case kFeatureGfttDaisy:
+			return "GFTT+Daisy";
+		case kFeatureSurfDaisy:
+			return "SURF+Daisy";
 		default:
 			return "Unknown";
 		}
@@ -455,6 +468,28 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };
 
+//SURF_FREAK
+class RTABMAP_EXP SURF_FREAK : public SURF
+{
+public:
+	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfFreak;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+	cv::Ptr<CV_FREAK> _freak;
+};
+
 //GFTT_ORB
 class RTABMAP_EXP GFTT_ORB : public GFTT
 {
@@ -538,6 +573,8 @@ private:
 private:
 	float scaleFactor_;
 	int nLevels_;
+	int patchSize_;
+	int edgeThreshold_;
 	int fastThreshold_;
 	int fastMinThreshold_;
 
@@ -568,6 +605,54 @@ private:
 	bool cuda_;
 };
 
+//GFTT_DAISY
+class RTABMAP_EXP GFTT_DAISY : public GFTT
+{
+public:
+	GFTT_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
+//SURF_DAISY
+class RTABMAP_EXP SURF_DAISY : public SURF
+{
+public:
+	SURF_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
 }
 
 #endif /* FEATURES2D_H_ */

corelib/include/rtabmap/core/FlannIndex.h

@@ -43,8 +43,8 @@ public:
 	void release();
 	unsigned int indexedFeatures() const;
 
-	// return KB
-	unsigned int memoryUsed() const;
+	// return Bytes
+	unsigned long memoryUsed() const;
 
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
@@ -74,7 +74,7 @@ public:
 	int featuresType() const {return featuresType_;}
 	int featuresDim() const {return featuresDim_;}
 
-	unsigned int addPoints(const cv::Mat & features);
+	std::vector<unsigned int> addPoints(const cv::Mat & features);
 
 	void removePoint(unsigned int index);
 

corelib/include/rtabmap/core/Graph.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/GPS.h>
+#include <rtabmap/core/CameraModel.h>
 
 namespace rtabmap {
 class Memory;
@@ -45,9 +46,9 @@ namespace graph {
 // Graph utilities
 ////////////////////////////////////////////
 
-	bool RTABMAP_EXP exportPoses(
+bool RTABMAP_EXP exportPoses(
 		const std::string & filePath,
-		int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
+		int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame, 11=10+ID), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
 		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
@@ -55,10 +56,10 @@ namespace graph {
 
 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
-		std::map<int, double> * stamps = 0); // optional for format 1
+		std::map<int, double> * stamps = 0); // optional for format 1 and 9
 
 bool RTABMAP_EXP exportGPS(
 		const std::string & filePath,
@@ -155,12 +156,20 @@ std::list<Link> RTABMAP_EXP findLinks(
 
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);
 
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
@@ -255,11 +264,26 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float linearVelocity = 0.0f,   // m/sec
 		float angularVelocity = 0.0f); // rad/sec
 
+/**
+ * Get the nearest node of the target pose
+ * @param nodes the nodes to search for
+ * @param targetPose the target pose to search around
+ * @param distance squared distance of the nearest node found (optional)
+ * @return the node id.
+ */
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose);
+		const rtabmap::Transform & targetPose,
+		float * distance = 0);
 
-std::vector<int> RTABMAP_EXP findNearestNodes(
+/**
+ * Get K nearest nodes of the target pose
+ * @param nodes the nodes to search for
+ * @param targetPose the target pose to search around
+ * @param k number of nearest neighbors to search for
+ * @return the node ids with squared distance to target pose.
+ */
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k);
@@ -275,11 +299,20 @@ std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius);
+std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius);
 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius,
 		float angle = 0.0f);
+std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		const Transform & targetPose,
+		const std::map<int, Transform> & nodes,
+		float radius,
+		float angle = 0.0f);
 
 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
@@ -294,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,
@@ -97,6 +121,7 @@ public:
 	float angleIncrement() const {return angleIncrement_;}
 	Transform localTransform() const {return localTransform_;}
 
+	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
 	int size() const {return data_.cols;}
 	int dataType() const {return data_.type();}
@@ -113,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/MarkerDetector.h

@@ -50,6 +50,8 @@ private:
 	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
 	float markerLength_;
 	float maxDepthError_;
+	float maxRange_;
+	float minRange_;
 	int dictionaryId_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 #endif

corelib/include/rtabmap/core/Memory.h

@@ -102,7 +102,7 @@ public:
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
 #else
@@ -110,7 +110,7 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
 	cv::Mat loadOptimizedMesh(
-			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
 #else
@@ -199,9 +199,10 @@ public:
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
 	void getNodeWordsAndGlobalDescriptors(int nodeId,
-			std::multimap<int, cv::KeyPoint> & words,
-			std::multimap<int, cv::Point3f> & words3,
-			std::multimap<int, cv::Mat> & wordsDescriptors,
+			std::multimap<int, int> & words,
+			std::vector<cv::KeyPoint> & wordsKpts,
+			std::vector<cv::Point3f> & words3,
+			cv::Mat & wordsDescriptors,
 			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
@@ -225,6 +226,7 @@ public:
 	virtual void dumpMemory(std::string directory) const;
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+	unsigned long getMemoryUsed() const; //Bytes
 
 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
 

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -104,6 +104,8 @@ public:
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
 
+	unsigned long getMemoryUsed() const;
+
 private:
 	ParametersMap parameters_;
 	int cloudDecimation_;

corelib/include/rtabmap/core/OctoMap.h

@@ -168,9 +168,6 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTr
   };
 
 class RTABMAP_EXP OctoMap {
-public:
-	static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
-
 public:
 	OctoMap(const ParametersMap & parameters);
 	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
@@ -196,7 +193,8 @@ public:
 			std::vector<int> * groundIndices = 0,
 			bool originalRefPoints = true,
 			std::vector<int> * frontierIndices = 0,
-			std::vector<double> * cloudProb = 0) const;
+			std::vector<double> * cloudProb = 0,
+            bool applyFloodFill = false ) const;
 
 	cv::Mat createProjectionMap(
 			float & xMin,

corelib/include/rtabmap/core/Odometry.h

@@ -49,11 +49,12 @@ public:
 		kTypeFovis = 2,
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
-		kTypeORBSLAM2 = 5,
+		kTypeORBSLAM = 5,
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
-		kTypeVINS = 9
+		kTypeVINS = 9,
+		kTypeOpenVINS = 10
 	};
 
 public:
@@ -67,7 +68,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

@@ -0,0 +1,47 @@
+/*
+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_INCLUDE_RTABMAP_CORE_PDALWRITER_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_PDALWRITER_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+std::string getPDALSupportedWriters();
+
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud,   const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_PDALWRITER_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -236,6 +236,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
     RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
     RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
+    RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
@@ -243,9 +244,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "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 Torch.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "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 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "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 Torch.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "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 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #endif
     RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
     RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
@@ -264,6 +265,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3,           "0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")");
     RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0,           "0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")");
     RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2,           "0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
+    RTABMAP_PARAM_STR(Db, TargetVersion,   "",               "Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., 0.19 vs 0.20 or 1.0 vs 2.0). Patch version is ignored (e.g., 0.20.1 and 0.20.3 will generate a 0.20 database).");
 
     // Keypoints descriptors/detectors
     RTABMAP_PARAM(SURF, Extended,          bool, false,  "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
@@ -294,14 +296,14 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(FAST, CV,                 int, 0,       "Enable FastCV implementation if non-zero (and RTAB-Map is built with FastCV support). Values should be 9 and 10.");
 
     RTABMAP_PARAM(GFTT, QualityLevel,      double, 0.001, "");
-    RTABMAP_PARAM(GFTT, MinDistance,       double, 3,    "");
+    RTABMAP_PARAM(GFTT, MinDistance,       double, 7,    "");
     RTABMAP_PARAM(GFTT, BlockSize,         int, 3,       "");
     RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false,  "");
     RTABMAP_PARAM(GFTT, K,                 double, 0.04, "");
 
-    RTABMAP_PARAM(ORB, ScaleFactor,   float, 1.2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
-    RTABMAP_PARAM(ORB, NLevels,       int, 8,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
-    RTABMAP_PARAM(ORB, EdgeThreshold, int, 31,     "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
+    RTABMAP_PARAM(ORB, ScaleFactor,   float, 2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
+    RTABMAP_PARAM(ORB, NLevels,       int, 3,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
+    RTABMAP_PARAM(ORB, EdgeThreshold, int, 19,     "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
     RTABMAP_PARAM(ORB, FirstLevel,    int, 0,      "It should be 0 in the current implementation.");
     RTABMAP_PARAM(ORB, WTA_K,         int, 2,      "The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).");
     RTABMAP_PARAM(ORB, ScoreType,     int, 0,      "The default HARRIS_SCORE=0 means that Harris algorithm is used to rank features (the score is written to KeyPoint::score and is used to retain best nfeatures features); FAST_SCORE=1 is alternative value of the parameter that produces slightly less stable keypoints, but it is a little faster to compute.");
@@ -330,6 +332,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
     RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
 
+    RTABMAP_PARAM_STR(PyDetector, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
+	RTABMAP_PARAM(PyDetector, Cuda,           bool, true,   "Use cuda.");
+
     // BayesFilter
     RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
     RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
@@ -342,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).");
@@ -351,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.");
@@ -360,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.");
@@ -373,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, 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, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path. Set to 0 to disable 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, ProximityAngle,               float, 45,   "Maximum angle (degrees) for visual proximity detection.");
-    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for visual proximity detection.");
+    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 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 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 for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
+    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
 
     // Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -406,9 +412,13 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
     RTABMAP_PARAM(Optimizer, LandmarksIgnored,   bool, false,  "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
+#if defined(RTABMAP_G2O) || defined(RTABMAP_GTSAM)
+    RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.3,      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()));
+#else
     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");
@@ -450,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.");
@@ -511,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(OdomORBSLAM2, 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(OdomORBSLAM2, Fps,             float,  0.0,   "Camera FPS.");
-    RTABMAP_PARAM(OdomORBSLAM2, 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_STR(OdomORBSLAM, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM, Fps,             float,  0.0,   "Camera FPS.");
+    RTABMAP_PARAM(OdomORBSLAM, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
+    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.");
@@ -572,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()));
@@ -586,9 +596,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "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 Torch.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "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 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "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 Torch.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "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 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #endif
     RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,   "0 no limits.");
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,   "Max depth of the features (0 means no limit).");
@@ -602,21 +612,21 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
     RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 40,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     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.");
 #endif
 
     // Features matching approaches
-    RTABMAP_PARAM_STR(PyMatcher, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/pymatcher/*). See the header to see where the script should be copied.");
+    RTABMAP_PARAM_STR(PyMatcher, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
 	RTABMAP_PARAM(PyMatcher, Iterations,     int, 20,      "Sinkhorn iterations. Used by SuperGlue.");
 	RTABMAP_PARAM(PyMatcher, Threshold,      float, 0.2,   "Used by SuperGlue.");
 	RTABMAP_PARAM(PyMatcher, Cuda,           bool, true,   "Used by SuperGlue.");
@@ -627,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).");
@@ -641,25 +656,28 @@ 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.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else
-    RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
 #endif
-    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
+    RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, 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, 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, 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, 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.");
@@ -740,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).");
@@ -752,6 +771,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
     RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
     RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,    "Maximum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,    "Miniminum range in which markers will be detected. <=0 for unlimited range.");
 
     RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
     RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
@@ -839,4 +860,3 @@ private:
 }
 
 #endif /* PARAMETERS_H_ */
-

corelib/include/rtabmap/core/PythonInterface.h

@@ -0,0 +1,41 @@
+/*
+ * PythonInterface.h
+ *
+ *  Created on: Jan. 14, 2021
+ *      Author: mathieu
+ */
+
+#ifndef CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
+#define CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
+
+
+#include <string>
+#include <rtabmap/utilite/UMutex.h>
+#include <Python.h>
+
+namespace rtabmap {
+
+class PythonInterface
+{
+public:
+	PythonInterface();
+	virtual ~PythonInterface();
+
+protected:
+	std::string getTraceback(); // should be called between lock() and unlock()
+	void lock();
+	void unlock();
+
+private:
+	static UMutex mutex_;
+	static int refCount_;
+
+protected:
+	static PyThreadState * mainThreadState_;
+	static unsigned long mainThreadID_;
+	PyThreadState * threadState_;
+};
+
+}
+
+#endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */

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,16 +67,24 @@ private:
 	int _maxIterations;
 	float _epsilon;
 	float _correspondenceRatio;
+	bool _force4DoF;
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;
+	float _pointToPlaneGroundNormalsUp;
 	float _pointToPlaneMinComplexity;
-	bool _libpointmatcher;
+	int _pointToPlaneLowComplexityStrategy;
 	std::string _libpointmatcherConfig;
 	int _libpointmatcherKnn;
 	float _libpointmatcherEpsilon;
-	float _libpointmatcherOutlierRatio;
+	bool _libpointmatcherIntensity;
+	float _outlierRatio;
+	unsigned int _ccSamplingLimit;
+	bool _ccFilterOutFarthestPoints;
+	double _ccMaxFinalRMS;
+
 	void * _libpointmatcherICP;
+	void * _libpointmatcherICPFilters;
 };
 
 }