CloudViewer: fixed build with vtk<6

* Added Kinect for Azure menu option

* Reintegrated Kinect for Azure initialization code, fixed previous GUI commit

* Added ifdefs to fix build

* Added preliminary K4A device support

* Edits to K4A camera implementation

* K4A minor initialization fix

* Typo

* Another initialization fix

* Update CameraRGBD tool to display K4A camera output

* K4A added model parameter so we can create maps

.appveyor.yml

@@ -18,9 +18,11 @@ init:
 
 install:
   # Qt
-  - set QTDIR=C:\Qt\5.8\msvc2015_64
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
   # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
   - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
   # Openni2
   - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
   - cmd: OpenNI2.exe -o"C:\Program Files" -y
@@ -31,29 +33,94 @@ 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/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
   - ECHO "Installed OpenCV:"
   - ps: "ls \"C:/Program Files/opencv/build\""
   - set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
-  # PCL (including QVTK)
-  - ps: wget 'https://dl.dropboxusercontent.com/s/atf4r8kb1xyc1ls/PCL%201.8.1.exe?dl=0' -outfile PCL_1.8.1.exe
+  # 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
+  - ECHO "Installed VTK:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed QHull:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed FLANN:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed Eigen:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe
   - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
   - ECHO "Installed PCL:"
-  - ps: "ls \"C:/Program Files/PCL 1.8.1\""
-  - set PATH=%PATH%;C:\Program Files\PCL 1.8.1\bin
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
   # zlib
   - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
   - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
   - ECHO "Installed zlib:"
   - ps: "ls \"C:/Program Files/zlib\""
   - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
+  # OctoMap
+  - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe
+  - cmd: octomap.exe -o"C:\Program Files" -y
+  - ECHO "Installed OctoMap:"
+  - ps: "ls \"C:/Program Files/octomap-distribution\""
+  - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin
+  # CPU-TSDF
+  - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe
+  - cmd: cpu_tsdf.exe -o"C:\Program Files" -y
+  - ECHO "Installed CPU-TSDF:"
+  - ps: "ls \"C:/Program Files/cpu_tsdf\""
+  - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin
+  # Open Chisel
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe
+  - cmd: open_chisel.exe -o"C:\Program Files" -y
+  - 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
+  - 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\""
   
 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 1.8.1\cmake" -DZLIB_ROOT="C:\Program Files\zlib" ..
+  - 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 ..
   
 after_build :
   - cmake --build . --config Release --target package
@@ -64,7 +131,7 @@ artifacts:
 notifications:
   - provider: Email
     to:
-      - matlabbe@email.com
+      - matlabbe@gmail.com
     on_build_success: false
     on_build_failure: false
     on_build_status_changed: true

.gitignore

@@ -2,6 +2,7 @@
 .DS_Store
 .settings/language.settings.xml
 .idea/
+.vscode
 cmake-build-debug/
 app/android/.classpath
 app/android/.project

.travis.yml

@@ -1,32 +1,57 @@
 sudo: true
-dist: trusty
 language: cpp
 group: deprecated-2017Q3
 
 compiler:
   - gcc
-  - clang
 
-addons:
-  apt:
-    packages:
-      - cmake
-      - libopencv-dev
-      - libqt4-dev
-      - libsqlite3-dev
-      - libyaml-cpp-dev
+matrix:
+  include:
 
-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 -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev
+    - 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
+      script: 
+        - source /opt/ros/indigo/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
+
+    - dist: xenial
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-kinetic-rtabmap-ros
+        - sudo apt-get -y remove ros-kinetic-rtabmap
+
+      script: 
+        - source /opt/ros/kinetic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
+
+    - dist: bionic
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-melodic-rtabmap-ros
+        - sudo apt-get -y remove ros-melodic-rtabmap
+
+      script: 
+        - source /opt/ros/melodic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
 
 notifications:
   email:

CMakeLists.txt

@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 17)
-SET(RTABMAP_PATCH_VERSION 1)
+SET(RTABMAP_MINOR_VERSION 20)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -43,7 +43,14 @@ ENDIF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
 
 IF(NOT ANDROID)
     SET(CMAKE_DEBUG_POSTFIX "d")
-ENDIF(NOT ANDROID)
+    option(FLANN_KDTREE_MEM_OPT  "Disable multi-threaded FLANN kd-tree to minimize memory allocations"   OFF)
+ELSE()
+    option(FLANN_KDTREE_MEM_OPT  "Disable multi-threaded FLANN kd-tree to minimize memory allocations"   ON)
+ENDIF()
+
+IF(FLANN_KDTREE_MEM_OPT)
+    ADD_DEFINITIONS("-DFLANN_KDTREE_MEM_OPT")
+ENDIF(FLANN_KDTREE_MEM_OPT)
 
 IF(WIN32 AND NOT MINGW)
     ADD_DEFINITIONS("-DNOMINMAX")
@@ -85,6 +92,17 @@ IF(CMAKE_COMPILER_IS_GNUCXX)
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fmessage-length=0")
 ENDIF(CMAKE_COMPILER_IS_GNUCXX)
 
+if(MSVC)
+  if(MSVC_VERSION GREATER 1500 AND ${CMAKE_VERSION} VERSION_GREATER "2.8.6")
+    include(ProcessorCount)
+    ProcessorCount(N)
+    if(NOT N EQUAL 0)
+      SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP${N}")
+      SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N}")
+    endif()
+  endif()
+endif()
+
 # [Eclipse] Automatic Discovery of Include directories (Optional, but handy) 
 #SET(CMAKE_VERBOSE_MAKEFILE ON)
 
@@ -131,9 +149,9 @@ IF(ANDROID_PREBUILD)
    return()
 ENDIF(ANDROID_PREBUILD)
 
-IF(APPLE)
-    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
-ENDIF(APPLE)
+IF(APPLE OR WIN32)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle with all embedded dependencies (DragNDrop for Mac, installer for Windows)" OFF)
+ENDIF(APPLE OR WIN32)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
@@ -144,30 +162,50 @@ option(WITH_QT            "Include Qt support"                   OFF)
 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_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
+option(WITH_K4A           "Include Kinect for Azure support"     ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
 option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_TORO          "Include TORO support"                 ON)
+option(WITH_CERES         "Include Ceres support"                ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_POINTMATCHER  "Include libpointmatcher 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_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_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
 option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
+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_OKVIS         "Include OKVIS support"                ON)
+option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"           OFF)
+option(WITH_VINS          "Include VINS-Fusion support"          ON)
+option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
+option(WITH_FASTCV        "Include FastCV support"               ON)
+IF(ANDROID)
+option(PCL_OMP            "With PCL OMP implementations"         OFF)
+ELSE()
 option(PCL_OMP            "With PCL OMP implementations"         ON)
+ENDIF()
+
+set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
+set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
 
@@ -176,14 +214,27 @@ FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface f
 ELSE()
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation )
 ENDIF()
-if("${PCL_DEFINITIONS}" MATCHES "-march=native")
-  MESSAGE(WARNING "PCL definitions contain \"-march=native\", make sure all libraries using Eigen are also compiled with that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+if(PCL_COMPILE_OPTIONS)
+  if("${PCL_COMPILE_OPTIONS}" MATCHES "-march=native")
+    MESSAGE(WARNING "PCL compile options contain \"-march=native\", make sure all libraries using Eigen are also compiled with that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+  else()
+    MESSAGE(STATUS "PCL compile options don't contain \"-march=native\", make sure all libraries using Eigen are also compiled without that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+  endif()
 else()
-  MESSAGE(STATUS "PCL definitions don't contain \"-march=native\", make sure all libraries using Eigen are also compiled without that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+  if("${PCL_DEFINITIONS}" MATCHES "-march=native")
+    MESSAGE(WARNING "PCL definitions contain \"-march=native\", make sure all libraries using Eigen are also compiled with that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+  else()
+    MESSAGE(STATUS "PCL definitions don't contain \"-march=native\", make sure all libraries using Eigen are also compiled without that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+  endif()
 endif()
 
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
+FIND_PACKAGE(Sqlite3 QUIET)
+IF(Sqlite3_FOUND)
+   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
+ENDIF(Sqlite3_FOUND)
+
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
    # fix libproj.so not found on Xenial
    list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
@@ -197,6 +248,7 @@ endif()
 if(OPENMP_FOUND)
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+  set(CMAKE_INSTALL_OPENMP_LIBRARIES TRUE)
   message (STATUS "Found OpenMP")
   if(PCL_OMP)
     add_definitions(-DPCL_OMP)
@@ -225,14 +277,18 @@ IF(WITH_QT)
     # If Qt is here, the GUI will be built
     # look for Qt5 (if vtk>5 is installed) before Qt4
     IF("${VTK_MAJOR_VERSION}" GREATER 5)
-        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
-        IF(Qt5_FOUND)
-             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
-        ENDIF(Qt5_FOUND)
+        if(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
+            FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
+            IF(Qt5_FOUND)
+                FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
+            ENDIF(Qt5_FOUND)
+        ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "5")
     ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
 
     IF(NOT Qt5_FOUND)
-        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
+        IF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4")
+            FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
+        ENDIF(RTABMAP_QT_VERSION STREQUAL "AUTO" OR RTABMAP_QT_VERSION STREQUAL "4")
     ENDIF(NOT Qt5_FOUND)
         
     IF(QT4_FOUND OR Qt5_FOUND)
@@ -244,10 +300,42 @@ IF(WITH_QT)
                 SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
                 SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
             ENDIF(value EQUAL -1)
+            MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}")
+
+            IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
+                ADD_DEFINITIONS("-DVTK_OPENGL2")
+                # see issue #525
+                list(FIND PCL_LIBRARIES vtkRenderingVolumeOpenGL2 value)
+                IF(value EQUAL -1)
+                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkRenderingVolumeOpenGL2")
+                ENDIF(value EQUAL -1)
+            ELSEIF(VTK_RENDERING_BACKEND STREQUAL "OpenGL")
+                # PCL 1.9 not adding vtkRenderingVolumeOpenGL as dependency anymore, so add it for us
+                list(FIND PCL_LIBRARIES vtkRenderingVolumeOpenGL value)
+                IF(value EQUAL -1)
+                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkRenderingVolumeOpenGL")
+                ENDIF(value EQUAL -1)
+            ENDIF()
+
         ENDIF()
+        ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals/foreach and Qt macros
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
+IF(WITH_SUPERPOINT_TORCH)
+    FIND_PACKAGE(Torch QUIET)
+    IF(TORCH_FOUND)
+        MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
+    ENDIF(TORCH_FOUND)
+ENDIF(WITH_SUPERPOINT_TORCH)
+
+IF(WITH_PYMATCHER)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    IF(Python3_FOUND)
+        MESSAGE(STATUS "Found Python3")
+    ENDIF(Python3_FOUND)
+ENDIF(WITH_PYMATCHER)
+
 IF(WITH_FREENECT)
     FIND_PACKAGE(Freenect QUIET)
     IF(Freenect_FOUND)
@@ -280,6 +368,21 @@ IF(WITH_K4W2 AND WIN32)
     ENDIF(KinectSDK2_FOUND)
 ENDIF(WITH_K4W2 AND WIN32)
 
+IF(WITH_K4A)
+    IF(WIN32)
+        FIND_PACKAGE(K4A QUIET)
+    ELSE()
+        FIND_PACKAGE(k4a QUIET)
+        FIND_PACKAGE(k4arecord QUIET)
+        IF(NOT (k4a_FOUND AND k4arecord_FOUND))
+            SET(k4a_FOUND FALSE)
+        ENDIF(NOT (k4a_FOUND AND k4arecord_FOUND))
+    ENDIF()
+	IF(k4a_FOUND)
+       MESSAGE(STATUS "Found Kinect for Azure: ${k4a_INCLUDE_DIRS}")
+    ENDIF(k4a_FOUND)
+ENDIF(WITH_K4A)
+
 # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
 IF(WITH_OPENNI2 OR OpenNI2_FOUND)
     FIND_PACKAGE(OpenNI2 QUIET)
@@ -303,7 +406,8 @@ IF(WITH_G2O)
 ENDIF(WITH_G2O)
 
 IF(WITH_GTSAM)
-    FIND_PACKAGE(GTSAM QUIET)
+    # Force config mode to ignore PCL's findGTSAM.cmake file
+    FIND_PACKAGE(GTSAM CONFIG QUIET)
 ENDIF(WITH_GTSAM)
 
 IF(WITH_FLYCAPTURE2)
@@ -327,23 +431,16 @@ IF(WITH_POINTMATCHER)
     ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)
 
+IF(WITH_LOAM)
+    find_package(loam_velodyne QUIET)
+    IF(loam_velodyne_FOUND)
+       MESSAGE(STATUS "Found loam_velodyne: ${loam_velodyne_INCLUDE_DIRS}")
+    ENDIF(loam_velodyne_FOUND)
+ENDIF(WITH_LOAM)
+
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
-    IF(WIN32) # Windows
-        SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
-        if (CMAKE_CL_64) # 64 bits
-            SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
-        else(CMAKE_CL_64) # 32 bits
-            message("32bits compilation is no more available with CUDA7.0")
-        endif(CMAKE_CL_64)
-        SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
-        IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
-            SET(ZED_FOUND TRUE)
-            LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
-        ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
-    ELSE() # Linux
-        find_package(ZED 2 QUIET)
-    ENDIF(WIN32)
+    find_package(ZED 2 QUIET)
     
     IF(ZED_FOUND)
        MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}")
@@ -371,14 +468,32 @@ IF(WITH_REALSENSE)
     ENDIF(RealSenseSlam_FOUND)
 ENDIF(WITH_REALSENSE)
 
+IF(WITH_REALSENSE2)
+    IF(WIN32)
+        FIND_PACKAGE(RealSense2 QUIET)
+	ELSE()
+        FIND_PACKAGE(realsense2 QUIET)
+    ENDIF()
+    IF(realsense2_FOUND)
+       MESSAGE(STATUS "Found RealSense2: ${realsense2_INCLUDE_DIRS}")
+    ENDIF(realsense2_FOUND)
+ENDIF(WITH_REALSENSE2)
+
+IF(WITH_MYNTEYE)
+    FIND_PACKAGE(mynteye QUIET)
+    IF(mynteye_FOUND)
+       MESSAGE(STATUS "Found mynteye-s: ${mynteye_INCLUDE_DIRS}")
+    ENDIF(mynteye_FOUND)
+ENDIF(WITH_MYNTEYE)
+
 IF(WITH_OCTOMAP)
-    FIND_PACKAGE(OCTOMAP QUIET)
-    IF(OCTOMAP_FOUND)
-       MESSAGE(STATUS "Found octomap ${OCTOMAP_VERSION}: ${OCTOMAP_INCLUDE_DIRS}")
-       IF(OCTOMAP_VERSION VERSION_LESS 1.8)
+    FIND_PACKAGE(octomap QUIET)
+    IF(octomap_FOUND)
+       MESSAGE(STATUS "Found octomap ${octomap_VERSION}: ${OCTOMAP_INCLUDE_DIRS}")
+       IF(octomap_VERSION VERSION_LESS 1.8)
           ADD_DEFINITIONS("-DOCTOMAP_PRE_18")
-       ENDIF(OCTOMAP_VERSION VERSION_LESS 1.8)
-    ENDIF(OCTOMAP_FOUND)
+       ENDIF(octomap_VERSION VERSION_LESS 1.8)
+    ENDIF(octomap_FOUND)
 ENDIF(WITH_OCTOMAP)
 
 IF(WITH_CPUTSDF)
@@ -395,6 +510,17 @@ IF(WITH_OPENCHISEL)
     endif(open_chisel_FOUND)
 ENDIF(WITH_OPENCHISEL)
 
+IF(WITH_ALICE_VISION)
+    find_package(AliceVision CONFIG QUIET)
+    IF(AliceVision_FOUND)
+        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")
+    ENDIF(AliceVision_FOUND)
+ENDIF(WITH_ALICE_VISION)
+
 IF(WITH_FOVIS)
     FIND_PACKAGE(libfovis QUIET)
     IF(libfovis_FOUND)
@@ -424,11 +550,40 @@ IF(WITH_OKVIS)
        MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
        find_package(opengv REQUIRED)
        MESSAGE(STATUS "Found opengv: ${OPENGV_INCLUDE_DIRS}")
-       find_package(Ceres REQUIRED CONFIG PATHS ${OKVIS_CERES_CONFIG} NO_DEFAULT_PATH)
-       MESSAGE(STATUS "Found ceres: ${CERES_INCLUDE_DIRS}")
+       find_package(Ceres 1.9.0 REQUIRED EXACT) # OKVIS requires this specific version
+       MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
     ENDIF(okvis_FOUND)
 ENDIF(WITH_OKVIS)
 
+# If built with okvis, we found already ceres above
+IF(NOT okvis_FOUND AND WITH_CERES)
+    FIND_PACKAGE(Ceres QUIET)
+ENDIF(NOT okvis_FOUND AND WITH_CERES)
+
+IF(WITH_MSCKF_VIO)
+    FIND_PACKAGE(msckf_vio QUIET)
+    IF(msckf_vio_FOUND)
+       MESSAGE(STATUS "Found msckf_vio: ${msckf_vio_INCLUDE_DIRS}")
+    ENDIF(msckf_vio_FOUND)
+ENDIF(WITH_MSCKF_VIO)
+
+IF(WITH_VINS)
+    FIND_PACKAGE(vins QUIET)
+    IF(vins_FOUND)
+       MESSAGE(STATUS "Found vins: ${vins_INCLUDE_DIRS}")
+       IF(okvis_FOUND)
+           MESSAGE(WARNING "VINS and OKVIS will be both linked to project, make sure VINS has been built with against same Ceres version than OKVIS to avoid some crashes.")
+       ENDIF(okvis_FOUND)
+    ENDIF(vins_FOUND)
+ENDIF(WITH_VINS)
+
+IF(WITH_FASTCV)
+    FIND_PACKAGE(FastCV QUIET)
+    IF(FastCV_FOUND)
+       MESSAGE(STATUS "Found FastCV: ${FastCV_INCLUDE_DIRS}")
+    ENDIF(FastCV_FOUND)
+ENDIF(WITH_FASTCV)
+
 IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     FIND_PACKAGE(ORB_SLAM2 QUIET)
     IF(ORB_SLAM2_FOUND)
@@ -445,7 +600,29 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     ENDIF(ORB_SLAM2_FOUND)
 ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_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)
@@ -456,10 +633,10 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SL
       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 if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
       ENDIF()
    ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
+ENDIF()
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -495,9 +672,33 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 SET(CONF_DEPENDENCIES 
     ${ZLIB_LIBRARIES}
 )
+
+# OpenCV2 has nonfree if OPENCV_NONFREE_FOUND
+# OpenCV<=3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND
+# OpenCV>3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND and OPENCV_ENABLE_NONFREE is defined
 IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
    SET(NONFREE "//")
-ENDIF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
+ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2")
+  FIND_FILE(OpenCV_MODULES_HPP opencv2/opencv_modules.hpp 
+    PATHS ${OpenCV_INCLUDE_DIRS}
+    NO_DEFAULT_PATH)
+  FILE(READ ${OpenCV_MODULES_HPP} TMPTXT)
+  STRING(FIND "${TMPTXT}" "#define OPENCV_ENABLE_NONFREE" matchres)
+  IF(${matchres} EQUAL -1)
+    SET(NONFREE "//")
+  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()
@@ -511,6 +712,9 @@ IF(NOT GTSAM_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
 ENDIF()
+IF(NOT CERES_FOUND)
+   SET(CERES "//")
+ENDIF(NOT CERES_FOUND)
 IF(NOT WITH_TORO)
    SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -525,6 +729,12 @@ ENDIF()
 IF(NOT libpointmatcher_FOUND)
    SET(POINTMATCHER "//")
 ENDIF(NOT libpointmatcher_FOUND)
+IF(NOT FastCV_FOUND)
+   SET(FASTCV "//")
+ENDIF(NOT FastCV_FOUND)
+IF(NOT loam_velodyne_FOUND)
+   SET(LOAM "//")
+ENDIF(NOT loam_velodyne_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
 ELSE()
@@ -540,6 +750,11 @@ IF(NOT KinectSDK2_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES})
 ENDIF()
+IF(NOT k4a_FOUND)
+   SET(K4A "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${k4a_LIBRARIES})
+ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
 ELSE()
@@ -568,7 +783,15 @@ ENDIF()
 IF(NOT RealSenseSlam_FOUND)
    SET(REALSENSESLAM "//")
 ENDIF(NOT RealSenseSlam_FOUND)
-IF(NOT OCTOMAP_FOUND)
+IF(NOT realsense2_FOUND)
+   SET(REALSENSE2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) 
+ENDIF()
+IF(NOT mynteye_FOUND)
+   SET(MYNTEYE "//")
+ENDIF(NOT mynteye_FOUND)
+IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
@@ -583,6 +806,9 @@ IF(NOT open_chisel_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) 
 ENDIF()
+IF(NOT AliceVision_FOUND)
+   SET(ALICE_VISION "//")
+ENDIF(NOT AliceVision_FOUND)
 IF(NOT libfovis_FOUND)
    SET(FOVIS "//")
 ELSE()
@@ -603,11 +829,30 @@ IF(NOT okvis_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) 
 ENDIF()
+IF(NOT msckf_vio_FOUND)
+   SET(MSCKF_VIO "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${msckf_vio_LIBRARIES}) 
+ENDIF()
+IF(NOT vins_FOUND)
+   SET(VINS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
+ENDIF()
 IF(NOT ORB_SLAM2_FOUND)
    SET(ORB_SLAM2 "//")
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) 
 ENDIF()
+IF(NOT WITH_ORB_OCTREE)
+   SET(ORB_OCTREE "//")
+ENDIF()
+IF(NOT TORCH_FOUND)
+   SET(SUPERPOINT_TORCH "//")
+ENDIF()
+IF(NOT Python3_FOUND)
+   SET(PYMATCHER "//")
+ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
@@ -617,10 +862,10 @@ ENDIF()
 IF(VTK_USE_QVTK)
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) 
 ENDIF(VTK_USE_QVTK)
+IF(NOT WITH_MADGWICK)
+   SET(MADGWICK "//")
+ENDIF()
 
-IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
-   SET(OPENCV3 "//")
-ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
 
 ADD_SUBDIRECTORY( utilite )
@@ -705,7 +950,9 @@ install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PR
 #######################
 # CPACK (Packaging)
 #######################
-INCLUDE(InstallRequiredSystemLibraries)
+IF(BUILD_AS_BUNDLE)
+  INCLUDE(InstallRequiredSystemLibraries)
+ENDIF(BUILD_AS_BUNDLE)
 
 SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}")
 SET(CPACK_PACKAGE_VENDOR "${PROJECT_NAME} project")
@@ -740,7 +987,11 @@ IF(WIN32)
   ELSE() 
     SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES") 
   ENDIF()
-  SET(CPACK_GENERATOR "ZIP;NSIS")
+  IF(BUILD_AS_BUNDLE)
+    SET(CPACK_GENERATOR "ZIP;NSIS")
+  ELSE()
+    SET(CPACK_GENERATOR "ZIP")
+  ENDIF()
   SET(CPACK_SOURCE_GENERATOR "ZIP")
   SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}")
   SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/src/${PROJECT_NAME}.ico")
@@ -795,28 +1046,188 @@ IF(NOT WIN32)
     # see comment above for the BUILD_SHARED_LIBS option on Windows
     MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
 ENDIF(NOT WIN32)
-IF(APPLE)
+IF(APPLE OR WIN32)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
-ENDIF(APPLE)
+ENDIF(APPLE OR WIN32)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
+MESSAGE(STATUS "  FLANN_KDTREE_MEM_OPT = ${FLANN_KDTREE_MEM_OPT}")
 MESSAGE(STATUS "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")
+MESSAGE(STATUS "  PCL_VERSION = ${PCL_VERSION}")
+IF(PCL_COMPILE_OPTIONS)
+MESSAGE(STATUS "  PCL_COMPILE_OPTIONS = ${PCL_COMPILE_OPTIONS}")
+ENDIF(PCL_COMPILE_OPTIONS)
 
+MESSAGE(STATUS "")
+MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
     IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      IF(NONFREE STREQUAL "//")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+      ELSE()
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
+      ENDIF()
     ELSE()
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
     ENDIF()
   ENDIF()
 ENDIF(OpenCV_FOUND)
 
+IF(QT4_FOUND)
+MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
+MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
+ELSEIF(Qt5_FOUND)
+MESSAGE(STATUS "  With Qt ${Qt5_VERSION}            = YES (License: Open Source or Commercial)")
+MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
+
+ELSEIF(NOT WITH_QT)
+MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
+ENDIF()
+
+IF(Sqlite3_FOUND)
+MESSAGE(STATUS "  With external SQLite3     = YES (License: Public Domain)")
+ELSE()
+MESSAGE(STATUS "  With external SQLite3     = NO (sqlite3 not found, internal version is used for convenience)")
+ENDIF()
+
+IF(WITH_ORB_OCTREE)
+MESSAGE(STATUS "  With ORB OcTree           = YES (License: GPLv3)")
+ELSE()
+MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
+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)")
+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)")
+ELSE()
+MESSAGE(STATUS "  With Python3              = NO (python3 not found)")
+ENDIF()
+
+IF(WITH_MADGWICK)
+MESSAGE(STATUS "  With Madgwick             = YES (License: GPL)")
+ELSE()
+MESSAGE(STATUS "  With Madgwick             = NO (WITH_MADGWICK=OFF)")
+ENDIF()
+
+IF(FastCV_FOUND)
+MESSAGE(STATUS "  With FastCV               = YES (License: Apache v2)")
+ELSEIF(NOT WITH_FASTCV)
+MESSAGE(STATUS "  With FastCV               = NO (WITH_FASTCV=OFF)")
+ELSE()
+MESSAGE(STATUS "  With FastCV               = NO (FastCV not found)")
+ENDIF()
+
+MESSAGE(STATUS "")
+MESSAGE(STATUS " Solvers:")
+IF(WITH_TORO)
+MESSAGE(STATUS "  With TORO                 = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])")
+ELSE()
+MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
+ENDIF()
+
+IF(G2O_FOUND)
+MESSAGE(STATUS " *With g2o                  = YES (License: BSD)")
+ELSEIF(NOT WITH_G2O)
+MESSAGE(STATUS " *With g2o                  = NO (WITH_G2O=OFF)")
+ELSE()
+MESSAGE(STATUS " *With g2o                  = NO (g2o not found)")
+ENDIF()
+
+IF(GTSAM_FOUND)
+MESSAGE(STATUS " *With GTSAM                = YES (License: BSD)")
+ELSEIF(NOT WITH_GTSAM)
+MESSAGE(STATUS " *With GTSAM                = NO (WITH_GTSAM=OFF)")
+ELSE()
+MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
+ENDIF()
+
+IF(CERES_FOUND)
+MESSAGE(STATUS " *With Ceres                = YES (License: BSD)")
+ELSEIF(NOT WITH_CERES)
+MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
+ELSE()
+MESSAGE(STATUS " *With Ceres                = NO (Ceres not found)")
+ENDIF()
+
+IF(G2O_FOUND OR GTSAM_FOUND)
+IF(WITH_VERTIGO)
+MESSAGE(STATUS "  With VERTIGO              = YES (License: GPLv3)")
+ELSE()
+MESSAGE(STATUS "  With VERTIGO              = NO (WITH_VERTIGO=OFF)")
+ENDIF()
+ELSE()
+MESSAGE(STATUS "  With VERTIGO              = NO (GTSAM or g2o required)")
+ENDIF()
+
+IF(cvsba_FOUND)
+MESSAGE(STATUS "  With cvsba                = YES (License: GPLv2)")
+ELSEIF(NOT WITH_CVSBA)
+MESSAGE(STATUS "  With cvsba                = NO (WITH_CVSBA=OFF)")
+ELSE()
+MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
+ENDIF()
+
+IF(libpointmatcher_FOUND)
+MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
+ELSEIF(NOT WITH_POINTMATCHER)
+MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
+ELSE()
+MESSAGE(STATUS " *With libpointmatcher      = NO (libpointmatcher not found)")
+ENDIF()
+
+MESSAGE(STATUS "")
+MESSAGE(STATUS " Reconstruction Approaches:")
+IF(octomap_FOUND)
+MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+ELSEIF(NOT WITH_OCTOMAP)
+MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+ENDIF()
+
+IF(CPUTSDF_FOUND)
+MESSAGE(STATUS "  With CPUTSDF              = YES (License: BSD)")
+ELSEIF(NOT WITH_CPUTSDF)
+MESSAGE(STATUS "  With CPUTSDF              = NO (WITH_CPUTSDF=OFF)")
+ELSE()
+MESSAGE(STATUS "  With CPUTSDF              = NO (CPUTSDF not found)")
+ENDIF()
+
+IF(open_chisel_FOUND)
+MESSAGE(STATUS "  With OpenChisel           = YES (License: ???)")
+ELSEIF(NOT WITH_OPENCHISEL)
+MESSAGE(STATUS "  With OpenChisel           = NO (WITH_OPENCHISEL=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
+ENDIF()
+
+IF(AliceVision_FOUND)
+MESSAGE(STATUS "  With AliceVision          = YES (License: MPLv2)")
+ELSEIF(NOT WITH_ALICE_VISION)
+MESSAGE(STATUS "  With AliceVision          = NO (WITH_ALICE_VISION=OFF)")
+ELSE()
+MESSAGE(STATUS "  With AliceVision          = NO (AliceVision not found)")
+ENDIF()
+
+MESSAGE(STATUS "")
+MESSAGE(STATUS " Camera Drivers:")
 IF(Freenect_FOUND)
 MESSAGE(STATUS "  With Freenect             = YES (License: Apache v2 and/or GPLv2)")
 ELSEIF(NOT WITH_FREENECT)
@@ -849,6 +1260,14 @@ ELSE()
 MESSAGE(STATUS "  With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)")
 ENDIF()
 
+IF(k4a_FOUND)
+MESSAGE(STATUS "  With Kinect for Azure     = YES (License: MIT)")
+ELSEIF(NOT WITH_K4A)
+MESSAGE(STATUS "  With Kinect for Azure     = NO (WITH_K4A=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Kinect for Azure     = NO (Kinect for Azure SDK not found)")
+ENDIF()
+
 IF(DC1394_FOUND)
 MESSAGE(STATUS "  With dc1394               = YES (License: LGPL)")
 ELSEIF(NOT WITH_DC1394)
@@ -865,64 +1284,12 @@ ELSE()
 MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
 ENDIF()
 
-IF(WITH_TORO)
-MESSAGE(STATUS "  With TORO                 = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])")
-ELSE()
-MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
-ENDIF()
-
-IF(G2O_FOUND)
-MESSAGE(STATUS "  With g2o                  = YES (License: BSD)")
-ELSEIF(NOT WITH_G2O)
-MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
-ELSE()
-MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
-ENDIF()
-
-IF(GTSAM_FOUND)
-MESSAGE(STATUS "  With GTSAM                = YES (License: BSD)")
-ELSEIF(NOT WITH_GTSAM)
-MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
-ELSE()
-MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
-ENDIF()
-
-IF(G2O_FOUND OR GTSAM_FOUND)
-IF(WITH_VERTIGO)
-MESSAGE(STATUS "  With VERTIGO              = YES (License: GPLv3)")
-ELSE()
-MESSAGE(STATUS "  With VERTIGO              = NO (WITH_VERTIGO=OFF)")
-ENDIF()
-ELSE()
-MESSAGE(STATUS "  With VERTIGO              = NO (GTSAM or g2o required)")
-ENDIF()
-
-IF(cvsba_FOUND)
-MESSAGE(STATUS "  With cvsba                = YES (License: GPLv2)")
-ELSEIF(NOT WITH_CVSBA)
-MESSAGE(STATUS "  With cvsba                = NO (WITH_CVSBA=OFF)")
-ELSE()
-MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
-ENDIF()
-
-IF(libpointmatcher_FOUND)
-MESSAGE(STATUS "  With libpointmatcher      = YES (License: BSD)")
-ELSEIF(NOT WITH_POINTMATCHER)
-MESSAGE(STATUS "  With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
-ELSE()
-MESSAGE(STATUS "  With libpointmatcher      = NO (libpointmatcher not found)")
-ENDIF()
-
-IF(ZED_FOUND)
-IF(CUDA_FOUND)
-MESSAGE(STATUS "  With ZED                  = YES (With CUDA)")
-ELSE()
-MESSAGE(STATUS "  With ZED                  = YES (Without CUDA)")
-ENDIF()
+IF(ZED_FOUND AND CUDA_FOUND)
+MESSAGE(STATUS "  With ZED                  = YES")
 ELSEIF(NOT WITH_ZED)
 MESSAGE(STATUS "  With ZED                  = NO (WITH_ZED=OFF)")
 ELSE()
-MESSAGE(STATUS "  With ZED                  = NO (ZED sdk not found)")
+MESSAGE(STATUS "  With ZED                  = NO (ZED sdk and/or cuda not found)")
 ENDIF()
 
 IF(RealSense_FOUND)
@@ -940,28 +1307,30 @@ ELSE()
 MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
 ENDIF()
 
-IF(OCTOMAP_FOUND)
-MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
-ELSEIF(NOT WITH_OCTOMAP)
-MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+IF(realsense2_FOUND)
+MESSAGE(STATUS "  With RealSense2           = YES (License: Apache-2)")
+ELSEIF(NOT WITH_REALSENSE2)
+MESSAGE(STATUS "  With RealSense2           = NO (WITH_REALSENSE2=OFF)")
 ELSE()
-MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+MESSAGE(STATUS "  With RealSense2           = NO (librealsense2 not found)")
 ENDIF()
 
-IF(CPUTSDF_FOUND)
-MESSAGE(STATUS "  With CPUTSDF              = YES (License: BSD)")
-ELSEIF(NOT WITH_CPUTSDF)
-MESSAGE(STATUS "  With CPUTSDF              = NO (WITH_CPUTSDF=OFF)")
+IF(mynteye_FOUND)
+MESSAGE(STATUS "  With MyntEyeS             = YES (License: Apache-2)")
+ELSEIF(NOT WITH_MYNTEYE)
+MESSAGE(STATUS "  With MyntEyeS             = NO (WITH_MYNTEYE=OFF)")
 ELSE()
-MESSAGE(STATUS "  With CPUTSDF              = NO (CPUTSDF not found)")
+MESSAGE(STATUS "  With MyntEyeS             = NO (mynteye s sdk not found)")
 ENDIF()
 
-IF(open_chisel_FOUND)
-MESSAGE(STATUS "  With OpenChisel           = YES (License: ???)")
-ELSEIF(NOT WITH_OPENCHISEL)
-MESSAGE(STATUS "  With OpenChisel           = NO (WITH_OPENCHISEL=OFF)")
+MESSAGE(STATUS "")
+MESSAGE(STATUS " Odometry Approaches:")
+IF(loam_velodyne_FOUND)
+MESSAGE(STATUS "  With loam_velodyne        = YES (License: BSD)")
+ELSEIF(NOT WITH_LOAM)
+MESSAGE(STATUS "  With loam_velodyne        = NO (WITH_LOAM=OFF)")
 ELSE()
-MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
+MESSAGE(STATUS "  With loam_velodyne        = NO (loam_velodyne not found)")
 ENDIF()
 
 IF(libfovis_FOUND)
@@ -990,12 +1359,28 @@ ENDIF()
 
 IF(okvis_FOUND)
 MESSAGE(STATUS "  With okvis                = YES (License: BSD)")
-ELSEIF(NOT WITH_DVO)
+ELSEIF(NOT WITH_OKVIS)
 MESSAGE(STATUS "  With okvis                = NO (WITH_OKVIS=OFF)")
 ELSE()
 MESSAGE(STATUS "  With okvis                = NO (okvis not found)")
 ENDIF()
 
+IF(msckf_vio_FOUND)
+MESSAGE(STATUS "  With msckf_vio            = YES (License: Penn Software License)")
+ELSEIF(NOT WITH_MSCKF_VIO)
+MESSAGE(STATUS "  With msckf_vio            = NO (WITH_MSCKF_VIO=OFF)")
+ELSE()
+MESSAGE(STATUS "  With msckf_vio            = NO (msckf_vio not found)")
+ENDIF()
+
+IF(vins_FOUND)
+MESSAGE(STATUS "  With VINS-Fusion          = YES (License: GPLv3)")
+ELSEIF(NOT WITH_VINS)
+MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS=OFF)")
+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)
@@ -1006,23 +1391,15 @@ ELSE()
 MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
 ENDIF()
 
-IF(QT4_FOUND)
-MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
-ELSEIF(Qt5_FOUND)
-MESSAGE(STATUS "  With Qt5                  = YES (License: Open Source or Commercial)")
-ELSEIF(NOT WITH_QT)
-MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
-ELSE()
-MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
-ENDIF()
-
+MESSAGE(STATUS "Show all options with: cmake -LA | grep WITH_")
 MESSAGE(STATUS "--------------------------------------------")
 
-IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
+IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
 MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of these options:
        g2o (https://github.com/RainerKuemmerle/g2o)
        GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
+       Ceres (http://ceres-solver.org)
        set -DWITH_TORO=ON")
-ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
+ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
 
 # vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :

README.md

@@ -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.16.3-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.18.0-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
@@ -18,3 +18,10 @@ RTAB-Map library and standalone application.
 For more information, visit the [RTAB-Map's home page](http://introlab.github.io/rtabmap) or the [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
 
 To use RTAB-Map under ROS, visit the [rtabmap](http://wiki.ros.org/rtabmap) page on the ROS wiki.
+
+### Acknowledgements
+This project is supported by [IntRoLab - Intelligent / Interactive / Integrated / Interdisciplinary Robot Lab](https://introlab.3it.usherbrooke.ca/), Sherbrooke, Québec, Canada.
+
+<a href="https://introlab.3it.usherbrooke.ca/">
+<img src="https://github.com/introlab/16SoundsUSB/blob/master/images/IntRoLab.png" alt="IntRoLab" height="100">
+</a>

RTABMapConfig.cmake.in

@@ -74,7 +74,7 @@ if(@CONF_WITH_GUI@ AND ${WITH_GUI})
    
    set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
 elseif(${WITH_GUI})
-   MESSAGE(ERROR "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
+   MESSAGE(WARNING "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
 endif()
 
 # Dependencies

Version.h.in

@@ -38,31 +38,45 @@ 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
 @GTSAM@#define RTABMAP_GTSAM
+@CERES@#define RTABMAP_CERES
 @VERTIGO@#define RTABMAP_VERTIGO
-@OPENCV3@#define RTABMAP_OPENCV3
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
 @K4W2@#define RTABMAP_K4W2
+@K4A@#define RTABMAP_K4A
 @CVSBA@#define RTABMAP_CVSBA
 @POINTMATCHER@#define RTABMAP_POINTMATCHER
+@FASTCV@#define RTABMAP_FASTCV
+@LOAM@#define RTABMAP_LOAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
 @REALSENSE@#define RTABMAP_REALSENSE
 @REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
+@MYNTEYE@#define RTABMAP_MYNTEYE
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
+@ALICE_VISION@#define RTABMAP_ALICE_VISION
 @OPENCHISEL@#define RTABMAP_OPENCHISEL
 @FOVIS@#define RTABMAP_FOVIS
 @VISO2@#define RTABMAP_VISO2
 @DVO@#define RTABMAP_DVO
 @OKVIS@#define RTABMAP_OKVIS
+@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
+@VINS@#define RTABMAP_VINS
 @ORB_SLAM2@#define RTABMAP_ORB_SLAM2
+@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
+@SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH
+@PYMATCHER@#define RTABMAP_PYMATCHER
+@MADGWICK@#define RTABMAP_MADGWICK
+
 
 #endif /* VERSION_H_ */
 

app/android/.gitignore

@@ -1 +1,2 @@
 /bin/
+/gen/

app/android/AndroidManifest.xml.in

@@ -2,22 +2,21 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="70"
+        android:versionCode="72"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
-    <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
-    <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
     <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-permission android:name="android.permission.ACCESS_WIFI_STATE" />
     <uses-feature android:name="android.hardware.location.gps" />
     <uses-feature android:glEsVersion="0x00020000" />
 
-    <!-- This is the platform API where NativeActivity was introduced. -->
-    <uses-sdk android:minSdkVersion="19" />
+    <!-- This is the platform API where depth16 support in android was introduced. -->
+    <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
 
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 
@@ -25,7 +24,9 @@
         android:icon="@drawable/ic_launcher"
         android:debuggable="@ANDROID_DEBUGGABLE@">
         
-        <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
+        <uses-library android:name="com.projecttango.libtango_device2" android:required="false" />
+        <meta-data android:name="com.google.ar.core" android:value="optional" />
+        <meta-data android:name="com.huawei.ar.engine" android:value="optional" />
 
         <!-- Our activity is the built-in NativeActivity framework class.
              This will take care of integrating with our NDK code. -->
@@ -46,6 +47,27 @@
 
         <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>
         <activity android:name="SketchfabActivity" android:label="@string/sketchfab" android:screenOrientation="fullSensor"/>
+    
+        <meta-data
+            android:name="com.google.ar.core.min_apk_version"
+            android:value="191106000" /> <!-- This activity is critical for installing ARCore when it is not already present. -->
+        <activity
+            android:name="com.google.ar.core.InstallActivity"
+            android:configChanges="keyboardHidden|orientation|screenSize"
+            android:excludeFromRecents="true"
+            android:exported="false"
+            android:launchMode="singleTop"
+            android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
+            
+        <provider
+            android:name="android.support.v4.content.FileProvider"
+            android:authorities="com.introlab.rtabmap.provider"
+            android:exported="false"
+            android:grantUriPermissions="true">
+            <meta-data
+                android:name="android.support.FILE_PROVIDER_PATHS"
+                android:resource="@xml/provider_paths"/>
+        </provider>
     </application>
 
 </manifest>

app/android/CMakeLists.txt

@@ -1,10 +1,95 @@
 
+option(WITH_TANGO         "Include Tango support"                ON)
+option(WITH_ARCORE        "Include ARCore support"               ON)
+option(WITH_ARENGINE      "Include AREngine support"             ON)
 option(DISABLE_LOG        "Disable Android logging (should be true in release)"    ON)
+option(DEPTH_TEST         "Enable depth test on ARCore"    OFF)
+
+# Google Tango needs access to system shared 
+# libraries (e.g. libbinder.so) that are not accessible 
+# with android >=24
+IF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    FIND_PACKAGE(Tango QUIET)
+    IF(Tango_FOUND)
+       MESSAGE(STATUS "Found Tango: ${Tango_INCLUDE_DIRS}")
+    ENDIF(Tango_FOUND)
+ENDIF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+
+IF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+    FIND_PACKAGE(ARCore QUIET)
+    IF(ARCore_FOUND)
+       MESSAGE(STATUS "Found ARCore: ${ARCore_INCLUDE_DIRS}")
+    ENDIF(ARCore_FOUND)
+ENDIF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+
+IF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    FIND_PACKAGE(AREngine QUIET)
+    IF(AREngine_FOUND)
+       MESSAGE(STATUS "Found AREngine: ${AREngine_INCLUDE_DIRS}")
+    ENDIF(AREngine_FOUND)
+ENDIF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+
+IF(NOT Tango_FOUND)
+   SET(TANGO "//")
+ENDIF(NOT Tango_FOUND)
+IF(NOT ARCore_FOUND)
+   SET(ARCORE "//")
+ENDIF(NOT ARCore_FOUND)
+IF(NOT AREngine_FOUND)
+   SET(ARENGINE "//")
+ENDIF(NOT AREngine_FOUND)
+
+CONFIGURE_FILE(CameraAvailability.h.in ${CMAKE_CURRENT_SOURCE_DIR}/jni/CameraAvailability.h)
+
+
 IF(DISABLE_LOG)
   ADD_DEFINITIONS(-DDISABLE_LOG)
 ENDIF(DISABLE_LOG)
+IF(DEPTH_TEST)
+  ADD_DEFINITIONS(-DDEPTH_TEST)
+ENDIF(DEPTH_TEST)
+
+MESSAGE(STATUS "--------------------------------------------")
+MESSAGE(STATUS "Android build info:")
+MESSAGE(STATUS "  DISABLE_LOG    = ${DISABLE_LOG}")
+MESSAGE(STATUS "  DEPTH_TEST     = ${DEPTH_TEST}")
+IF(Tango_FOUND)
+  MESSAGE(STATUS "  With Tango     = YES")
+ELSEIF(NOT WITH_TANGO)
+  MESSAGE(STATUS "  With Tango     = NO (WITH_TANGO=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    MESSAGE(STATUS "  With Tango     = NO (ANDROID_NATIVE_API_LEVEL should be <= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With Tango     = NO (tango not found)")
+  ENDIF()
+ENDIF()
+IF(ARCore_FOUND)
+  MESSAGE(STATUS "  With ARCore    = YES")
+ELSEIF(NOT WITH_ARCORE)
+  MESSAGE(STATUS "  With ARCore    = NO (WITH_ARCORE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 23)
+    MESSAGE(STATUS "  With ARCore    = NO (ANDROID_NATIVE_API_LEVEL should be >= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With ARCore    = NO (ARCore not found)")
+  ENDIF()
+ENDIF()
+IF(AREngine_FOUND)
+  MESSAGE(STATUS "  With AREngine  = YES")
+ELSEIF(NOT WITH_ARENGINE)
+  MESSAGE(STATUS "  With AREngine  = NO (WITH_ARENGINE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    MESSAGE(STATUS "  With AREngine  = NO (ANDROID_NATIVE_API_LEVEL should be >= 24)")
+  ELSE()
+    MESSAGE(STATUS "  With AREngine  = NO (AREngine not found)")
+  ENDIF()
+ENDIF()
+MESSAGE(STATUS "  ANDROID_NATIVE_API_LEVEL       = ${ANDROID_NATIVE_API_LEVEL}")
+MESSAGE(STATUS "  ANDROID_COMPILER_FLAGS_RELEASE = ${ANDROID_COMPILER_FLAGS_RELEASE}")
+MESSAGE(STATUS "  ANDROID_TOOLCHAIN_PREFIX       = ${ANDROID_TOOLCHAIN_PREFIX}")
 
-MESSAGE(STATUS "DISABLE_LOG = ${DISABLE_LOG}")
 
 IF(DISABLE_LOG)
    SET(ANDROID_DEBUGGABLE false)
@@ -29,7 +114,8 @@ endif()
 
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
-  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml")
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
+  @ONLY)
   
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"

app/android/CameraAvailability.h.in

@@ -0,0 +1,39 @@
+/*
+Copyright (c) 2010-2014, 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 UNIVERTY DE SHERBROOKE 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 CAMERAAVAILABILITY_H_
+#define CAMERAAVAILABILITY_H_
+
+// This is auto-generated!
+
+@TANGO@#define RTABMAP_TANGO
+@ARCORE@#define RTABMAP_ARCORE
+@ARENGINE@#define RTABMAP_ARENGINE
+
+
+#endif /* CAMERAAVAILABILITY_H_ */
+

app/android/custom_rules.xml

@@ -6,8 +6,8 @@
 		<copy todir="${native.libs.dir}/${android.abi}">
 			<fileset dir="${srcdir}/jni/third-party/lib" includes="*.so"/>
 		</copy>
-                <copy todir="${native.libs.dir}">
-                        <fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
-                </copy>
+		<copy todir="${native.libs.dir}">
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
+		</copy>
 	</target>
 </project>

app/android/info.txt.in

@@ -1,7 +1,7 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
 Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
-Copyright 2016-2017<br>
+Copyright 2016-2020<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
 <b>http://introlab.github.io/rtabmap</b><br><br>
 

app/android/jni/.gitignore

@@ -0,0 +1 @@
+CameraAvailability.h

app/android/jni/CMakeLists.txt

@@ -1,6 +1,4 @@
 
-find_package(Tango REQUIRED)
-
 SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}
     ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
@@ -9,23 +7,17 @@ SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/utilite/include
     ${OpenCV_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
-    ${Tango_INCLUDE_DIRS}
     "${ANDROID_NDK}/platforms/android-${ANDROID_NATIVE_API_LEVEL}/arch-${ANDROID_ARCH_NAME}/usr/include"
 )
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES} 
 	${PCL_LIBRARIES}
-	${Tango_LIBRARIES}
 )
 
-add_definitions(${PCL_DEFINITIONS})
-
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
 set(sources
   jni_interface.cpp
-  CameraTango.cpp
+  CameraMobile.cpp
   RTABMapApp.cpp
   scene.cpp
   point_cloud_drawable.cpp
@@ -44,6 +36,82 @@ set(sources
   tango-gl/util.cpp
 )
 
+IF(OPENMP_FOUND)
+file(COPY ${OpenMP_CXX_LIBRARIES}
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+ENDIF(OPENMP_FOUND)
+
+IF(Tango_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraTango.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${Tango_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${Tango_LIBRARIES}
+  )
+  
+  file(COPY ${Tango_support_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}) 
+ENDIF(Tango_FOUND)
+
+IF(ARCore_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraARCore.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${ARCore_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${ARCore_LIBRARIES}
+  )
+ 
+  file(COPY ${ARCore_c_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${ARCore_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(ARCore_FOUND)
+
+IF(AREngine_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraAREngine.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${AREngine_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${AREngine_LIBRARIES}
+    camera2ndk
+    mediandk
+  )
+ 
+  file(COPY ${AREngine_impl_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_ndk_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(AREngine_FOUND)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
 add_library(NativeRTABMap SHARED ${sources})        
 target_link_libraries(NativeRTABMap ${LIBRARIES}
   android
@@ -58,3 +126,10 @@ set_target_properties(NativeRTABMap PROPERTIES
          LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
          LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
          LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}")
+         
+IF(ANDROID_NATIVE_API_LEVEL GREATER 22)
+add_custom_command(TARGET NativeRTABMap POST_BUILD
+    COMMAND "${ANDROID_TOOLCHAIN_PREFIX}strip" -g -S -d --strip-debug --verbose
+    "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}/libNativeRTABMap.so"
+    COMMENT "Strip debug symbols done on final binary.")
+ENDIF(ANDROID_NATIVE_API_LEVEL GREATER 22)

app/android/jni/CameraARCore.cpp

@@ -0,0 +1,702 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CameraARCore.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+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):
+	CameraMobile(smoothing),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false)
+{
+	glGenTextures(1, &textureId_);
+}
+
+CameraARCore::~CameraARCore() {
+	// Disconnect ARCore service
+	close();
+
+	glDeleteTextures(1, &textureId_);
+}
+
+
+struct CameraConfig {
+  int32_t width = 0;
+  int32_t height = 0;
+  std::string config_label;
+  ArCameraConfig* config = nullptr;
+};
+
+void getCameraConfigLowestAndHighestResolutions(
+		std::vector<CameraConfig> & camera_configs,
+    CameraConfig** lowest_resolution_config,
+    CameraConfig** highest_resolution_config) {
+  if (camera_configs.empty()) {
+    return;
+  }
+
+  int low_resolution_config_idx = 0;
+  int high_resolution_config_idx = 0;
+  int32_t smallest_height = camera_configs[0].height;
+  int32_t largest_height = camera_configs[0].height;
+
+  for (int i = 1; i < camera_configs.size(); ++i) {
+    int32_t image_height = camera_configs[i].height;
+    if (image_height < smallest_height) {
+      smallest_height = image_height;
+      low_resolution_config_idx = i;
+    } else if (image_height > largest_height) {
+      largest_height = image_height;
+      high_resolution_config_idx = i;
+    }
+  }
+
+  if (low_resolution_config_idx == high_resolution_config_idx) {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+  } else {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+    *highest_resolution_config = &camera_configs[high_resolution_config_idx];
+  }
+}
+
+void copyCameraConfig(
+    const ArSession* ar_session, const ArCameraConfigList* all_configs,
+    int index, int num_configs, CameraConfig* camera_config) {
+  if (camera_config != nullptr && index >= 0 && index < num_configs) {
+    ArCameraConfig_create(ar_session, &camera_config->config);
+    ArCameraConfigList_getItem(ar_session, all_configs, index,
+                               camera_config->config);
+    ArCameraConfig_getImageDimensions(ar_session, camera_config->config,
+                                      &camera_config->width,
+                                      &camera_config->height);
+    camera_config->config_label = "(" + std::to_string(camera_config->width) +
+                                  "x" + std::to_string(camera_config->height) +
+                                  ")";
+  }
+}
+
+void destroyCameraConfigs(std::vector<CameraConfig> & camera_configs) {
+  for (int i = 0; i < camera_configs.size(); ++i) {
+    if (camera_configs[i].config != nullptr) {
+      ArCameraConfig_destroy(camera_configs[i].config);
+    }
+  }
+}
+
+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;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	ArCoreApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case AR_INSTALL_STATUS_INSTALLED:
+			break;
+		case AR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
+	UASSERT(arSession_);
+
+	ArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
+	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
+
+	ArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	ArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_);
+	UASSERT(arCameraIntrinsics_);
+
+	ArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	ArCameraConfigList* all_camera_configs = nullptr;
+	int32_t num_configs = 0;
+	ArCameraConfigList_create(arSession_, &all_camera_configs);
+	// Create filter first to get both 30 and 60 fps.
+	ArCameraConfigFilter* camera_config_filter = nullptr;
+	ArCameraConfigFilter_create(arSession_, &camera_config_filter);
+	ArCameraConfigFilter_setTargetFps(arSession_, camera_config_filter, AR_CAMERA_CONFIG_TARGET_FPS_30 | AR_CAMERA_CONFIG_TARGET_FPS_60);
+	ArSession_getSupportedCameraConfigsWithFilter(arSession_, camera_config_filter, all_camera_configs);
+	ArCameraConfigList_getSize(arSession_, all_camera_configs, &num_configs);
+
+	if (num_configs < 1) {
+		UERROR("No camera config found");
+		close();
+		return false;
+	}
+
+	std::vector<CameraConfig> camera_configs;
+	CameraConfig* cpu_low_resolution_camera_config_ptr = nullptr;
+	CameraConfig* cpu_high_resolution_camera_config_ptr = nullptr;
+	camera_configs.resize(num_configs);
+	for (int i = 0; i < num_configs; ++i) {
+		copyCameraConfig(arSession_, all_camera_configs, i, num_configs,
+						 &camera_configs[i]);
+	}
+	// Determine the highest and lowest CPU resolutions.
+	cpu_low_resolution_camera_config_ptr = nullptr;
+	cpu_high_resolution_camera_config_ptr = nullptr;
+	getCameraConfigLowestAndHighestResolutions(
+			camera_configs,
+			&cpu_low_resolution_camera_config_ptr,
+			&cpu_high_resolution_camera_config_ptr);
+
+	// Cleanup the list obtained as it is safe to destroy the list as camera
+	// config instances were explicitly created and copied. Refer to the
+	// previous comment.
+	ArCameraConfigList_destroy(all_camera_configs);
+	ArSession_setCameraConfig(arSession_, cpu_low_resolution_camera_config_ptr->config);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	ArConfig_setUpdateMode(arSession_, arConfig_, AR_UPDATE_MODE_BLOCKING);
+
+	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!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraARCore::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if(arSession_!= nullptr)
+	{
+		ArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		ArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		ArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		ArCameraIntrinsics_destroy(arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		ArPose_destroy(arPose_);
+	}
+	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();
+}
+
+LaserScan CameraARCore::scanFromPointCloudData(
+		const float * pointCloudData,
+		int points,
+		const Transform & pose,
+		const CameraModel & model,
+		const cv::Mat & rgb,
+		std::vector<cv::KeyPoint> * kpts,
+		std::vector<cv::Point3f> * kpts3D)
+{
+	if(pointCloudData && points>0)
+	{
+		cv::Mat scanData(1, points, CV_32FC4);
+		float * ptr = scanData.ptr<float>();
+		for(unsigned int i=0;i<points; ++i)
+		{
+			cv::Point3f pt(pointCloudData[i*4], pointCloudData[i*4 + 1], pointCloudData[i*4 + 2]);
+			pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
+			ptr[i*4] = pt.x;
+			ptr[i*4 + 1] = pt.y;
+			ptr[i*4 + 2] = pt.z;
+
+			//get color from rgb image
+			cv::Point3f org= pt;
+			pt = util3d::transformPoint(pt, opticalRotationInv);
+			int u,v;
+			model.reproject(pt.x, pt.y, pt.z, u, v);
+			unsigned char r=255,g=255,b=255;
+			if(model.inFrame(u, v))
+			{
+				b=rgb.at<cv::Vec3b>(v,u).val[0];
+				g=rgb.at<cv::Vec3b>(v,u).val[1];
+				r=rgb.at<cv::Vec3b>(v,u).val[2];
+				if(kpts)
+					kpts->push_back(cv::KeyPoint(u,v,3));
+				if(kpts3D)
+					kpts3D->push_back(org);
+			}
+			*(int*)&ptr[i*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+
+			//confidence
+			//*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
+
+		}
+		return LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
+	}
+	return LaserScan();
+}
+
+SensorData CameraARCore::captureImage(CameraInfo * info)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	// 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;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArTrackingState camera_tracking_state;
+	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		ArCamera_getPose(arSession_, ar_camera, arPose_);
+		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+
+		// Get calibration parameters
+		float fx,fy, cx, cy;
+		int32_t width, height;
+		ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &width, &height);
+#ifndef DISABLE_LOG
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
+#endif
+
+		if(fx > 0 && fy > 0 && width > 0 && height > 0 && cx > 0 && cy > 0)
+		{
+			model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(width, height));
+
+			ArPointCloud * pointCloud = nullptr;
+			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
+
+			ArImage * image = nullptr;
+			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			if(status == AR_SUCCESS)
+			{
+				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);
+					for(int i=0;i<num_planes; ++i)
+					{
+						int32_t pixel_stride;
+						int32_t row_stride;
+						ArImage_getPlanePixelStride(arSession_, image, i, &pixel_stride);
+						ArImage_getPlaneRowStride(arSession_, image, i, &row_stride);
+						LOGI("Plane %d/%d: pixel stride=%d, row stride=%d", i+1, num_planes, pixel_stride, row_stride);
+					}
+#endif
+					const uint8_t * plane_data;
+					const uint8_t * plane_uv_data;
+					int32_t data_length;
+					ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
+					int32_t uv_data_length;
+					ArImage_getPlaneData(arSession_, image, 2, &plane_uv_data, &uv_data_length);
+
+					if(plane_data != nullptr && data_length == height*width)
+					{
+						double stamp = double(timestamp_ns)/10e8;
+#ifndef DISABLE_LOG
+						LOGI("data_length=%d stamp=%f", data_length, stamp);
+#endif
+						cv::Mat rgb;
+						if((long)plane_uv_data-(long)plane_data != data_length)
+						{
+							// The uv-plane is not concatenated to y plane in memory, so concatenate them
+							cv::Mat yuv(height+height/2, width, CV_8UC1);
+							memcpy(yuv.data, plane_data, data_length);
+							memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
+							cv::cvtColor(yuv, rgb, CV_YUV2BGR_NV21);
+						}
+						else
+						{
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
+						}
+
+						std::vector<cv::KeyPoint> kpts;
+						std::vector<cv::Point3f> kpts3;
+						LaserScan scan;
+						if(pointCloud)
+						{
+							int32_t points = 0;
+							ArPointCloud_getNumberOfPoints(arSession_, pointCloud, &points);
+							const float * pointCloudData = 0;
+							ArPointCloud_getData(arSession_, pointCloud, &pointCloudData);
+#ifndef DISABLE_LOG
+							LOGI("pointCloudData=%d size=%d", pointCloudData?1:0, points);
+#endif
+							if(pointCloudData && points>0)
+							{
+								scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3);
+							}
+						}
+						else
+						{
+							LOGI("pointCloud empty");
+						}
+
+						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+						data.setFeatures(kpts,  kpts3, cv::Mat());
+					}
+				}
+				else
+				{
+					LOGE("CameraARCore: cannot convert image format %d", format);
+				}
+			}
+			else
+			{
+				LOGE("CameraARCore: failed to get rgb image (status=%d)", (int)status);
+			}
+
+			ArImage_release(image);
+			ArPointCloud_release(pointCloud);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+
+	if(pose.isNull())
+	{
+		LOGE("CameraARCore: Pose is null");
+	}
+	else
+	{
+		this->poseReceived(pose);
+		info->odomPose = pose;
+	}
+	return data;
+
+}
+
+void CameraARCore::capturePoseOnly()
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArTrackingState camera_tracking_state;
+	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		ArCamera_getPose(arSession_, ar_camera, arPose_);
+		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		if(!pose.isNull())
+		{
+			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+			this->poseReceived(pose);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+}
+
+} /* namespace rtabmap */

app/android/jni/CameraARCore.h

@@ -0,0 +1,114 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAARCORE_H_
+#define CAMERAARCORE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+
+#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 {
+
+class CameraARCore : public CameraMobile {
+public:
+	static LaserScan scanFromPointCloudData(
+			const float * pointCloudData,
+			int points,
+			const Transform & pose,
+			const CameraModel & model,
+			const cv::Mat & rgb,
+			std::vector<cv::KeyPoint> * kpts = 0,
+			std::vector<cv::Point3f> * kpts3D = 0);
+
+public:
+	CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
+	virtual ~CameraARCore();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+
+#ifdef DEPTH_TEST
+	void imageCallback(AImageReader *reader);
+#endif // DEPTH_TEST
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly();
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	ArSession* arSession_ = nullptr;
+	ArConfig* arConfig_ = nullptr;
+	ArFrame* arFrame_ = nullptr;
+	ArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	ArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	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;
+
+	ACameraDevice_StateCallbacks deviceStateCallbacks_;
+	ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
+
+	ACameraManager* cameraManager_ = nullptr;
+	AImageReader* imageReader_ = nullptr;
+#endif // DEPTH_TEST
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARCORE_H_ */

app/android/jni/CameraAREngine.cpp

@@ -0,0 +1,351 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CameraAREngine.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+#include <media/NdkImage.h>
+
+namespace rtabmap {
+
+
+//////////////////////////////
+// CameraAREngine
+//////////////////////////////
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing):
+	CameraMobile(smoothing),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false)
+{
+	glGenTextures(1, &textureId_);
+}
+
+CameraAREngine::~CameraAREngine() {
+	// Disconnect ARCore service
+	close();
+
+	glDeleteTextures(1, &textureId_);
+}
+
+std::string CameraAREngine::getSerial() const
+{
+	return "AREngine";
+}
+
+bool CameraAREngine::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	UScopeMutex lock(arSessionMutex_);
+
+	HwArInstallStatus install_status;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	HwArEnginesApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case HWAR_INSTALL_STATUS_INSTALLED:
+			break;
+		case HWAR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(HwArSession_create(env_, context_, &arSession_) == HWAR_SUCCESS);
+	UASSERT(arSession_);
+
+	HwArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	HwArConfig_setFocusMode(arSession_, arConfig_, HWAR_FOCUS_MODE_FIXED);
+	UASSERT(HwArSession_configure(arSession_, arConfig_) == HWAR_SUCCESS);
+
+	HwArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	HwArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_); // May fail?!
+	//UASSERT(arCameraIntrinsics_);
+
+	HwArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	HwArConfig_setUpdateMode(arSession_, arConfig_, HWAR_UPDATE_MODE_BLOCKING);
+
+	deviceTColorCamera_ = opticalRotation;
+
+	// Required as ArSession_update does some off-screen OpenGL stuff...
+	HwArSession_setCameraTextureName(arSession_, textureId_);
+
+	if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
+	{
+		UERROR("Cannot resume camera!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraAREngine::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		HwArCameraIntrinsics_destroy(arSession_, arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if(arSession_!= nullptr)
+	{
+		HwArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		HwArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		HwArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		HwArPose_destroy(arPose_);
+	}
+	arPose_ = nullptr;
+
+	CameraMobile::close();
+}
+
+SensorData CameraAREngine::captureImage(CameraInfo * info)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
+		LOGE("CameraAREngine::captureImage() ArSession_update error");
+		return data;
+	}
+
+	HwArCamera* ar_camera;
+	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	HwArTrackingState camera_tracking_state;
+	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		HwArCamera_getPose(arSession_, ar_camera, arPose_);
+		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+
+		// Get calibration parameters
+		// FIXME: Hard-coded as getting intrinsics with the api fails
+		float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
+		int32_t camWidth=640, camHeight=480;
+		//HwArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		//HwArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		//HwArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		//HwArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &camWidth, &camHeight);
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, camWidth, camHeight);
+
+		if(fx > 0 && fy > 0 && camWidth > 0 && camHeight > 0 && cx > 0 && cy > 0)
+		{
+			//ArPointCloud * point_cloud;
+			//ArFrame_acquirePointCloud(ar_session_, ar_frame_, &point_cloud);
+
+			HwArImage * image = nullptr;
+			HwArImage * depthImage = nullptr;
+			HwArStatus statusRgb = HwArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			HwArStatus statusDepth = HwArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+			if(statusRgb == HWAR_SUCCESS && statusDepth == HWAR_SUCCESS)
+			{
+				int64_t timestamp_ns;
+				HwArFrame_getTimestamp(arSession_, arFrame_, &timestamp_ns);
+
+				int planeCount;
+				uint8_t *imageData = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				const AImage* ndkImageRGB;
+				HwArImage_getNdkImage(image, &ndkImageRGB);
+
+				AImage_getNumberOfPlanes(ndkImageRGB, &planeCount);
+				AImage_getWidth(ndkImageRGB, &width);
+				AImage_getHeight(ndkImageRGB, &height);
+				AImage_getPlaneRowStride(ndkImageRGB, 0, &stride);
+				AImage_getPlaneData(ndkImageRGB, 0, &imageData, &len);
+				LOGI("RGB: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputRGB;
+				if(imageData != nullptr && len>0)
+				{
+					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, CV_YUV2BGR_NV21);
+				}
+
+				//Depth
+				const AImage* ndkImageDepth;
+				HwArImage_getNdkImage(depthImage, &ndkImageDepth);
+				AImage_getNumberOfPlanes(ndkImageDepth, &planeCount);
+				AImage_getWidth(ndkImageDepth, &width);
+				AImage_getHeight(ndkImageDepth, &height);
+				AImage_getPlaneRowStride(ndkImageDepth, 0, &stride);
+				AImage_getPlaneData(ndkImageDepth, 0, &imageData, &len);
+				LOGI("Depth: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputDepth(height, width, CV_16UC1);
+				uint16_t *dataShort = (uint16_t *)imageData;
+				for (int y = 0; y < outputDepth.rows; ++y)
+				{
+					for (int x = 0; x < outputDepth.cols; ++x)
+					{
+						uint16_t depthSample = dataShort[y*outputDepth.cols + x];
+						uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
+						outputDepth.at<uint16_t>(y,x) = depthRange;
+					}
+				}
+
+				if(!outputRGB.empty() && !outputDepth.empty())
+				{
+					double stamp = double(timestamp_ns)/10e8;
+					CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
+					data = SensorData(outputRGB, outputDepth, model, 0, stamp);
+				}
+			}
+			else
+			{
+				LOGE("CameraAREngine: failed to get rgb image (status=%d %d)", (int)statusRgb, (int)statusDepth);
+			}
+
+			HwArImage_release(image);
+			HwArImage_release(depthImage);
+		}
+		else
+		{
+			LOGE("Invalid intrinsics!");
+		}
+	}
+
+	HwArCamera_release(ar_camera);
+
+	if(pose.isNull())
+	{
+		LOGE("CameraAREngine: Pose is null");
+	}
+	else
+	{
+		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+		this->poseReceived(pose);
+		info->odomPose = pose;
+	}
+	return data;
+
+}
+
+void CameraAREngine::capturePoseOnly()
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return;
+	}
+
+	HwArCamera* ar_camera;
+	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	HwArTrackingState camera_tracking_state;
+	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		HwArCamera_getPose(arSession_, ar_camera, arPose_);
+		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		if(!pose.isNull())
+		{
+			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+			this->poseReceived(pose);
+		}
+	}
+
+	HwArCamera_release(ar_camera);
+}
+
+} /* namespace rtabmap */

app/android/jni/CameraAREngine.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAARENGINE_H_
+#define CAMERAARENGINE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+
+#include <huawei_arengine_interface.h>
+
+namespace rtabmap {
+
+class CameraAREngine : public CameraMobile {
+public:
+	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
+	virtual ~CameraAREngine();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly();
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	HwArSession* arSession_ = nullptr;
+	HwArConfig* arConfig_ = nullptr;
+	HwArFrame* arFrame_ = nullptr;
+	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	HwArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	GLuint textureId_;
+	UMutex arSessionMutex_;
+
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARENGINE_H_ */

app/android/jni/CameraMobile.cpp

@@ -0,0 +1,314 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "CameraMobile.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+namespace rtabmap {
+
+#define nullptr 0
+
+//////////////////////////////
+// CameraMobile
+//////////////////////////////
+const float CameraMobile::bilateralFilteringSigmaS = 2.0f;
+const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
+
+const rtabmap::Transform CameraMobile::opticalRotation = Transform(
+		0.0f,  0.0f,  1.0f, 0.0f,
+	   -1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f, -1.0f,  0.0f, 0.0f);
+const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
+		0.0f, -1.0f,  0.0f, 0.0f,
+	    0.0f,  0.0f, -1.0f, 0.0f,
+		1.0f,  0.0f,  0.0f, 0.0f);
+
+CameraMobile::CameraMobile(bool smoothing) :
+		Camera(10),
+		deviceTColorCamera_(Transform::getIdentity()),
+		spinOncePreviousStamp_(0.0),
+		previousStamp_(0.0),
+		stampEpochOffset_(0.0),
+		smoothing_(smoothing),
+		colorCameraToDisplayRotation_(ROTATION_0),
+		originUpdate_(false)
+{
+}
+
+CameraMobile::~CameraMobile() {
+	// Disconnect camera service
+	close();
+}
+
+bool CameraMobile::init(const std::string &, const std::string &)
+{
+	deviceTColorCamera_ = opticalRotation;
+	return true;
+}
+
+void CameraMobile::close()
+{
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	originOffset_ = Transform();
+	originUpdate_ = false;
+	pose_ = Transform();
+	data_ = SensorData();
+}
+
+void CameraMobile::resetOrigin()
+{
+	originUpdate_ = true;
+}
+
+void CameraMobile::poseReceived(const Transform & pose)
+{
+	if(!pose.isNull())
+	{
+		// send pose of the camera (without optical rotation)
+		Transform p = pose*deviceTColorCamera_;
+		if(originUpdate_)
+		{
+			originOffset_ = p.translation().inverse();
+			originUpdate_ = false;
+		}
+
+		if(!originOffset_.isNull())
+		{
+			this->post(new PoseEvent(originOffset_*p));
+		}
+		else
+		{
+			this->post(new PoseEvent(p));
+		}
+	}
+}
+
+bool CameraMobile::isCalibrated() const
+{
+	return model_.isValidForProjection();
+}
+
+void CameraMobile::setGPS(const GPS & gps)
+{
+	lastKnownGPS_ = gps;
+}
+
+void CameraMobile::setData(const SensorData & data, const Transform & pose)
+{
+	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	data_ = data;
+	pose_ = pose;
+}
+
+void CameraMobile::addEnvSensor(int type, float value)
+{
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
+void CameraMobile::spinOnce()
+{
+	if(!this->isRunning())
+	{
+		bool ignoreFrame = false;
+		float rate = 10.0f; // maximum 10 FPS for image data
+		double now = UTimer::now();
+		if(rate>0.0f)
+		{
+			if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
+				((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
+			{
+				ignoreFrame = true;
+			}
+		}
+
+		if(!ignoreFrame)
+		{
+			spinOnceFrameRateTimer_.start();
+			spinOncePreviousStamp_ = now;
+			mainLoop();
+		}
+		else
+		{
+			// just send pose
+			capturePoseOnly();
+		}
+	}
+}
+
+void CameraMobile::mainLoopBegin()
+{
+	double t = cameraStartedTime_.elapsed();
+	if(t < 5.0)
+	{
+		uSleep((5.0-t)*1000); // just to make sure that the camera is started
+	}
+}
+
+void CameraMobile::mainLoop()
+{
+	CameraInfo info;
+	SensorData data = this->captureImage(&info);
+
+	if(data.isValid() && !info.odomPose.isNull())
+	{
+		if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
+		{
+			data.setGPS(lastKnownGPS_);
+		}
+		else if(lastKnownGPS_.stamp()>0.0)
+		{
+			LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
+		}
+
+		if(lastEnvSensors_.size())
+		{
+			data.setEnvSensors(lastEnvSensors_);
+			lastEnvSensors_.clear();
+		}
+
+		if(smoothing_ && !data.depthRaw().empty())
+		{
+			//UTimer t;
+			data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
+			//LOGD("Bilateral filtering, time=%fs", t.ticks());
+		}
+
+		// Rotate image depending on the camera orientation
+		if(colorCameraToDisplayRotation_ == ROTATION_90)
+		{
+			cv::Mat rgb, depth;
+			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
+			cv::flip(data.imageRaw(),rgb,1);
+			cv::transpose(rgb,rgbt);
+			rgb = rgbt;
+			cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
+			cv::flip(data.depthRaw(),depth,1);
+			cv::transpose(depth,deptht);
+			depth = deptht;
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_180)
+		{
+			cv::Mat rgb, depth;
+			cv::flip(data.imageRaw(),rgb,1);
+			cv::flip(rgb,rgb,0);
+			cv::flip(data.depthOrRightRaw(),depth,1);
+			cv::flip(depth,depth,0);
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageWidth(), model.imageHeight());
+			model = CameraModel(
+					model.fx(),
+					model.fy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_270)
+		{
+			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
+			cv::transpose(data.imageRaw(),rgb);
+			cv::flip(rgb,rgb,1);
+			cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
+			cv::transpose(data.depthOrRightRaw(),depth);
+			cv::flip(depth,depth,1);
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.cx(),
+					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+		}
+
+		rtabmap::Transform pose = info.odomPose;
+		data.setGroundTruth(Transform());
+
+		// convert stamp to epoch
+		bool firstFrame = previousPose_.isNull();
+		if(firstFrame)
+		{
+			stampEpochOffset_ = UTimer::now()-data.stamp();
+		}
+		data.setStamp(stampEpochOffset_ + data.stamp());
+		OdometryInfo info;
+		if(!firstFrame)
+		{
+			info.interval = data.stamp()-previousStamp_;
+			info.transform = previousPose_.inverse() * pose;
+		}
+		// linear cov = 0.0001
+		info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
+		if(!firstFrame)
+		{
+			// angular cov = 0.000001
+			info.reg.covariance.at<double>(3,3) *= 0.01;
+			info.reg.covariance.at<double>(4,4) *= 0.01;
+			info.reg.covariance.at<double>(5,5) *= 0.01;
+		}
+		LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
+		this->post(new OdometryEvent(data, pose, info));
+		previousPose_ = pose;
+		previousStamp_ = data.stamp();
+	}
+	else if(!this->isKilled())
+	{
+		LOGW("Odometry lost");
+		this->post(new OdometryEvent());
+	}
+}
+
+SensorData CameraMobile::captureImage(CameraInfo * info)
+{
+	if(info)
+	{
+		info->odomPose = pose_;
+	}
+	return data_;
+}
+
+} /* namespace rtabmap */

app/android/jni/CameraMobile.h

@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAMOBILE_H_
+#define CAMERAMOBILE_H_
+
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include "util.h"
+
+namespace rtabmap {
+
+class CameraInfoEvent: public UEvent
+{
+public:
+	CameraInfoEvent(int type, const std::string & key, const std::string & value) : type_(type), key_(key), value_(value) {}
+	virtual std::string getClassName() const {return "CameraInfoEvent";}
+	int type() const {return type_;}
+	const std::string & key() const {return key_;}
+	const std::string & value() const {return value_;}
+
+private:
+	int type_;
+	std::string key_;
+	std::string value_;
+
+};
+
+class PoseEvent: public UEvent
+{
+public:
+	PoseEvent(const Transform & pose) : pose_(pose) {}
+	virtual std::string getClassName() const {return "PoseEvent";}
+	const Transform & pose() const {return pose_;}
+
+private:
+	Transform pose_;
+};
+
+class CameraMobile : public Camera, public UThread, public UEventsSender {
+public:
+	static const float bilateralFilteringSigmaS;
+	static const float bilateralFilteringSigmaR;
+
+	static const rtabmap::Transform opticalRotation;
+	static const rtabmap::Transform opticalRotationInv;
+
+public:
+	CameraMobile(bool smoothing = false);
+	virtual ~CameraMobile();
+
+	// abstract functions
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // inherited classes should call its parent in their close().
+	virtual std::string getSerial() const {return "CameraMobile";}
+
+	const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
+	void resetOrigin();
+	virtual bool isCalibrated() const;
+
+	void poseReceived(const Transform & pose); // in rtabmap frame
+
+	const CameraModel & getCameraModel() const {return model_;}
+	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
+	void setSmoothing(bool enabled) {smoothing_ = enabled;}
+	void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setGPS(const GPS & gps);
+	void addEnvSensor(int type, float value);
+	void setData(const SensorData & data, const Transform & pose);
+
+	void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly() {}
+
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+protected:
+	CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
+	Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
+	UTimer spinOnceFrameRateTimer_;
+	double spinOncePreviousStamp_;
+
+private:
+	Transform previousPose_;
+	double previousStamp_;
+	UTimer cameraStartedTime_;
+	double stampEpochOffset_;
+	bool smoothing_;
+	ScreenRotation colorCameraToDisplayRotation_;
+	GPS lastKnownGPS_;
+	EnvSensors lastEnvSensors_;
+	Transform originOffset_;
+	bool originUpdate_;
+
+	SensorData data_;
+	Transform pose_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERATANGO_H_ */

app/android/jni/CameraTango.cpp

@@ -91,7 +91,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 	if(pose->status_code == TANGO_POSE_VALID)
 	{
 		CameraTango* app = static_cast<CameraTango*>(context);
-		app->poseReceived(app->tangoPoseToTransform(pose));
+		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
 	}
 }
 
@@ -104,23 +104,15 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-const float CameraTango::bilateralFilteringSigmaS = 2.0f;
-const float CameraTango::bilateralFilteringSigmaR = 0.075f;
-
 CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
-		Camera(0),
+		CameraMobile(smoothing),
 		tango_config_(0),
-		previousStamp_(0.0),
-		stampEpochOffset_(0.0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),
 		rawScanPublished_(publishRawScan),
-		smoothing_(smoothing),
 		cloudStamp_(0),
 		tangoColorType_(0),
-		tangoColorStamp_(0),
-		colorCameraToDisplayRotation_(ROTATION_0),
-		originUpdate_(false)
+		tangoColorStamp_(0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -350,6 +342,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			pose_data.orientation[1],
 			pose_data.orientation[2],
 			pose_data.orientation[3]);
+	deviceTColorCamera_ = rtabmap_world_T_opengl_world * deviceTColorCamera_;
 
 	// camera intrinsic
 	TangoCameraIntrinsics color_camera_intrinsics;
@@ -408,18 +401,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	model_ = CameraModel(colorCamera_?"color":"fisheye",
 			cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
 			K, D, R, P,
-			tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_); // device to camera optical rotation in rtabmap frame
+			deviceTColorCamera_);
 
 	if(!colorCamera_)
 	{
 		initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
 	}
 
-	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
-	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
-
-	cameraStartedTime_.restart();
-
+	LOGI("deviceTColorCameraRtabmap  =%s", deviceTColorCamera_.prettyPrint().c_str());
 	return true;
 }
 
@@ -433,18 +422,10 @@ void CameraTango::close()
 		TangoService_disconnect();
 		LOGI("TangoService_disconnect() done.");
 	}
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
-	lastKnownGPS_ = GPS();
-	originOffset_ = Transform();
-	originUpdate_ = false;
-}
 
-void CameraTango::resetOrigin()
-{
-	originUpdate_ = true;
+	CameraMobile::close();
 }
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -499,40 +480,9 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	}
 }
 
-static rtabmap::Transform opticalRotation(
-								1.0f,  0.0f,  0.0f, 0.0f,
-							    0.0f, -1.0f,  0.0f, 0.0f,
-								0.0f,  0.0f, -1.0f, 0.0f);
-void CameraTango::poseReceived(const Transform & pose)
-{
-	if(!pose.isNull())
-	{
-		// send pose of the camera (without optical rotation), not the device
-		Transform p = pose*deviceTColorCamera_*opticalRotation;
-		if(originUpdate_)
-		{
-			originOffset_ = p.translation().inverse();
-			originUpdate_ = false;
-		}
-		if(!originOffset_.isNull())
-		{
-			this->post(new PoseEvent(originOffset_*p));
-		}
-		else
-		{
-			this->post(new PoseEvent(p));
-		}
-	}
-}
-
 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
 {
-	this->post(new CameraTangoEvent(type, key, value));
-}
-
-bool CameraTango::isCalibrated() const
-{
-	return model_.isValidForProjection();
+	this->post(new CameraInfoEvent(type, key, value));
 }
 
 std::string CameraTango::getSerial() const
@@ -540,11 +490,6 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }
 
-void CameraTango::setGPS(const GPS & gps)
-{
-	lastKnownGPS_ = gps;
-}
-
 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
@@ -588,7 +533,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	else
 	{
 
-		pose = tangoPoseToTransform(&pose_start_service_T_device);
+		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
 	}
 
 	return pose;
@@ -604,7 +549,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		if(this->isRunning())
 		{
 			LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
-			this->post(new CameraTangoEvent(0, "CameraTango", "No frames received since 2 seconds."));
+			this->post(new CameraInfoEvent(0, "CameraTango", "No frames received since 2 seconds."));
 
 			boost::mutex::scoped_lock  lock(dataMutex_);
 			if(!cloud_.empty() && !tangoColor_.empty())
@@ -798,7 +743,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 			if(closePoints > 100)
 			{
-				this->post(new CameraTangoEvent(0, "TooClose", ""));
+				this->post(new CameraInfoEvent(0, "TooClose", ""));
 			}
 
 			if(oi)
@@ -816,79 +761,23 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
 
-			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
-
-			// adjust origin
-			if(!originOffset_.isNull())
-			{
-				poseDevice = originOffset_ * poseDevice;
-			}
+			Transform odom = getPoseAtTimestamp(rgbStamp);
 
 			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
 			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
 			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
 
-			//Rotate in RTAB-Map's coordinate
-			Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
+			// adjust origin
+			if(!getOriginOffset().isNull())
+			{
+				odom = getOriginOffset() * odom;
+			}
 
 			//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
 			//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
 
 			Transform scanLocalTransform = model.localTransform();
 
-			// Rotate image depending on the camera orientation
-			if(colorCameraToDisplayRotation_ == ROTATION_90)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::flip(rgb,rgb,1);
-				cv::transpose(rgb,rgbt);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::flip(depth,depth,1);
-				cv::transpose(depth,deptht);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy(), model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_180)
-			{
-				cv::flip(rgb,rgb,1);
-				cv::flip(rgb,rgb,0);
-				cv::flip(depth,depth,1);
-				cv::flip(depth,depth,0);
-				cv::Size sizet(model.imageWidth(), model.imageHeight());
-				model = CameraModel(
-						model.fx(),
-						model.fy(),
-						model.cx()>0?model.imageWidth()-model.cx():0,
-						model.cy()>0?model.imageHeight()-model.cy():0,
-						model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144*2.0));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_270)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::transpose(rgb,rgbt);
-				cv::flip(rgbt,rgbt,1);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::transpose(depth,deptht);
-				cv::flip(deptht,deptht,1);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy()>0?model.imageHeight()-model.cy():0, model.cx(), model.localTransform()*rtabmap::Transform(0,0,0,0,0,-1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-
-			if(smoothing_)
-			{
-				//UTimer t;
-				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
-				data.setDepthOrRightRaw(depth);
-				//LOGD("Bilateral filtering, time=%fs", t.ticks());
-			}
-
 			if(rawScanPublished_)
 			{
 				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
@@ -897,16 +786,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			{
 				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
-			data.setGroundTruth(odom);
-
-			if(lastKnownGPS_.stamp() > 0.0 && rgbStamp-lastKnownGPS_.stamp()<1.0)
-			{
-				data.setGPS(lastKnownGPS_);
-			}
-			else if(lastKnownGPS_.stamp()>0.0)
-			{
-				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
-			}
+			info->odomPose = odom;
 		}
 		else
 		{
@@ -917,56 +797,4 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 }
 
-void CameraTango::mainLoopBegin()
-{
-	double t = cameraStartedTime_.elapsed();
-	if(t < 5.0)
-	{
-		uSleep((5.0-t)*1000); // just to make sure that the camera is started
-	}
-}
-
-void CameraTango::mainLoop()
-{
-	if(tango_config_)
-	{
-		SensorData data = this->captureImage();
-
-		if(!data.groundTruth().isNull())
-		{
-			rtabmap::Transform pose = data.groundTruth();
-			data.setGroundTruth(Transform());
-
-			// convert stamp to epoch
-			bool firstFrame = previousPose_.isNull();
-			if(firstFrame)
-			{
-				stampEpochOffset_ = UTimer::now()-data.stamp();
-			}
-			data.setStamp(stampEpochOffset_ + data.stamp());
-			OdometryInfo info;
-			if(!firstFrame)
-			{
-				info.interval = data.stamp()-previousStamp_;
-				info.transform = previousPose_.inverse() * pose;
-			}
-			info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
-			LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
-			this->post(new OdometryEvent(data, pose, info));
-			previousPose_ = pose;
-			previousStamp_ = data.stamp();
-		}
-		else if(!this->isKilled())
-		{
-			LOGW("Odometry lost");
-			this->post(new OdometryEvent());
-		}
-	}
-	else
-	{
-		UERROR("Camera not initialized, cannot start thread.");
-		this->kill();
-	}
-}
-
 } /* namespace rtabmap */

app/android/jni/CameraTango.h

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CAMERATANGO_H_
 #define CAMERATANGO_H_
 
+#include "CameraMobile.h"
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
@@ -37,65 +38,26 @@ 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 <tango_client_api.h>
 #include <tango_support_api.h>
 
-class TangoPoseData;
-
 namespace rtabmap {
 
-class PoseEvent: public UEvent
-{
-public:
-	PoseEvent(const Transform & pose) : pose_(pose) {}
-	virtual std::string getClassName() const {return "PoseEvent";}
-	const Transform & pose() const {return pose_;}
-
-private:
-	Transform pose_;
-};
-
-class CameraTangoEvent: public UEvent
-{
-public:
-	CameraTangoEvent(int type, const std::string & key, const std::string & value) : type_(type), key_(key), value_(value) {}
-	virtual std::string getClassName() const {return "CameraTangoEvent";}
-	int type() const {return type_;}
-	const std::string & key() const {return key_;}
-	const std::string & value() const {return value_;}
-
-private:
-	int type_;
-	std::string key_;
-	std::string value_;
-
-};
-
-class CameraTango : public Camera, public UThread, public UEventsSender {
-public:
-	static const float bilateralFilteringSigmaS;
-	static const float bilateralFilteringSigmaR;
-
+class CameraTango : public CameraMobile {
 public:
 	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	void close(); // close Tango connection
-	void resetOrigin();
-	virtual bool isCalibrated() const;
+	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
-	const CameraModel & getCameraModel() const {return model_;}
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
 	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
-	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
-	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
-	void setGPS(const GPS & gps);
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
-	void poseReceived(const Transform & pose);
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
@@ -104,19 +66,11 @@ protected:
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
 
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-
 private:
 	void * tango_config_;
-	Transform previousPose_;
-	double previousStamp_;
-	UTimer cameraStartedTime_;
-	double stampEpochOffset_;
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	bool smoothing_;
 	cv::Mat cloud_;
 	double cloudStamp_;
 	cv::Mat tangoColor_;
@@ -124,14 +78,8 @@ private:
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	CameraModel model_;
-	Transform deviceTColorCamera_;
-	TangoSupportRotation colorCameraToDisplayRotation_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
-	GPS lastKnownGPS_;
-	Transform originOffset_;
-	bool originUpdate_;
 };
 
 } /* namespace rtabmap */

app/android/jni/RTABMapApp.h

@@ -31,11 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <jni.h>
 #include <memory>
 
-#include <tango_client_api.h>  // NOLINT
 #include <tango-gl/util.h>
 
 #include "scene.h"
-#include "CameraTango.h"
+#include "CameraMobile.h"
 #include "util.h"
 #include "ProgressionStatus.h"
 
@@ -49,39 +48,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class RTABMapApp : public UEventsHandler {
  public:
   // Constructor and deconstructor.
-  RTABMapApp();
+  RTABMapApp(JNIEnv* env, jobject caller_activity);
   ~RTABMapApp();
 
-  void onCreate(JNIEnv* env, jobject caller_activity);
-
   void setScreenRotation(int displayRotation, int cameraRotation);
 
   int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
 
-  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
-
-  // Explicitly reset motion tracking and restart the pipeline.
-  // Note that this will cause motion tracking to re-initialize.
-  void TangoResetMotionTracking();
-
-  // Tango Service point cloud callback function for depth data. Called when new
-  // new point cloud data is available from the Tango Service.
-  //
-  // @param pose: The current point cloud returned by the service,
-  //              caller allocated.
-  void onPointCloudAvailable(const TangoXYZij* xyz_ij);
-
-  // Tango service pose callback function for pose data. Called when new
-  // information about device pose is available from the Tango Service.
-  //
-  // @param pose: The current pose returned by the service, caller allocated.
-  void onPoseAvailable(const TangoPoseData* pose);
-
-  // Tango service event callback function for event data. Called when new events
-  // are available from the Tango Service.
-  //
-  // @param event: Tango event, caller allocated.
-  void onTangoEventAvailable(const TangoEvent* event);
+  bool isBuiltWith(int cameraDriver) const;
+  bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
 
   // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
   void InitializeGLContent();
@@ -92,8 +67,7 @@ class RTABMapApp : public UEventsHandler {
   // Main render loop.
   int Render();
 
-  // Release all non-OpenGL allocated resources.
-  void onPause();
+  void stopCamera();
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
@@ -148,8 +122,8 @@ class RTABMapApp : public UEventsHandler {
   void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
   void setGPS(const rtabmap::GPS & gps);
+  void addEnvSensor(int type, float value);
 
-  void resetMapping();
   void save(const std::string & databasePath);
   void cancelProcessing();
   bool exportMesh(
@@ -173,21 +147,33 @@ class RTABMapApp : public UEventsHandler {
   bool writeExportedMesh(const std::string & directory, const std::string & name);
   int postProcessing(int approach);
 
+  void postCameraPoseEvent(
+  		float x, float y, float z, float qx, float qy, float qz, float qw);
+
+  void postOdometryEvent(
+  		float x, float y, float z, float qx, float qy, float qz, float qw,
+  		float fx, float fy, float cx, float cy,
+  		double stamp,
+		void * yPlane, void * uPlane, void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+  		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		float * points, int pointsLen);
+
  protected:
   virtual bool handleEvent(UEvent * event);
 
  private:
   rtabmap::ParametersMap getRtabmapParameters();
-  bool smoothMesh(int id, Mesh & mesh);
+  bool smoothMesh(int id, rtabmap::Mesh & mesh);
   void gainCompensation(bool full = false);
   std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
   std::vector<pcl::Vertices> filterPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
 
  private:
-  rtabmap::CameraTango * camera_;
+  int cameraDriver_;
+  rtabmap::CameraMobile * camera_;
   rtabmap::RtabmapThread * rtabmapThread_;
   rtabmap::Rtabmap * rtabmap_;
-  LogHandler * logHandler_;
+  rtabmap::LogHandler * logHandler_;
 
   bool odomCloudShown_;
   bool graphOptimization_;
@@ -211,7 +197,6 @@ class RTABMapApp : public UEventsHandler {
 
   rtabmap::ParametersMap mappingParameters_;
 
-  bool paused_;
   bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
   bool openingDatabase_;
@@ -243,14 +228,13 @@ class RTABMapApp : public UEventsHandler {
   Scene main_scene_;
 
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
-	std::list<rtabmap::RtabmapEvent*> visLocalizationEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 
 	rtabmap::Transform mapToOdom_;
 
+	boost::mutex cameraMutex_;
 	boost::mutex rtabmapMutex_;
-	boost::mutex visLocalizationMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
@@ -258,7 +242,7 @@ class RTABMapApp : public UEventsHandler {
 
 	USemaphore screenshotReady_;
 
-	std::map<int, Mesh> createdMeshes_;
+	std::map<int, rtabmap::Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;

app/android/jni/point_cloud_drawable.cpp

@@ -318,7 +318,7 @@ PointCloudDrawable::PointCloudDrawable(
 }
 
 PointCloudDrawable::PointCloudDrawable(
-		const Mesh & mesh,
+		const rtabmap::Mesh & mesh,
 		bool createWireframe) :
 				vertex_buffers_(0),
 				textures_(0),
@@ -525,7 +525,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	nPoints_ = totalPoints;
 }
 
-void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
+void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;

app/android/jni/point_cloud_drawable.h

@@ -55,13 +55,13 @@ private:
 		  float gainG = 1.0f,
 		  float gainB = 1.0f);
   PointCloudDrawable(
-    	  const Mesh & mesh,
+    	  const rtabmap::Mesh & mesh,
 		  bool createWireframe = false);
   virtual ~PointCloudDrawable();
 
   void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
   void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
-  void updateMesh(const Mesh & mesh, bool createWireframe = false);
+  void updateMesh(const rtabmap::Mesh & mesh, bool createWireframe = false);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}

app/android/jni/scene.cpp

@@ -79,7 +79,8 @@ Scene::Scene() :
 		graphVisible_(true),
 		gridVisible_(true),
 		traceVisible_(true),
-		color_camera_to_display_rotation_(ROTATION_0),
+		frustumVisible_(true),
+		color_camera_to_display_rotation_(rtabmap::ROTATION_0),
 		currentPose_(0),
 		graph_shader_program_(0),
 		blending_(true),
@@ -102,12 +103,13 @@ Scene::Scene() :
 {
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
-	      tango_gl::GestureCamera::kFirstPerson);
+	      tango_gl::GestureCamera::kThirdPersonFollow);
 }
 
 Scene::~Scene() {
 	DeleteResources();
 	delete gesture_camera_;
+	delete currentPose_;
 }
 
 //Should only be called in OpenGL thread!
@@ -126,7 +128,6 @@ void Scene::InitGLContent()
 	trace_ = new tango_gl::Trace();
 	grid_ = new tango_gl::Grid();
 	box_ = new BoundingBoxDrawable();
-	currentPose_ = new rtabmap::Transform();
 
 
 	axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
@@ -158,7 +159,6 @@ void Scene::DeleteResources() {
 		delete frustum_;
 		delete trace_;
 		delete grid_;
-		delete currentPose_;
 		delete box_;
 	}
 
@@ -188,6 +188,10 @@ void Scene::clear()
 	{
 		delete iter->second;
 	}
+	for(std::map<int, tango_gl::Axis*>::iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
+	{
+		delete iter->second;
+	}
 	if(trace_)
 	{
 		trace_->ClearVertexArray();
@@ -198,6 +202,7 @@ void Scene::clear()
 		graph_ = 0;
 	}
 	pointClouds_.clear();
+	markers_.clear();
 	if(grid_)
 	{
 		grid_->SetPosition(kHeightOffset);
@@ -362,6 +367,10 @@ bool intersectFrustumAABB(
 int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
 
+	if(currentPose_ == 0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
 	Eigen::Quaternionf quat = currentPose_->getQuaternionf();
 	glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
@@ -488,7 +497,7 @@ int Scene::Render() {
 
 	if(!currentPose_->isNull())
 	{
-		if (gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
 		{
 			frustum_->SetPosition(position);
 			frustum_->SetRotation(rotation);
@@ -497,8 +506,13 @@ int Scene::Render() {
 			frustum_->SetScale(kFrustumScale);
 			frustum_->Render(projectionMatrix, viewMatrix);
 
-			axis_->SetPosition(position);
-			axis_->SetRotation(rotation);
+			rtabmap::Transform cameraFrame = *currentPose_*rtabmap::optical_T_opengl*rtabmap::CameraMobile::opticalRotationInv;
+			glm::vec3 positionCamera(cameraFrame.x(), cameraFrame.y(), cameraFrame.z());
+			Eigen::Quaternionf quatCamera = cameraFrame.getQuaternionf();
+			glm::quat rotationCamera(quatCamera.w(), quatCamera.x(), quatCamera.y(), quatCamera.z());
+
+			axis_->SetPosition(positionCamera);
+			axis_->SetRotation(rotationCamera);
 			axis_->Render(projectionMatrix, viewMatrix);
 		}
 
@@ -507,11 +521,11 @@ int Scene::Render() {
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+	}
 
-		if(gridVisible_)
-		{
-			grid_->Render(projectionMatrix, viewMatrix);
-		}
+	if(gridVisible_)
+	{
+		grid_->Render(projectionMatrix, viewMatrix);
 	}
 
 	if(graphVisible_ && graph_)
@@ -551,6 +565,12 @@ int Scene::Render() {
 		glDepthMask(GL_TRUE);
 	}
 
+	//draw markers on foreground
+	for(std::map<int, tango_gl::Axis*>::const_iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
+	{
+		iter->second->Render(projectionMatrix, viewMatrix);
+	}
+
 	return (int)cloudsToDraw.size();
 }
 
@@ -560,8 +580,11 @@ void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {
 
 void Scene::SetCameraPose(const rtabmap::Transform & pose)
 {
-	UASSERT(currentPose_ != 0);
 	UASSERT(!pose.isNull());
+	if(currentPose_ ==0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	*currentPose_ = pose;
 }
 
@@ -589,7 +612,7 @@ rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 	{
 		*fov = gesture_camera_->getFOV();
 	}
-	return glmToTransform(gesture_camera_->GetTransformationMatrix());
+	return rtabmap::glmToTransform(gesture_camera_->GetTransformationMatrix());
 }
 
 void Scene::OnTouchEvent(int touch_count,
@@ -647,7 +670,59 @@ void Scene::setTraceVisible(bool visible)
 	traceVisible_ = visible;
 }
 
+void Scene::setFrustumVisible(bool visible)
+{
+	frustumVisible_ = visible;
+}
+
 //Should only be called in OpenGL thread!
+void Scene::addMarker(
+		int id,
+		const rtabmap::Transform & pose)
+{
+	LOGI("add marker %d", id);
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter == markers_.end())
+	{
+		//create
+		tango_gl::Axis * drawable = new tango_gl::Axis();
+		drawable->SetScale(glm::vec3(0.05f,0.05f,0.05f));
+		drawable->SetLineWidth(5);
+		markers_.insert(std::make_pair(id, drawable));
+	}
+	setMarkerPose(id, pose);
+}
+void Scene::setMarkerPose(int id, const rtabmap::Transform & pose)
+{
+	UASSERT(!pose.isNull());
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter != markers_.end())
+	{
+		glm::vec3 position(pose.x(), pose.y(), pose.z());
+		Eigen::Quaternionf quat = pose.getQuaternionf();
+		glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
+		iter->second->SetPosition(position);
+		iter->second->SetRotation(rotation);
+	}
+}
+bool Scene::hasMarker(int id) const
+{
+	return markers_.find(id) != markers_.end();
+}
+void Scene::removeMarker(int id)
+{
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter != markers_.end())
+	{
+		delete iter->second;
+		markers_.erase(iter);
+	}
+}
+std::set<int> Scene::getAddedMarkers() const
+{
+	return uKeysSet(markers_);
+}
+
 void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -670,7 +745,7 @@ void Scene::addCloud(
 
 void Scene::addMesh(
 		int id,
-		const Mesh & mesh,
+		const rtabmap::Mesh & mesh,
 		const rtabmap::Transform & pose,
 		bool createWireframe)
 {
@@ -789,7 +864,7 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
 	}
 }
 
-void Scene::updateMesh(int id, const Mesh & mesh)
+void Scene::updateMesh(int id, const rtabmap::Mesh & mesh)
 {
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())

app/android/jni/scene.h

@@ -21,7 +21,7 @@
 #include <memory>
 #include <set>
 
-#include <tango_client_api.h>  // NOLINT
+#include "CameraMobile.h"
 #include <tango-gl/axis.h>
 #include <tango-gl/camera.h>
 #include <tango-gl/color.h>
@@ -60,7 +60,8 @@ class Scene {
   int getViewPortWidth() const {return screenWidth_;}
   int getViewPortHeight() const {return screenHeight_;}
 
-  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
+  rtabmap::ScreenRotation getScreenRotation() const {return color_camera_to_display_rotation_;}
+  void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 
   void clear(); // removed all point clouds
 
@@ -78,7 +79,7 @@ class Scene {
   //         top down
   void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
 
-  void SetCameraPose(const rtabmap::Transform & pose);
+  void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
   rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
   rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;
 
@@ -101,6 +102,13 @@ class Scene {
   void setGraphVisible(bool visible);
   void setGridVisible(bool visible);
   void setTraceVisible(bool visible);
+  void setFrustumVisible(bool visible);
+
+  void addMarker(int id, const rtabmap::Transform & pose);
+  void setMarkerPose(int id, const rtabmap::Transform & pose);
+  bool hasMarker(int id) const;
+  void removeMarker(int id);
+  std::set<int> getAddedMarkers() const;
 
   void addCloud(
   		  int id,
@@ -109,7 +117,7 @@ class Scene {
   		  const rtabmap::Transform & pose);
   void addMesh(
   		int id,
-  		const Mesh & mesh,
+  		const rtabmap::Mesh & mesh,
   		const rtabmap::Transform & pose,
 		bool createWireframe = false);
 
@@ -120,7 +128,7 @@ class Scene {
   bool hasTexture(int id) const;
   std::set<int> getAddedClouds() const;
   void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
-  void updateMesh(int id, const Mesh & mesh);
+  void updateMesh(int id, const rtabmap::Mesh & mesh);
   void updateGains(int id, float gainR, float gainG, float gainB);
 
   void setBlending(bool enabled) {blending_ = enabled;}
@@ -166,8 +174,11 @@ class Scene {
   bool graphVisible_;
   bool gridVisible_;
   bool traceVisible_;
+  bool frustumVisible_;
 
-  TangoSupportRotation color_camera_to_display_rotation_;
+  std::map<int, tango_gl::Axis*> markers_;
+
+  rtabmap::ScreenRotation color_camera_to_display_rotation_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
 

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

@@ -18,11 +18,14 @@
 namespace tango_gl {
 
 static const float float_vertices[] = {
-    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  1.0f,
-    1.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f, 0.0f,  0.0f,
-    0.0f,  1.0f,  -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
-    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f,
-    -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
+    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f,  -1.0f, -1.0f,
+	-1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f,
+	1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f,
+	-1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
 
 Frustum::Frustum() : Line(3.0f, GL_LINES) {
   SetShader();

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

@@ -31,13 +31,17 @@ Grid::Grid(float density, int qx, int qy) : Line(1.0f, GL_LINES) {
 
   // Horizontal line.
   for (int i = 0; i < (qy + 1); i++) {
-    vec_vertices_.push_back(glm::vec3(-width, 0.0f, -height + i * density));
-    vec_vertices_.push_back(glm::vec3(width, 0.0f, -height + i * density));
+	  for (int j = 0; j < (qx + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + j*density, 0.0f, -height + i * density));
+		vec_vertices_.push_back(glm::vec3(-width+ + (j+1)*density, 0.0f, -height + i * density));
+	  }
   }
 
   for (int i = 0; i < (qx + 1); i++) {
-    vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height));
-    vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, height));
+	  for (int j = 0; j < (qy + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + j*density));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + (j+1)*density));
+	  }
   }
 }
 }  // namespace tango_gl

app/android/jni/tango-gl/include/tango-gl/util.h

@@ -25,7 +25,6 @@
 #include <android/log.h>
 #include <GLES2/gl2.h>
 #include <GLES2/gl2ext.h>
-#include <tango_support_api.h>
 
 #include "glm/glm.hpp"
 #include "glm/gtc/matrix_transform.hpp"
@@ -44,6 +43,7 @@
 #define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
 #endif
 #define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
+#define LOGF(...) __android_log_print(ANDROID_LOG_FATAL,LOG_TAG,__VA_ARGS__)
 
 #ifndef M_PI
 #define M_PI 3.1415926f
@@ -84,32 +84,6 @@ namespace util {
 
   glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec);
 
-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @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
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      int display_rotation, int color_camera_rotation);
-
-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @param display: the device display orientation.
-  // @param color_camera: integer value of color camera oreintation, values
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      TangoSupportRotation display_rotation, int color_camera_rotation);
-
 }  // namespace util
 }  // namespace tango_gl
 #endif  // TANGO_GL_RENDERER_GL_UTIL

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

@@ -238,23 +238,4 @@ glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
   return glm::vec3(mat * glm::vec4(vec, 1.0f));
 }
 
-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    int display_rotation, int color_camera_rotation) {
-  TangoSupportRotation r =
-      static_cast<TangoSupportRotation>(display_rotation);
-  return util::GetAndroidRotationFromColorCameraToDisplay(
-      r, color_camera_rotation);
-}
-
-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    TangoSupportRotation display_rotation, int color_camera_rotation) {
-  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
-
-  int ret = static_cast<int>(display_rotation) - color_camera_n;
-  if (ret < 0) {
-    ret += 4;
-  }
-  return static_cast<TangoSupportRotation>(ret % 4);
-}
-
 }  // namespace tango_gl

app/android/jni/util.h

@@ -39,6 +39,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/Vertices.h>
 #include <pcl/pcl_base.h>
 
+namespace rtabmap {
+
 class LogHandler : public UEventsHandler
 {
 public:
@@ -58,19 +60,23 @@ protected:
 			ULogEvent * logEvent = (ULogEvent*)event;
 			if(logEvent->getCode() == ULogger::kDebug)
 			{
-				LOGD(logEvent->getMsg().c_str());
+				LOGD("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() == ULogger::kInfo)
 			{
-				LOGI(logEvent->getMsg().c_str());
+				LOGI("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() == ULogger::kWarning)
 			{
-				LOGW(logEvent->getMsg().c_str());
+				LOGW("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() >= ULogger::kError)
 			{
-				LOGE(logEvent->getMsg().c_str());
+				LOGE("%s", logEvent->getMsg().c_str());
+			}
+			else if(logEvent->getCode() >= ULogger::kFatal)
+			{
+				LOGF("%s", logEvent->getMsg().c_str());
 			}
 
 		}
@@ -78,6 +84,11 @@ protected:
 	}
 };
 
+static const rtabmap::Transform optical_T_opengl(
+		1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f, -1.0f,  0.0f, 0.0f,
+		0.0f,  0.0f, -1.0f, 0.0f);
+
 static const rtabmap::Transform opengl_world_T_tango_world(
 		1.0f,  0.0f,  0.0f, 0.0f,
 		0.0f,  0.0f,  1.0f, 0.0f,
@@ -88,20 +99,25 @@ static const rtabmap::Transform rtabmap_world_T_tango_world(
 	    -1.0f, 0.0f, 0.0f, 0.0f,
 		 0.0f, 0.0f, 1.0f, 0.0f);
 
-static const rtabmap::Transform tango_device_T_rtabmap_device(
+static const rtabmap::Transform tango_device_T_rtabmap_world(
 		 0.0f, -1.0f, 0.0f, 0.0f,
 	     0.0f,  0.0f, 1.0f, 0.0f,
 		-1.0f,  0.0f, 0.0f, 0.0f);
 
+static const rtabmap::Transform tango_world_T_rtabmap_world(
+		 0.0f, -1.0f, 0.0f, 0.0f,
+	     1.0f, 0.0f, 0.0f, 0.0f,
+		 0.0f, 0.0f, 1.0f, 0.0f);
+
 static const rtabmap::Transform opengl_world_T_rtabmap_world(
 		 0.0f, -1.0f, 0.0f, 0.0f,
 		 0.0f,  0.0f, 1.0f, 0.0f,
 		-1.0f,  0.0f, 0.0f, 0.0f);
 
-static const rtabmap::Transform rtabmap_device_T_opengl_device(
-		 0.0f, 0.0f, -1.0f, 0.0f,
-		-1.0f, 0.0f,  0.0f, 0.0f,
-		 0.0f, 1.0f,  0.0f, 0.0f);
+static const rtabmap::Transform rtabmap_world_T_opengl_world(
+		 0.0f,  0.0f,-1.0f, 0.0f,
+		-1.0f,  0.0f, 0.0f, 0.0f,
+		 0.0f,  1.0f, 0.0f, 0.0f);
 
 inline glm::mat4 glmFromTransform(const rtabmap::Transform & transform)
 {
@@ -173,4 +189,81 @@ public:
 	cv::Mat texture;
 };
 
+typedef enum {
+  /// Not apply any rotation.
+  ROTATION_IGNORED = -1,
+
+  /// 0 degree rotation (natural orientation)
+  ROTATION_0 = 0,
+
+  /// 90 degree rotation.
+  ROTATION_90 = 1,
+
+  /// 180 degree rotation.
+  ROTATION_180 = 2,
+
+  /// 270 degree rotation.
+  ROTATION_270 = 3
+} ScreenRotation;
+
+inline int NormalizedColorCameraRotation(int camera_rotation) {
+  int camera_n = 0;
+  switch (camera_rotation) {
+    case 90:
+      camera_n = 1;
+      break;
+    case 180:
+      camera_n = 2;
+      break;
+    case 270:
+      camera_n = 3;
+      break;
+    default:
+      camera_n = 0;
+      break;
+  }
+  return camera_n;
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @param display: the device display orientation.
+// @param color_camera: integer value of color camera oreintation, values
+// available are 0, 90, 180, 270. Followed by Android camera orientation
+// standard:
+// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
+		ScreenRotation display_rotation, int color_camera_rotation) {
+  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
+
+  int ret = static_cast<int>(display_rotation) - color_camera_n;
+  if (ret < 0) {
+    ret += 4;
+  }
+  return static_cast<ScreenRotation>(ret % 4);
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @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
+// available are 0, 90, 180, 270. Followed by Android camera orientation
+// standard:
+// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
+    int display_rotation, int color_camera_rotation) {
+	ScreenRotation r =
+      static_cast<ScreenRotation>(display_rotation);
+  return GetAndroidRotationFromColorCameraToDisplay(
+      r, color_camera_rotation);
+}
+
+}
+
 #endif /* UTIL_H_ */

app/android/libs/.gitignore

@@ -0,0 +1 @@
+*.jar

app/android/res/layout/activity_rtabmap.xml

@@ -27,6 +27,26 @@
         android:layout_width="wrap_content"
         android:layout_height="match_parent"
         android:fitsSystemWindows="true">
+        
+    <ImageButton
+        android:id="@+id/start_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentBottom="true"
+        android:layout_centerHorizontal="true"
+        android:src="@drawable/start96"
+        android:background="@null"
+        android:layout_marginBottom="10dp" />
+
+    <ImageButton
+        android:id="@+id/stop_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentBottom="true"
+        android:layout_centerHorizontal="true"
+        android:src="@drawable/stop96"
+        android:background="@null"
+        android:layout_marginBottom="10dp" />
     
     <ToggleButton
         android:id="@+id/wireframe_button"
@@ -80,16 +100,6 @@
         android:text="@string/camera_button"
         android:spinnerMode="dropdown"/>
 
-    <ToggleButton
-        android:id="@+id/pause_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-		android:layout_marginRight="5dp"
-        android:layout_alignParentTop="true"
-        android:layout_marginTop="10dp"
-        android:layout_alignParentRight="true"
-        android:textOff="@string/pause"
-        android:textOn="@string/resume" />
 
     <Button
         android:id="@+id/button_shareToSketchfab"
@@ -138,7 +148,22 @@
         android:paddingBottom="10dp"
         android:progressDrawable="@drawable/custom_seekbar" />
 
+    <Button
+        android:id="@+id/button_library"
+        android:layout_width="match_parent"
+        android:layout_height="100dp"
+        android:layout_centerHorizontal="true"
+        android:layout_gravity="top"
+        android:layout_marginTop="100dp"
+        android:text="@string/library" />
 
+    <Button
+        android:id="@+id/button_new_scan"
+        android:layout_width="match_parent"
+        android:layout_height="100dp"
+        android:layout_alignLeft="@+id/button_library"
+        android:layout_below="@+id/button_library"
+        android:text="@string/new_scan" />
 
 </RelativeLayout>        
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -1,5 +1,13 @@
 <PreferenceScreen xmlns:android="http://schemas.android.com/apk/res/android">
     
+    <ListPreference 
+            android:key="@string/pref_key_camera_driver"
+    		android:title="@string/pref_title_camera_driver"
+    		android:summary="@string/pref_summary_camera_driver"
+   			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 
@@ -78,7 +86,7 @@
             android:title="@string/pref_title_mapping_sub"
             android:summary="@string/pref_summary_mapping"
             android:persistent="false">
-        
+                    
 		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_append"
 	            android:title="@string/pref_title_append"
@@ -191,6 +199,20 @@
 		    		android:title="@string/pref_title_optimize_end"
 		    		android:summary="@string/pref_summary_optimize_end"
 		   			android:defaultValue="@string/pref_default_optimize_end"/>
+		   		<ListPreference 
+		            android:key="@string/pref_key_marker_detection"
+		    		android:title="@string/pref_title_marker_detection"
+		    		android:summary="@string/pref_summary_marker_detection"
+		   			android:entries="@array/pref_marker_detection_keys"
+		   			android:entryValues="@array/pref_marker_detection_values"
+		   			android:defaultValue="@string/pref_default_marker_detection"/>
+		   		<ListPreference 
+		            android:key="@string/pref_key_marker_detection_depth_error"
+		    		android:title="@string/pref_title_marker_detection_depth_error"
+		    		android:summary="@string/pref_summary_marker_detection_depth_error"
+		   			android:entries="@array/pref_marker_detection_depth_error_keys"
+		   			android:entryValues="@array/pref_marker_detection_depth_error_values"
+		   			android:defaultValue="@string/pref_default_marker_detection_depth_error"/>
 		    </PreferenceCategory>
 	        <PreferenceCategory
 	        	android:title="@string/pref_title_mapping_database">
@@ -209,6 +231,11 @@
 		            android:title="@string/pref_title_gps_saved"
 		            android:summary="@string/pref_summary_gps_saved"
 		            android:defaultValue="@string/pref_default_gps_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_env_sensors_saved"
+		            android:title="@string/pref_title_env_sensors_saved"
+		            android:summary="@string/pref_summary_env_sensors_saved"
+		            android:defaultValue="@string/pref_default_env_sensors_saved"/>
 		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_db_in_memory"
 		            android:title="@string/pref_title_db_in_memory"

app/android/res/layout/activity_sketchfab.xml

@@ -62,20 +62,11 @@
         android:checked="true"
         android:text="Draft Mode" />
 
-    <LinearLayout
+    <Button
+        android:id="@+id/button_ok"
         android:layout_width="match_parent"
-        android:layout_height="0dip"
-        android:layout_weight="1"
-        android:gravity="right|bottom"
-        android:orientation="horizontal" >
-
-        <Button
-            android:id="@+id/button_ok"
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:layout_weight="0.06"
-            android:text="Upload" />
-    </LinearLayout>
+        android:layout_height="wrap_content"
+        android:text="Upload" />
 
 </LinearLayout>
 </ScrollView>

app/android/res/menu/optionmenu.xml

@@ -7,9 +7,12 @@
 		  <item android:id="@+id/mesh" android:title="Mesh" />
 		  <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
 	   </group>
-	   <item android:id="@+id/save" android:title="Save" android:showAsAction="ifRoom"/>
+	   
+       <item android:id="@+id/new_scan" android:title="New Scan"/>
+       <item android:id="@+id/open" android:title="Library" android:showAsAction="ifRoom"/>
+	   <item android:id="@+id/save" android:title="Save"/>
                      
-       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
+       <item android:id="@+id/export" android:title="Export">
           <menu>
               <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
                   <menu>
@@ -26,7 +29,7 @@
           </menu>
        </item>
        
-        <item android:id="@+id/post_processing" android:title="Optimize" android:showAsAction="ifRoom">
+        <item android:id="@+id/post_processing" android:title="Optimize">
           <menu>
              <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
              <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
@@ -43,8 +46,6 @@
           </menu>
        </item>
        
-       <item android:id="@+id/open" android:title="Open" android:showAsAction="ifRoom"/>
-       
        <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
@@ -68,7 +69,7 @@
 	       </menu>
 	   </item>
 	   <item android:id="@+id/settings" android:title="Settings"/>
-       <item android:id="@+id/reset" android:title="Reset"/>
+       <item android:id="@+id/resume" android:title="Resume"/>
        <item android:id="@+id/about" android:title="About"/>
     </group>
 </menu>

app/android/res/values/dimens.xml

@@ -1,4 +1,3 @@
-
-    <resources>
-        <dimen name="image_width">150dp</dimen>
-    </resources>
+<resources>
+    <dimen name="image_width">150dp</dimen>
+</resources>

app/android/res/values/strings.xml

@@ -9,8 +9,10 @@
     <string name="status">"Status: "</string>
     <string name="words">"Words: "</string>
     <string name="camera_button">First View</string>
-    <string name="pause">Pause</string>
-    <string name="resume">Resume</string>
+    <string name="library">Library</string>
+    <string name="new_scan">New Scan</string>
+    <string name="pause">Stop</string>
+    <string name="resume">Start</string>
     <string name="backface_on">Backface</string>
     <string name="backface_off">Backface</string>
     <string name="light_on">Lighting</string>
@@ -36,6 +38,7 @@
     <string name="fps">"FPS (rendering): "</string>
     <string name="distance">"Distance travelled: "</string>
     <string name="gps">"GPS (long,lat,alt,bearing,err): "</string>
+    <string name="env_sensors">"Sensors: "</string>
     <string name="time">"Time: "</string>
     
     <!-- Preference keys: BEGIN -->
@@ -62,7 +65,7 @@
     <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
     <string name="pref_default_rendering_texture_decimation">4</string>
     <string name="pref_key_blending">pref_key_blending</string>
-    <string name="pref_default_blending">true</string>
+    <string name="pref_default_blending">false</string>
     <string name="pref_key_background_color">pref_key_background_color</string>
     <string name="pref_default_background_color">0.2</string>
     <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>
@@ -76,6 +79,8 @@
     <string name="pref_key_fisheye">pref_key_fisheye</string>
     <string name="pref_default_fisheye">false</string>
     
+    <string name="pref_key_camera_driver">pref_key_camera_driver</string>
+    <string name="pref_default_camera_driver">0</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>
@@ -91,7 +96,7 @@
     <string name="pref_key_min_inliers">pref_key_min_inliers</string>
     <string name="pref_default_min_inliers">25</string>
     <string name="pref_key_opt_error">pref_key_opt_error</string>
-    <string name="pref_default_opt_error">2</string>
+    <string name="pref_default_opt_error">3</string>
     <string name="pref_key_features_voc">pref_key_features_voc</string>
     <string name="pref_default_features_voc">200</string>
     <string name="pref_key_features">pref_key_features</string>
@@ -102,12 +107,18 @@
     <string name="pref_default_optimizer">2</string>
     <string name="pref_key_optimize_end">pref_key_optimize_end</string>
     <string name="pref_default_optimize_end">true</string>
+    <string name="pref_key_marker_detection">pref_key_marker_detection</string>
+    <string name="pref_default_marker_detection">-1</string>
+    <string name="pref_key_marker_detection_depth_error">pref_key_marker_detection_depth_error</string>
+    <string name="pref_default_marker_detection_depth_error">0.1</string>
     <string name="pref_key_keep_all_db">pref_key_keep_all_db</string>
     <string name="pref_default_keep_all_db">true</string>
     <string name="pref_key_raw_scan_saved">pref_key_raw_scan_saved</string>
     <string name="pref_default_raw_scan_saved">false</string>
     <string name="pref_key_gps_saved">pref_key_gps_saved</string>
     <string name="pref_default_gps_saved">false</string>
+    <string name="pref_key_env_sensors_saved">pref_key_env_sensors_saved</string>
+    <string name="pref_default_env_sensors_saved">false</string>
     <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
     <string name="pref_default_db_in_memory">false</string>
     
@@ -310,6 +321,8 @@
     <string name="pref_summary_mapping">Advanced mapping parameters for fine tuning.</string>
     <string name="pref_title_mapping_core">Core</string>
     <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_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>
@@ -344,15 +357,33 @@
     <string name="pref_summary_optimizer">Graph optimization approach.</string>
     <string name="pref_title_optimize_end">Optimization from Graph End</string>
     <string name="pref_summary_optimize_end">The map\'s graph is optimized from the last node. The map is moved when a loop closure happens instead of jumping the current pose back to localized area. </string>
+    <string name="pref_title_marker_detection">ArUco Marker Detection</string>
+    <string name="pref_summary_marker_detection">ArUco markers can be detected for localization and graph optimization.</string>
+    <string name="pref_title_marker_detection_depth_error">Marker Depth Error Estimation</string>
+    <string name="pref_summary_marker_detection_depth_error">Size of markers are automatically initialized on the first marker seen. All markers should have the same size. This value is the maximum depth error to do the initialization to get accurate size of the tag. The lower it is, the more perpendicular the camera should be from the marker to do initialization, but size estimated would be more accurate.</string>
     <string name="pref_title_keep_all_db">Save All Frames in Database</string>
     <string name="pref_summary_keep_all_db">Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop.</string>
     <string name="pref_title_raw_scan_saved">Save Raw Scan</string>
     <string name="pref_summary_raw_scan_saved">Save raw point clouds in database.</string>
     <string name="pref_title_gps_saved">Save GPS</string>
-    <string name="pref_summary_gps_saved">Save GPS in database.</string>
+    <string name="pref_summary_gps_saved">Save GPS to database.</string>
+    <string name="pref_title_env_sensors_saved">Save Environmental Sensors</string>
+    <string name="pref_summary_env_sensors_saved">Save Wifi strength, temperature, air pressure, light intensity and relative humidity to database.</string>
     <string name="pref_title_db_in_memory">Database In Memory</string>
     <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
 
+    <string-array name="pref_camera_driver_keys">
+        <item>"Google Tango NDK"</item>
+        <item>"ARCore NDK"</item>
+        <item>"AREngine NDK"</item>
+        <item>"ARCore Java"</item>
+    </string-array>
+    <string-array name="pref_camera_driver_values">
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+    </string-array>
     <string-array name="pref_update_rate_keys">
         <item>"Max"</item>
         <item>"5 Hz"</item>
@@ -591,6 +622,91 @@
         <item>"1"</item>
         <item>"0"</item>
     </string-array>
+    <string-array name="pref_marker_detection_keys">
+        <item>"Disabled"</item>
+        <item>"4X4_50"</item>
+        <item>"4X4_100"</item>
+        <item>"4X4_250"</item>
+        <item>"4X4_1000"</item>
+        <item>"5X5_50"</item>
+        <item>"5X5_100"</item>
+        <item>"5X5_250"</item>
+        <item>"5X5_1000"</item>
+        <item>"6X6_50"</item>
+        <item>"6X6_100"</item>
+        <item>"6X6_250"</item>
+        <item>"6X6_1000"</item>
+        <item>"7X7_50"</item>
+        <item>"7X7_100"</item>
+        <item>"7X7_250"</item>
+        <item>"7X7_1000"</item>
+        <item>"ARUCO_ORIGINAL"</item>
+        <item>"APRILTAG_16h5"</item>
+        <item>"APRILTAG_25h9"</item>
+        <item>"APRILTAG_36h10"</item>
+        <item>"APRILTAG_36h11"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+        <item>"4"</item>
+        <item>"5"</item>
+        <item>"6"</item>
+        <item>"7"</item>
+        <item>"8"</item>
+        <item>"9"</item>
+        <item>"10"</item>
+        <item>"11"</item>
+        <item>"12"</item>
+        <item>"13"</item>
+        <item>"14"</item>
+        <item>"15"</item>
+        <item>"16"</item>
+        <item>"17"</item>
+        <item>"18"</item>
+        <item>"19"</item>
+        <item>"20"</item>
+    </string-array>
+    
+    <string-array name="pref_marker_detection_depth_error_keys">
+        <item>"1 cm"</item>
+        <item>"2 cm"</item>
+        <item>"3 cm"</item>
+        <item>"4 cm"</item>
+        <item>"5 cm"</item>
+        <item>"6 cm"</item>
+        <item>"7 cm"</item>
+        <item>"8 cm"</item>
+        <item>"9 cm"</item>
+        <item>"10 cm"</item>
+        <item>"15 cm"</item>
+        <item>"20 cm"</item>
+        <item>"30 cm"</item>
+        <item>"40 cm"</item>
+        <item>"50 cm"</item>
+        <item>"100 cm"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_depth_error_values">
+        <item>"0.01"</item>
+        <item>"0.02"</item>
+        <item>"0.03"</item>
+        <item>"0.04"</item>
+        <item>"0.05"</item>
+        <item>"0.06"</item>
+        <item>"0.07"</item>
+        <item>"0.08"</item>
+        <item>"0.09"</item>
+        <item>"0.1"</item>
+        <item>"0.15"</item>
+        <item>"0.20"</item>
+        <item>"0.30"</item>
+        <item>"0.40"</item>
+        <item>"0.50"</item>
+        <item>"1"</item>
+    </string-array>
 
     <string name="pref_title_export_sub">Exporting&#8230;</string>
     <string name="pref_title_export">Exporting</string>

app/android/res/xml/provider_paths.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<paths xmlns:android="http://schemas.android.com/apk/res/android">
+    <external-path name="external_files" path="."/>
+</paths>

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

@@ -0,0 +1,588 @@
+package com.introlab.rtabmap;
+
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.EnumSet;
+import java.util.List;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+import com.google.ar.core.Camera;
+import com.google.ar.core.CameraIntrinsics;
+import com.google.ar.core.Config;
+import com.google.ar.core.Frame;
+import com.google.ar.core.ImageMetadata;
+import com.google.ar.core.PointCloud;
+import com.google.ar.core.Pose;
+import com.google.ar.core.Session;
+import com.google.ar.core.SharedCamera;
+import com.google.ar.core.TrackingState;
+import com.google.ar.core.exceptions.CameraNotAvailableException;
+import com.google.ar.core.exceptions.NotYetAvailableException;
+import com.google.ar.core.exceptions.UnavailableException;
+
+import android.content.Context;
+import android.graphics.ImageFormat;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCaptureSession;
+import android.hardware.camera2.CameraCharacteristics;
+import android.hardware.camera2.CameraDevice;
+import android.hardware.camera2.CameraManager;
+import android.hardware.camera2.CaptureFailure;
+import android.hardware.camera2.CaptureRequest;
+import android.hardware.camera2.TotalCaptureResult;
+import android.media.Image;
+import android.opengl.GLES20;
+import android.opengl.GLSurfaceView;
+import android.os.Handler;
+import android.os.HandlerThread;
+import android.support.annotation.NonNull;
+import android.util.Log;
+import android.view.Surface;
+
+public class ARCoreSharedCamera {
+
+	public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
+
+
+	private static RTABMapActivity mActivity;
+	public ARCoreSharedCamera(RTABMapActivity c) {
+		mActivity = c;
+	}
+
+	// Depth TOF Image.
+	// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
+	private static final int DEPTH_WIDTH = 240;
+	private static final int DEPTH_HEIGHT = 180;
+
+	// GL Surface used to draw camera preview image.
+	public GLSurfaceView surfaceView;
+
+	// ARCore session that supports camera sharing.
+	private Session sharedSession;
+
+	// Camera capture session. Used by both non-AR and AR modes.
+	private CameraCaptureSession captureSession;
+
+	// Reference to the camera system service.
+	private CameraManager cameraManager;
+
+	// Camera device. Used by both non-AR and AR modes.
+	private CameraDevice cameraDevice;
+
+	// Looper handler thread.
+	private HandlerThread backgroundThread;
+	// Looper handler.
+	private Handler backgroundHandler;
+
+	// ARCore shared camera instance, obtained from ARCore session that supports sharing.
+	private SharedCamera sharedCamera;
+
+	// Camera ID for the camera used by ARCore.
+	private String cameraId;
+	
+	private AtomicBoolean mReady = new AtomicBoolean(false);
+
+	// Camera preview capture request builder
+	private CaptureRequest.Builder previewCaptureRequestBuilder;
+
+	private int cameraTextureId = -1;
+
+	// Image reader that continuously processes CPU images.
+	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
+	private boolean mTOFAvailable = false;
+	
+	public boolean isDepthSupported() {return mTOFAvailable;}
+
+	// Camera device state callback.
+	private final CameraDevice.StateCallback cameraDeviceCallback =
+			new CameraDevice.StateCallback() {
+		@Override
+		public void onOpened(@NonNull CameraDevice cameraDevice) {
+			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " opened.");
+			ARCoreSharedCamera.this.cameraDevice = cameraDevice;
+			createCameraPreviewSession();
+		}
+
+		@Override
+		public void onClosed(@NonNull CameraDevice cameraDevice) {
+			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
+			ARCoreSharedCamera.this.cameraDevice = null;
+		}
+
+		@Override
+		public void onDisconnected(@NonNull CameraDevice cameraDevice) {
+			Log.w(TAG, "Camera device ID " + cameraDevice.getId() + " disconnected.");
+			cameraDevice.close();
+			ARCoreSharedCamera.this.cameraDevice = null;
+		}
+
+		@Override
+		public void onError(@NonNull CameraDevice cameraDevice, int error) {
+			Log.e(TAG, "Camera device ID " + cameraDevice.getId() + " error " + error);
+			cameraDevice.close();
+			ARCoreSharedCamera.this.cameraDevice = null;
+		}
+	};
+
+	// Repeating camera capture session state callback.
+	CameraCaptureSession.StateCallback cameraCaptureCallback =
+			new CameraCaptureSession.StateCallback() {
+
+		// Called when the camera capture session is first configured after the app
+		// is initialized, and again each time the activity is resumed.
+		@Override
+		public void onConfigured(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session configured.");
+			captureSession = session;
+			setRepeatingCaptureRequest();
+		}
+
+		@Override
+		public void onSurfacePrepared(
+				@NonNull CameraCaptureSession session, @NonNull Surface surface) {
+			Log.d(TAG, "Camera capture surface prepared.");
+		}
+
+		@Override
+		public void onReady(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session ready.");
+		}
+
+		@Override
+		public void onActive(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session active.");
+			resumeARCore();
+		}
+
+		@Override
+		public void onClosed(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session closed.");
+		}
+
+		@Override
+		public void onConfigureFailed(@NonNull CameraCaptureSession session) {
+			Log.e(TAG, "Failed to configure camera capture session.");
+		}
+	};
+
+	// Repeating camera capture session capture callback.
+	private final CameraCaptureSession.CaptureCallback captureSessionCallback =
+			new CameraCaptureSession.CaptureCallback() {
+
+		@Override
+		public void onCaptureCompleted(
+				@NonNull CameraCaptureSession session,
+				@NonNull CaptureRequest request,
+				@NonNull TotalCaptureResult result) {
+			Log.i(TAG, "onCaptureCompleted");
+		}
+
+		//@Override // android 23 
+		public void onCaptureBufferLost(
+				@NonNull CameraCaptureSession session,
+				@NonNull CaptureRequest request,
+				@NonNull Surface target,
+				long frameNumber) {
+			Log.e(TAG, "onCaptureBufferLost: " + frameNumber);
+		}
+
+		@Override
+		public void onCaptureFailed(
+				@NonNull CameraCaptureSession session,
+				@NonNull CaptureRequest request,
+				@NonNull CaptureFailure failure) {
+			Log.e(TAG, "onCaptureFailed: " + failure.getFrameNumber() + " " + failure.getReason());
+		}
+
+		@Override
+		public void onCaptureSequenceAborted(
+				@NonNull CameraCaptureSession session, int sequenceId) {
+			Log.e(TAG, "onCaptureSequenceAborted: " + sequenceId + " " + session);
+		}
+	};
+
+	private void resumeARCore() {
+		// Ensure that session is valid before triggering ARCore resume. Handles the case where the user
+		// manually uninstalls ARCore while the app is paused and then resumes.
+		if (sharedSession == null) {
+			return;
+		}
+
+		try {
+			Log.i(TAG, "Resume ARCore.");
+			// Resume ARCore.
+			sharedSession.resume();
+			// Set capture session callback while in AR mode.
+			sharedCamera.setCaptureCallback(captureSessionCallback, backgroundHandler);
+		} catch (CameraNotAvailableException e) {
+			Log.e(TAG, "Failed to resume ARCore session", e);
+			return;
+		}
+
+	}
+
+
+	// Called when starting non-AR mode or switching to non-AR mode.
+	// Also called when app starts in AR mode, or resumes in AR mode.
+	private void setRepeatingCaptureRequest() {
+		try {
+			captureSession.setRepeatingRequest(
+					previewCaptureRequestBuilder.build(), captureSessionCallback, backgroundHandler);
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "Failed to set repeating request", e);
+		}
+	}
+
+
+	private void createCameraPreviewSession() {
+		Log.e(TAG, "createCameraPreviewSession: " + "starting camera preview session.");
+		try {
+			// Note that isGlAttached will be set to true in AR mode in onDrawFrame().
+			sharedSession.setCameraTextureName(cameraTextureId);
+
+			// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
+			previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
+
+			// Build surfaces list, starting with ARCore provided surfaces.
+			List<Surface> surfaceList = sharedCamera.getArCoreSurfaces();
+			Log.e(TAG, " createCameraPreviewSession: " + "surfaceList: sharedCamera.getArCoreSurfaces(): " + surfaceList.size());
+
+			// Add a CPU image reader surface. On devices that don't support CPU image access, the image
+			// may arrive significantly later, or not arrive at all.
+			if (mTOFAvailable) surfaceList.add(mTOFImageReader.imageReader.getSurface());
+			// Surface list should now contain three surfacemReadymReadys:
+			// 0. sharedCamera.getSurfaceTexture()
+			// 1. …
+			// 2. depthImageReader.getSurface()
+
+			// Add ARCore surfaces and CPU image surface targets.
+			for (Surface surface : surfaceList) {
+				previewCaptureRequestBuilder.addTarget(surface);
+			}
+
+			// Wrap our callback in a shared camera callback.
+			CameraCaptureSession.StateCallback wrappedCallback = sharedCamera.createARSessionStateCallback(cameraCaptureCallback, backgroundHandler);
+
+			// Create camera capture session for camera preview using ARCore wrapped callback.
+			cameraDevice.createCaptureSession(surfaceList, wrappedCallback, backgroundHandler);
+			
+			mReady.set(true);
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "CameraAccessException", e);
+		}
+
+
+	}
+
+	// Start background handler thread, used to run callbacks without blocking UI thread.
+	private void startBackgroundThread() {
+		backgroundThread = new HandlerThread("sharedCameraBackground");
+		backgroundThread.start();
+		backgroundHandler = new Handler(backgroundThread.getLooper());
+		mTOFImageReader.startBackgroundThread();
+	}
+
+	// Stop background handler thread.
+	private void stopBackgroundThread() {
+		if (backgroundThread != null) {
+			backgroundThread.quitSafely();
+			try {
+				backgroundThread.join();
+				backgroundThread = null;
+				backgroundHandler = null;
+			} catch (InterruptedException e) {
+				Log.e(TAG, "Interrupted while trying to join background handler thread", e);
+			}
+		}
+		mTOFImageReader.stopBackgroundThread();
+	}
+	
+	private long mPreviousTime = 0;
+
+	// Perform various checks, then open camera device and create CPU image reader.
+	public boolean openCamera() {
+
+		close();
+		
+		startBackgroundThread();
+		
+		mPreviousTime = System.currentTimeMillis();
+
+		if(cameraTextureId == -1)
+		{
+			int[] textures = new int[1];
+			GLES20.glGenTextures(1, textures, 0);
+			cameraTextureId = textures[0];
+		}
+
+		Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
+		// Don't open camera if already opened.
+		if (cameraDevice != null) {
+			return false;
+		}
+
+		if (sharedSession == null) {
+			try {
+				// Create ARCore session that supports camera sharing.
+				sharedSession = new Session(mActivity, EnumSet.of(Session.Feature.SHARED_CAMERA));
+			} catch (UnavailableException e) {
+				Log.e(TAG, "Failed to create ARCore session that supports camera sharing", e);
+				return false;
+			}
+
+			// Enable auto focus mode while ARCore is running.
+			Config config = sharedSession.getConfig();
+			config.setFocusMode(Config.FocusMode.FIXED);
+			config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
+			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
+			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
+			//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
+			sharedSession.configure(config);
+
+		}
+
+		// Store the ARCore shared camera reference.
+		sharedCamera = sharedSession.getSharedCamera();
+		// Store the ID of the camera used by ARCore.
+		cameraId = sharedSession.getCameraConfig().getCameraId();
+		initCamera(mActivity, cameraId, 1);
+		ArrayList<String> resolutions;
+
+		mTOFAvailable = false;
+
+		resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
+		if (resolutions != null) {
+			for( String temp : resolutions) {
+				Log.e(TAG + "DEPTH16 resolution: ", temp);
+			};
+			if (resolutions.size()>0) mTOFAvailable = true;
+		}
+
+		// Color CPU Image.
+		// Use the currently configured CPU image size.
+		//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
+
+		if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
+
+		// When ARCore is running, make sure it also updates our CPU image surface.
+		if (mTOFAvailable) {
+			sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
+		}
+
+		try {
+
+			// Wrap our callback in a shared camera callback.
+			CameraDevice.StateCallback wrappedCallback = sharedCamera.createARDeviceStateCallback(cameraDeviceCallback, backgroundHandler);
+
+			// Store a reference to the camera system service.
+			cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
+
+			// Get the characteristics for the ARCore camera.
+			//CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(this.cameraId);
+
+			// Open the camera device using the ARCore wrapped callback.
+			cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
+			
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "Failed to open camera", e);
+			return false;
+		} catch (IllegalArgumentException e) {
+			Log.e(TAG, "Failed to open camera", e);
+			return false;
+		} catch (SecurityException e) {
+			Log.e(TAG, "Failed to open camera", e);
+			return false;
+		} 
+
+		Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
+		return true;
+	}
+
+
+	// Close the camera device.
+	public void close() {
+		
+		if (sharedSession != null)  {
+			sharedSession.pause();
+		}
+
+		if (captureSession != null) {
+			captureSession.close();
+			captureSession = null;
+		}
+		if (cameraDevice != null) {
+			cameraDevice.close();
+		}
+
+		if (mTOFImageReader.imageReader != null) {
+			mTOFImageReader.imageReader.close();
+			mTOFImageReader.imageReader = null;
+		}
+
+		if(cameraTextureId>=0)
+		{
+			GLES20.glDeleteTextures(1, new int[] {cameraTextureId}, 0);
+		}
+		
+		stopBackgroundThread();
+	}
+
+	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
+
+	// Draw frame when in AR mode. Called on the GL thread.
+	public void updateGL() throws CameraNotAvailableException {
+
+		if(!mReady.get())
+		{
+			return;
+		}
+		
+		if (mTOFAvailable && mTOFImageReader.frameCount == 0) return;
+		
+		// Perform ARCore per-frame update.
+		Frame frame = null;
+		try {
+			frame = sharedSession.update();
+		} catch (Exception e) {
+			e.printStackTrace();
+			return;
+		}
+
+		Camera camera = null;
+		if (frame != null) {
+			camera = frame.getCamera();
+		}else
+		{
+			return;
+		}
+
+		if (camera == null) return;
+		// If not tracking, don't draw 3D objects.
+		if (camera.getTrackingState() == TrackingState.PAUSED) return;
+
+		if (frame.getTimestamp() != 0) {
+			
+			Pose pose = camera.getPose();
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
+			
+			int rateMs = 100; // send images at most 10 Hz
+			if(System. currentTimeMillis() - mPreviousTime < rateMs)
+			{
+				return;
+			}
+			mPreviousTime = System. currentTimeMillis();
+			
+			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
+			try{
+				Image image = frame.acquireCameraImage();
+				PointCloud cloud = frame.acquirePointCloud();
+				FloatBuffer points = cloud.getPoints();
+
+				if (image.getFormat() != ImageFormat.YUV_420_888) {
+					throw new IllegalArgumentException(
+							"Expected image in YUV_420_888 format, got format " + image.getFormat());
+				}
+				
+				if(!RTABMapActivity.DISABLE_LOG)
+				{
+					for(int i =0;i<image.getPlanes().length;++i)
+					{
+						Log.d(TAG, String.format("Plane[%d] pixel stride = %d,  row stride = %d", i, image.getPlanes()[i].getPixelStride(), image.getPlanes()[i].getRowStride()));
+					}
+				}
+				
+				float[] fl = intrinsics.getFocalLength();
+				float[] pp = intrinsics.getPrincipalPoint();
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("fx=%f fy=%f cx=%f cy=%f", fl[0], fl[1], pp[0], pp[1]));
+				
+				ByteBuffer y = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+				ByteBuffer u = image.getPlanes()[1].getBuffer().asReadOnlyBuffer();
+				ByteBuffer v = image.getPlanes()[2].getBuffer().asReadOnlyBuffer();
+
+				double stamp = (double)image.getTimestamp()/10e8;
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
+						image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
+
+				if(mTOFAvailable)
+				{
+					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
+					
+					RTABMapLib.postOdometryEvent(
+							RTABMapActivity.nativeApplication,
+							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], stamp, 
+							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							points, points.limit()/4);
+				}
+				else
+				{
+					ByteBuffer bb = ByteBuffer.allocate(0);
+					RTABMapLib.postOdometryEvent(
+							RTABMapActivity.nativeApplication,
+							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], stamp, 
+							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							bb, 0, 0, 0, ImageFormat.DEPTH16,
+							points, points.limit()/4);
+				}
+				
+				image.close();
+				cloud.close();
+				
+			} catch (NotYetAvailableException e) {
+
+			}
+		}
+	}
+
+	/********************************************************************************************************************* */
+	/*************************************************** End ************************************************************* */
+	/********************************************************************************************************************* */
+
+
+	public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
+		Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
+
+		ArrayList<String> output = new ArrayList<String>();
+		try {
+			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
+			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
+
+			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(imageFormat)) {
+				output.add(s.getWidth() + "x" + s.getHeight());
+			}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
+		return output;
+	}
+
+
+	public void initCamera (Context context, String cameraId,int index){
+		boolean ok = false;
+		try {
+			int current = 0;
+			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
+			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
+			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.DEPTH16)) {
+				ok = true;
+				if (current == index)
+					break;
+				else ;
+				current++;
+			}
+		} catch (Exception e) {
+			e.printStackTrace();
+		}
+		if (!ok) {
+			Log.e(TAG + " initCamera", "Depth sensor not found!");
+		}
+	}
+
+
+}

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

@@ -0,0 +1,79 @@
+/*
+ * 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.
+ */
+package com.introlab.rtabmap;
+
+import android.Manifest;
+
+import android.app.Activity;
+import android.content.Intent;
+import android.content.pm.PackageManager;
+import android.net.Uri;
+import android.provider.Settings;
+import android.support.v4.app.ActivityCompat;
+import android.support.v4.content.ContextCompat;
+
+/** Helper to ask camera permission. */
+public class PermissionHelper {
+	public static final int CAMERA_CODE = 0;
+	public static final int READ_EXTERNAL_STORAGE_CODE = 1;
+	public static final int WRITE_EXTERNAL_STORAGE_CODE = 2;
+	public static final int INTERNET_CODE = 3;
+	public static final int ACCESS_NETWORK_STATE_CODE = 4;
+	public static final int ACCESS_FINE_LOCATION_CODE = 5;
+	public static final int ACCESS_WIFI_STATE_CODE = 6;
+
+	/** Check to see we have the necessary permissions for this app. */
+	public static boolean hasPermission(Activity activity, String permission) {
+		return ContextCompat.checkSelfPermission(activity, permission) == PackageManager.PERMISSION_GRANTED;
+	}
+
+	/** Check to see we have the necessary permissions for this app, and ask for them if we don't. */
+	public static void requestPermission(Activity activity, String permission) {
+		int requestCode = -1;
+		if (permission == Manifest.permission.CAMERA) {
+			requestCode = CAMERA_CODE;
+		} else if(permission == Manifest.permission.READ_EXTERNAL_STORAGE) {
+			requestCode = READ_EXTERNAL_STORAGE_CODE;
+		} else if(permission == Manifest.permission.WRITE_EXTERNAL_STORAGE) {
+			requestCode = WRITE_EXTERNAL_STORAGE_CODE;
+		} else if(permission == Manifest.permission.INTERNET) {
+			requestCode = INTERNET_CODE;
+		} else if(permission == Manifest.permission.ACCESS_NETWORK_STATE) {
+			requestCode = ACCESS_NETWORK_STATE_CODE;
+		} else if(permission == Manifest.permission.ACCESS_FINE_LOCATION) {
+			requestCode = ACCESS_FINE_LOCATION_CODE;
+		} else if(permission == Manifest.permission.ACCESS_WIFI_STATE) {
+			requestCode = ACCESS_WIFI_STATE_CODE;
+		}
+		if(requestCode >=0)
+		{
+			ActivityCompat.requestPermissions(
+					activity, new String[] {permission}, requestCode);
+		}
+	}
+
+	/** Check to see if we need to show the rationale for this permission. */
+	public static boolean shouldShowRequestPermissionRationale(Activity activity, String permission) {
+		return ActivityCompat.shouldShowRequestPermissionRationale(activity, permission);
+	}
+
+	/** Launch Application Setting to grant permission. */
+	public static void launchPermissionSettings(Activity activity) {
+		Intent intent = new Intent();
+		intent.setAction(Settings.ACTION_APPLICATION_DETAILS_SETTINGS);
+		intent.setData(Uri.fromParts("package", activity.getPackageName(), null));
+		activity.startActivity(intent);
+	}
+}

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

@@ -1,5 +1,10 @@
 
 package com.introlab.rtabmap;
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+
+import android.app.Activity;
+import android.content.Context;
 import android.os.IBinder;
 import android.view.KeyEvent;
 import android.util.Log;
@@ -15,7 +20,7 @@ public class RTABMapLib
         // the correct library first.
         if (TangoInitializationHelper.loadTangoSharedLibrary() ==
                 TangoInitializationHelper.ARCH_ERROR) {
-            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+            Log.w(RTABMapActivity.class.getSimpleName(), "WArning! Unable to load libtango_client_api.so! This can be safely ignored if RTAB-Map NDK is not build with tango support.");
         }
         System.loadLibrary("NativeRTABMap");
     }
@@ -23,87 +28,84 @@ public class RTABMapLib
  // Initialize the Tango Service, this function starts the communication
     // between the application and Tango Service.
     // The activity object is used for checking if the API version is outdated.
-    public static native void onCreate(RTABMapActivity activity);
+    public static native long createNativeApplication(RTABMapActivity activity);
     
-    public static native void setScreenRotation(int displayRotation, int cameraRotation);
+    public static native void destroyNativeApplication(long nativeApplication);
     
-    public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
-    public static native int openDatabase2(String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
     
-    /*
-     * Called when the Tango service is connected.
-     *
-     * @param binder The native binder object.
-     */
-    public static native boolean onTangoServiceConnected(IBinder binder);
-
-    // Release all non OpenGl resources that are allocated from the program.
-    public static native void onPause();
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase2(long nativeApplication, String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
+    
+    public static native boolean isBuiltWith(long nativeApplication, int cameraDriver);
+    public static native boolean startCamera(long nativeApplication, IBinder binder, Context context, Activity activity, int driver);
+    public static native void stopCamera(long nativeApplication);
 
     // Allocate OpenGL resources for rendering.
-    public static native void initGlContent();
+    public static native void initGlContent(long nativeApplication);
 
     // Setup the view port width and height.
-    public static native void setupGraphic(int width, int height);
+    public static native void setupGraphic(long nativeApplication, int width, int height);
 
     // Main render loop.
-    public static native int render();
+    public static native int render(long nativeApplication);
 
     // Set the render camera's viewing angle:
     //   first person, third person, or top down.
-    public static native void setCamera(int cameraIndex);
+    public static native void setCamera(long nativeApplication, int cameraIndex);
     
     // Pass touch events to the native layer.
-    public static native void onTouchEvent(int touchCount, int event0,
+    public static native void onTouchEvent(long nativeApplication, int touchCount, int event0,
                                            float x0, float y0, float x1, float y1);
     
-
-    public static native void setPausedMapping(boolean paused);
-    public static native void setOnlineBlending(boolean enabled);
-    public static native void setMapCloudShown(boolean shown);
-    public static native void setOdomCloudShown(boolean shown);
-    public static native void setMeshRendering(boolean enabled, boolean withTexture);
-    public static native void setLocalizationMode(boolean enabled);
-    public static native void setTrajectoryMode(boolean enabled);
-    public static native void setGraphOptimization(boolean enabled);
-    public static native void setNodesFiltering(boolean enabled);
-    public static native void setGraphVisible(boolean visible);
-    public static native void setGridVisible(boolean visible);
-    public static native void setRawScanSaved(boolean enabled);
-    public static native void setFullResolution(boolean enabled);
-    public static native void setSmoothing(boolean enabled);
-    public static native void setCameraColor(boolean enabled);
-    public static native void setAppendMode(boolean enabled);
-    public static native void setDataRecorderMode(boolean enabled);
-    public static native void setMaxCloudDepth(float value);
-    public static native void setMinCloudDepth(float value);
-    public static native void setPointSize(float value);
-    public static native void setFOV(float value);
-    public static native void setOrthoCropFactor(float value);
-    public static native void setGridRotation(float value);
-    public static native void setLighting(boolean enabled);
-    public static native void setBackfaceCulling(boolean enabled);
-    public static native void setWireframe(boolean enabled);
-    public static native void setCloudDensityLevel(int value);
-    public static native void setMeshAngleTolerance(float value);
-    public static native void setMeshTriangleSize(int value);
-    public static native void setClusterRatio(float value);
-    public static native void setMaxGainRadius(float value);
-    public static native void setRenderingTextureDecimation(int value);
-    public static native void setBackgroundColor(float gray);
-    public static native int setMappingParameter(String key, String value);
+    public static native void setPausedMapping(long nativeApplication, boolean paused);
+    public static native void setOnlineBlending(long nativeApplication, boolean enabled);
+    public static native void setMapCloudShown(long nativeApplication, boolean shown);
+    public static native void setOdomCloudShown(long nativeApplication, boolean shown);
+    public static native void setMeshRendering(long nativeApplication, boolean enabled, boolean withTexture);
+    public static native void setLocalizationMode(long nativeApplication, boolean enabled);
+    public static native void setTrajectoryMode(long nativeApplication, boolean enabled);
+    public static native void setGraphOptimization(long nativeApplication, boolean enabled);
+    public static native void setNodesFiltering(long nativeApplication, boolean enabled);
+    public static native void setGraphVisible(long nativeApplication, boolean visible);
+    public static native void setGridVisible(long nativeApplication, boolean visible);
+    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 setCameraColor(long nativeApplication, boolean enabled);
+    public static native void setAppendMode(long nativeApplication, boolean enabled);
+    public static native void setDataRecorderMode(long nativeApplication, boolean enabled);
+    public static native void setMaxCloudDepth(long nativeApplication, float value);
+    public static native void setMinCloudDepth(long nativeApplication, float value);
+    public static native void setPointSize(long nativeApplication, float value);
+    public static native void setFOV(long nativeApplication, float value);
+    public static native void setOrthoCropFactor(long nativeApplication, float value);
+    public static native void setGridRotation(long nativeApplication, float value);
+    public static native void setLighting(long nativeApplication, boolean enabled);
+    public static native void setBackfaceCulling(long nativeApplication, boolean enabled);
+    public static native void setWireframe(long nativeApplication, boolean enabled);
+    public static native void setCloudDensityLevel(long nativeApplication, int value);
+    public static native void setMeshAngleTolerance(long nativeApplication, float value);
+    public static native void setMeshTriangleSize(long nativeApplication, int value);
+    public static native void setClusterRatio(long nativeApplication, float value);
+    public static native void setMaxGainRadius(long nativeApplication, float value);
+    public static native void setRenderingTextureDecimation(long nativeApplication, int value);
+    public static native void setBackgroundColor(long nativeApplication, float gray);
+    public static native int setMappingParameter(long nativeApplication, String key, String value);
     public static native void setGPS(
+    		long nativeApplication, 
 			double stamp,
 			double longitude, 
 			double latitude, 
 			double altitude,  
 			double accuracy,
 			double bearing);
+    public static native void addEnvSensor(long nativeApplication, int type, float value);
 
-    public static native void resetMapping();
-    public static native void save(String outputDatabasePath);
-    public static native void cancelProcessing();
+    public static native void save(long nativeApplication, String outputDatabasePath);
+    public static native void cancelProcessing(long nativeApplication);
     public static native boolean exportMesh(
+    		long nativeApplication, 
     		float cloudVoxelSize,
     		boolean regenerateCloud,
     		boolean meshing,
@@ -120,15 +122,24 @@ public class RTABMapLib
     		float optimizedMaxTextureDistance,
     		int optimizedMinTextureClusterSize,
     		boolean blockRendering);
-    public static native boolean writeExportedMesh(String directory, String name);
-    public static native boolean postExportation(boolean visualize);
-    public static native int postProcessing(int approach);
+    public static native boolean writeExportedMesh(long nativeApplication, String directory, String name);
+    public static native boolean postExportation(long nativeApplication, boolean visualize);
+    public static native int postProcessing(long nativeApplication, int approach);
     
-    public static native String getStatus();
-    public static native int getTotalNodes();
-    public static native int getTotalWords();
-    public static native int getTotalPoints();
-    public static native float getUpdateTime();
-    public static native int getLoopClosureId();
+    public static native String getStatus(long nativeApplication);
+    public static native int getTotalNodes(long nativeApplication);
+    public static native int getTotalWords(long nativeApplication);
+    public static native int getTotalPoints(long nativeApplication);
+    public static native float getUpdateTime(long nativeApplication);
+    public static native int getLoopClosureId(long nativeApplication);
+    
+    public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw);
+    public static native void postOdometryEvent(long nativeApplication,
+    		float x, float y, float z, float qx, float qy, float qz, float qw,
+    		float fx, float fy, float cx, float cy,
+    		double stamp, 
+    		ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+    		ByteBuffer depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+    		FloatBuffer points, int pointsLen);
     
 }

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

@@ -43,6 +43,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private float mSurfaceHeight = 0.0f;
 	private float mTextColor = 1.0f;
 	private int mOffset = 0;
+	private ARCoreSharedCamera mCamera = null;
 	
 	private Vector<TextObject> mTexts;
 
@@ -71,91 +72,109 @@ public class Renderer implements GLSurfaceView.Renderer {
 	{
 		mOffset = offset;
 	}
+	
+	public void setCamera(ARCoreSharedCamera camera)
+	{
+		mCamera = camera;
+	}
 
 	// Render loop of the Gl context.
 	public void onDrawFrame(GL10 useGLES20instead) {
 
-		try
-		{
-			final int value = RTABMapLib.render();
-
-			if(mTextManager!=null)
+		synchronized (this) {
+			if(mActivity.nativeApplication != 0)
 			{
-				if(mTextChanged)
+				try
 				{
-					mTextChanged = false;
-					Vector<TextObject> txtcollection = new Vector<TextObject>();
-					
-					mTextLock.lock();
-				    try {
-				    	if(mTexts.size() > 0)
-				    	{
-				    		txtcollection.addAll(mTexts);
-				    	}
-				    } finally {
-				    	mTextLock.unlock();
-				    }
-				    
-					// Prepare the text for rendering
-					mTextManager.PrepareDraw(txtcollection);
-				}
-				
-				float[] mvp = new float[16];
-				Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
-				mTextManager.Draw(mvp);
-			}
-
-			if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
-						mProgressDialog.dismiss();
-						mActivity.resetNoTouchTimer();
-					}
-				});
-			}
-			if(value==-1)
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(mToast!=null)
-						{
-							mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
-						}
-					}
-				});
-			}
-			else if(value==-2)
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(mToast!=null)
-						{
-							mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
-						}
-					}
-				});
-			}
-		}
-		catch(final Exception e)
-		{
-			mActivity.runOnUiThread(new Runnable() {
-				public void run() {
-					if(mToast!=null)
+					if(mCamera!=null)
 					{
-						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+						mCamera.updateGL();
+					}
+					
+					final int value = RTABMapLib.render(mActivity.nativeApplication);
+		
+					if(mTextManager!=null)
+					{
+						if(mTextChanged)
+						{
+							mTextChanged = false;
+							Vector<TextObject> txtcollection = new Vector<TextObject>();
+							
+							mTextLock.lock();
+						    try {
+						    	if(mTexts.size() > 0)
+						    	{
+						    		txtcollection.addAll(mTexts);
+						    	}
+						    } finally {
+						    	mTextLock.unlock();
+						    }
+						    
+							// Prepare the text for rendering
+							mTextManager.PrepareDraw(txtcollection);
+						}
+						
+						float[] mvp = new float[16];
+						Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
+						mTextManager.Draw(mvp);
+					}
+		
+					if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
+								mProgressDialog.dismiss();
+								mActivity.resetNoTouchTimer();
+							}
+						});
+					}
+					if(value==-1)
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(mToast!=null)
+								{
+									mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+								}
+							}
+						});
+					}
+					else if(value==-2)
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(mToast!=null)
+								{
+									mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+								}
+							}
+						});
 					}
 				}
-
-			});
+				catch(final Exception e)
+				{
+					mActivity.runOnUiThread(new Runnable() {
+						public void run() {
+							if(mToast!=null)
+							{
+								mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+							}
+						}
+		
+					});
+				}
+			}
 		}
 	}
 
 	// Called when the surface size changes.
 	public void onSurfaceChanged(GL10 useGLES20instead, int width, int height) {
 		
-		RTABMapLib.setupGraphic(width, height);
+		if(mActivity.nativeApplication!=0)
+		{
+			RTABMapLib.setupGraphic(mActivity.nativeApplication, width, height);
+		}
 		
 		mSurfaceHeight = (float)height;
 
@@ -180,7 +199,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 	// Called when the surface is created or recreated.
 	public void onSurfaceCreated(GL10 useGLES20instead, EGLConfig config) {
 
-		RTABMapLib.initGlContent();
+		if(mActivity.nativeApplication != 0)
+		{
+			RTABMapLib.initGlContent(mActivity.nativeApplication);
+		}
 		
 		// Create our text manager
 		mTextManager = new TextManager(mActivity);

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

@@ -7,11 +7,14 @@ import java.util.Arrays;
 import java.util.Iterator;
 import java.util.Map.Entry;
 
+import android.Manifest;
 import android.app.AlertDialog;
 import android.content.DialogInterface;
 import android.content.SharedPreferences;
 import android.content.SharedPreferences.OnSharedPreferenceChangeListener;
+import android.content.pm.PackageManager;
 import android.os.Bundle;
+import android.preference.CheckBoxPreference;
 import android.preference.ListPreference;
 import android.preference.Preference;
 import android.preference.PreferenceActivity;
@@ -19,6 +22,7 @@ import android.text.InputType;
 import android.view.WindowManager;
 import android.view.inputmethod.EditorInfo;
 import android.widget.EditText;
+import android.widget.Toast;
 
 public class SettingsActivity extends PreferenceActivity implements OnSharedPreferenceChangeListener {
 	
@@ -34,7 +38,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	@Override
         	public boolean onPreferenceClick(Preference preference) {   
         		getPreferenceScreen().getSharedPreferences().edit().clear().commit();
-        		
+        	
                 recreate();
         		
         		return true;
@@ -184,7 +188,31 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	}
         });
         
+        Preference buttonGPS = findPreference(getString(R.string.pref_key_gps_saved));
+        buttonGPS.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {  
+        		if(((CustomSwitchPreference)preference).isChecked())
+        		{
+	        		if (!PermissionHelper.hasPermission(getActivity(), Manifest.permission.ACCESS_FINE_LOCATION)) {
+	        			((CustomSwitchPreference)preference).setChecked(false);
+	        			PermissionHelper.requestPermission(getActivity(), Manifest.permission.ACCESS_FINE_LOCATION);
+	        			return false;
+	        		}
+        		}
+        		return true;
+        	}
+        });
+        if(((CustomSwitchPreference)buttonGPS).isChecked())
+		{
+    		if (!PermissionHelper.hasPermission(this, Manifest.permission.ACCESS_FINE_LOCATION)) {
+    			((CustomSwitchPreference)buttonGPS).setChecked(false);
+    			PermissionHelper.requestPermission(this, Manifest.permission.ACCESS_FINE_LOCATION);
+    		}
+		}
         
+        
+        ((Preference)findPreference(getString(R.string.pref_key_camera_driver))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_camera_driver))).getEntry() + ") "+getString(R.string.pref_summary_camera_driver));
         ((Preference)findPreference(getString(R.string.pref_key_density))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_density))).getEntry() + ") "+getString(R.string.pref_summary_density));
         ((Preference)findPreference(getString(R.string.pref_key_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth))).getEntry() + ") "+getString(R.string.pref_summary_depth));
         ((Preference)findPreference(getString(R.string.pref_key_min_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getEntry() + ") "+getString(R.string.pref_summary_min_depth));
@@ -206,6 +234,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_features))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features))).getEntry() + ") "+getString(R.string.pref_summary_features));
         ((Preference)findPreference(getString(R.string.pref_key_features_type))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_type))).getEntry() + ") "+getString(R.string.pref_summary_features_type));
         ((Preference)findPreference(getString(R.string.pref_key_optimizer))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_optimizer))).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        ((Preference)findPreference(getString(R.string.pref_key_marker_detection))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_marker_detection))).getEntry() + ") "+getString(R.string.pref_summary_marker_detection));
+        ((Preference)findPreference(getString(R.string.pref_key_marker_detection_depth_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_marker_detection_depth_error))).getEntry() + ") "+getString(R.string.pref_summary_marker_detection_depth_error));
         
         ((Preference)findPreference(getString(R.string.pref_key_cloud_voxel))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cloud_voxel))).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         ((Preference)findPreference(getString(R.string.pref_key_texture_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_size))).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
@@ -226,6 +256,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         Preference pref = findPreference(key);
 
         if (pref instanceof ListPreference) {
+        	if(key.compareTo(getString(R.string.pref_key_camera_driver))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_camera_driver));
         	if(key.compareTo(getString(R.string.pref_key_density))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_density));
         	if(key.compareTo(getString(R.string.pref_key_depth))==0) 
     		{
@@ -265,6 +296,8 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_features))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features));
         	if(key.compareTo(getString(R.string.pref_key_features_type))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_type));
         	if(key.compareTo(getString(R.string.pref_key_optimizer))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        	if(key.compareTo(getString(R.string.pref_key_marker_detection))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_marker_detection));
+        	if(key.compareTo(getString(R.string.pref_key_marker_detection_depth_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_marker_detection_depth_error));
         	
         	if(key.compareTo(getString(R.string.pref_key_cloud_voxel))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         	if(key.compareTo(getString(R.string.pref_key_texture_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
@@ -327,4 +360,28 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
 		}
 		ed.commit(); //save it.
     }
+    
+    @Override
+	public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
+
+		switch (requestCode) {
+			case PermissionHelper.ACCESS_FINE_LOCATION_CODE: {
+				// If request is cancelled, the result arrays are empty.
+				if (results.length > 0 && results[0] == PackageManager.PERMISSION_GRANTED) {
+					// permission was granted, yay! Do the
+					// contacts-related task you need to do.
+					Preference buttonGPS = findPreference(getString(R.string.pref_key_gps_saved));
+					((CustomSwitchPreference)buttonGPS).setChecked(true);
+				} else {
+					// permission denied, boo! Disable the
+					// functionality that depends on this permission.
+					Toast.makeText(this, "Location permission is needed to use GPS functionality", Toast.LENGTH_LONG).show();
+					if (!PermissionHelper.shouldShowRequestPermissionRationale(this, Manifest.permission.ACCESS_FINE_LOCATION)) {
+						PermissionHelper.launchPermissionSettings(this); // Permission denied with checking "Do not ask again".
+					}
+				}
+				return;
+			}
+		}
+	}
 }

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

@@ -66,6 +66,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		mButtonOk = (Button)findViewById(R.id.button_ok);
 		
 		mProgressDialog = new ProgressDialog(this);
+		mProgressDialog.setCancelable(false);
 		mProgressDialog.setCanceledOnTouchOutside(false);
 		
 		mAuthToken = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY);
@@ -142,7 +143,8 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		if(!isNetworkAvailable())
 		{
 			// Visualize the result?
-			new AlertDialog.Builder(this)
+			AlertDialog ad = new AlertDialog.Builder(this)
+			.setCancelable(false)
 			.setTitle("Sharing to Sketchfab...")
 			.setMessage("Network is not available. Make sure you have internet before continuing.")
 			.setPositiveButton("Try Again", new DialogInterface.OnClickListener() {
@@ -154,7 +156,9 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 				public void onClick(DialogInterface dialog, int which) {
 				}
 			})
-			.show();
+			.create();
+			ad.setCanceledOnTouchOutside(false);
+			ad.show();
 			return;
 		}	
 
@@ -165,6 +169,8 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 
 			WebView web;
 			mAuthDialog = new Dialog(this);
+			mAuthDialog.setCancelable(true);
+			mAuthDialog.setCanceledOnTouchOutside(true);
 			mAuthDialog.setContentView(R.layout.auth_dialog);
 			web = (WebView)mAuthDialog.findViewById(R.id.webv);
 			web.setWebContentsDebuggingEnabled(!RTABMapActivity.DISABLE_LOG);
@@ -200,7 +206,6 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 			});
 			mAuthDialog.show();
 			mAuthDialog.setTitle("Authorize RTAB-Map");
-			mAuthDialog.setCancelable(true);
 			web.loadUrl(auth_url);
 		}
 		else
@@ -213,6 +218,8 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	{		
 		mProgressDialog.setTitle("Upload to Sketchfab");
 		mProgressDialog.setMessage(String.format("Compressing the files..."));
+		mProgressDialog.setCancelable(false);
+		mProgressDialog.setCanceledOnTouchOutside(false);
 		mProgressDialog.show();
 
 		Thread workingThread = new Thread(new Runnable() {
@@ -238,7 +245,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 					File exportDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_EXPORT_DIR);
 					exportDir.mkdirs();
 					
-					if(RTABMapLib.writeExportedMesh(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
+					if(RTABMapLib.writeExportedMesh(RTABMapActivity.nativeApplication, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
 					{
 						String[] files = new String[0];
 						// verify if we have all files
@@ -308,7 +315,10 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 											// do nothing...
 										}
 									});
-									builder.show();
+									AlertDialog ad = builder.create();
+									ad.setCancelable(false);
+									ad.setCanceledOnTouchOutside(false);
+									ad.show();
 								}
 							});
 						}
@@ -371,6 +381,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 						finish();
 					}
 				}).create();
+				d.setCanceledOnTouchOutside(false);
 				d.show();
 				((TextView)d.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
 			}

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

@@ -0,0 +1,87 @@
+package com.introlab.rtabmap;
+
+
+import android.graphics.ImageFormat;
+import android.media.Image;
+import android.media.ImageReader;
+import android.os.Handler;
+import android.os.HandlerThread;
+import android.util.Log;
+
+import java.nio.ByteBuffer;
+
+public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
+
+    public int WIDTH;
+    public int HEIGHT;
+    public ImageReader imageReader;
+    public int frameCount = 0;
+    public long timestamp;
+    
+    // Looper handler thread.
+    private HandlerThread backgroundThread;
+    // Looper handler.
+    private Handler backgroundHandler;
+
+    public ByteBuffer depth16_raw;
+
+    TOF_ImageReader(){
+    }
+
+    public void createImageReader(int width, int height){
+        this.WIDTH = width;
+        this.HEIGHT = height;
+        this.imageReader =
+                ImageReader.newInstance(
+                        width,
+                        height,
+                        ImageFormat.DEPTH16,
+                        2);
+        this.imageReader.setOnImageAvailableListener(this, this.backgroundHandler);
+    }
+
+    // CPU image reader callback.
+    @Override
+    public void onImageAvailable(ImageReader imageReader) {
+        Image image  = imageReader.acquireLatestImage();
+        if (image == null) {
+            Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
+            return;
+        }
+        else{
+            if(image.getFormat() == ImageFormat.DEPTH16){
+                this.timestamp = image.getTimestamp();
+                depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+                // copy raw undecoded DEPTH16 format depth data to NativeBuffer
+                frameCount++;
+            }
+            else{
+                Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
+            }
+        }
+        image.close();
+    }
+    
+ // Start background handler thread, used to run callbacks without blocking UI thread.
+    public void startBackgroundThread() {
+        this.backgroundThread = new HandlerThread("DepthDecoderThread");
+        this.backgroundThread.start();
+        this.backgroundHandler = new Handler(backgroundThread.getLooper());
+    }
+
+    // Stop background handler thread.
+    public void stopBackgroundThread() {
+        if (this.backgroundThread != null) {
+            this.backgroundThread.quitSafely();
+            try {
+                this.backgroundThread.join();
+                this.backgroundThread = null;
+                this.backgroundHandler = null;
+            } catch (InterruptedException e) {
+                Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
+            }
+        }
+    }
+
+}
+

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

@@ -17,6 +17,7 @@ import android.graphics.Typeface;
 import android.opengl.GLES20;
 import android.opengl.GLUtils;
 import android.text.TextPaint;
+import android.util.Log;
 
 public class TextManager {
 	
@@ -50,7 +51,7 @@ public class TextManager {
 	public static final int RI_TEXT_TEXTURE_SIZE = 512; // 512
 	public static final float RI_TEXT_HEIGHT_BASE = 32.0f;
 	public static final char RI_TEXT_START = ' ';
-	public static final char RI_TEXT_STOP = '~'+1;
+	public static final char RI_TEXT_STOP = '\u00B0'+1;
 	
 	public float getMaxTextHeight() {return mTextHeight;}
 	
@@ -336,6 +337,8 @@ public class TextManager {
 		float y = val.y;
 		String text = val.text;
 		
+		Log.i("RTABMapActivity", String.format("convertTextToTriangleInfo() set status=%s", text));
+		
 		// Create 
 		for(int j=0; j<text.length(); j++)
 		{

app/src/CMakeLists.txt

@@ -104,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
         WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
        
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
   SET(plugin_dest_dir bin)
   SET(qtconf_dest_dir bin)
@@ -154,11 +154,28 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
 		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
 		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
 		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}\"")
 		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)
   ENDIF()
   
   # install a qt.conf file
@@ -189,5 +206,5 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
   " COMPONENT runtime)
-ENDIF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

app/src/ObjDeletionHandler.h

@@ -46,7 +46,7 @@ public:
 	}
 	virtual ~ObjDeletionHandler() {}
 
-signals:
+Q_SIGNALS:
 	void objDeletionEventReceived(int);
 
 protected:
@@ -55,7 +55,7 @@ protected:
 		if(event->getClassName().compare("UObjDeletedEvent") == 0 &&
 		   event->getCode() == _watchedId)
 		{
-			emit objDeletionEventReceived(_watchedId);
+			Q_EMIT objDeletionEventReceived(_watchedId);
 		}
 		return false;
 	}

app/src/main.cpp

@@ -43,13 +43,13 @@ int main(int argc, char* argv[])
 {
 	/* Set logger type */
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setLevel(ULogger::kWarning);
 
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box
 
-	ParametersMap parameters = Parameters::parseArguments(argc, argv, true);
+	ParametersMap parameters = Parameters::parseArguments(argc, argv, false);
 	MainWindow * mainWindow = new MainWindow();
     app->installEventFilter(mainWindow); // to catch FileOpen events.
     
@@ -61,11 +61,10 @@ int main(int argc, char* argv[])
            UFile::getExtension(value).compare("db") == 0)
         {
             database = value;
-            break;
         }
     }
 
-	UINFO("Program started...");
+	printf("Program started...\n");
 
 	UEventsManager::addHandler(mainWindow);
 
@@ -85,9 +84,9 @@ int main(int argc, char* argv[])
     
     if(!database.empty())
     {
-    	mainWindow->openDatabase(database.c_str());
+    	mainWindow->openDatabase(database.c_str(), parameters);
     }
-    if(parameters.size())
+    else if(parameters.size())
     {
     	mainWindow->updateParameters(parameters);
     }
@@ -101,14 +100,13 @@ int main(int argc, char* argv[])
 	UEventsManager::removeHandler(mainWindow);
 	UEventsManager::removeHandler(rtabmap);
 
-	UINFO("Killing threads...");
 	rtabmap->join(true);
 
-	UINFO("Closing RTAB-Map...");
+	printf("Closing RTAB-Map...\n");
 	delete rtabmap;
 	delete mainWindow;
 	delete app;
-	UINFO("All done!");
+	printf("All done!\n");
 
     return 0;
 }

cmake_modules/FindARCore.cmake

@@ -0,0 +1,31 @@
+# - Find ARCore
+# This module finds an installed ARCore client C-API package.
+#
+# It sets the following variables:
+#  ARCore_FOUND       - Set to false, or undefined, if ARCore isn't found.
+#  ARCore_INCLUDE_DIRS - The ARCore include directory.
+#  ARCore_LIBRARIES     - The ARCore library to link against.
+
+FIND_PATH(ARCore_INCLUDE_DIR arcore_c_api.h)
+
+FIND_LIBRARY(ARCore_c_LIBRARY NAMES arcore_sdk_c PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(ARCore_jni_LIBRARY NAMES arcore_sdk_jni PATH_SUFFIXES ${ANDROID_ABI})
+
+IF (ARCore_INCLUDE_DIR AND ARCore_c_LIBRARY AND ARCore_jni_LIBRARY)
+   SET(ARCore_FOUND TRUE)
+   SET(ARCore_INCLUDE_DIRS ${ARCore_INCLUDE_DIR})
+   SET(ARCore_LIBRARIES ${ARCore_c_LIBRARY} ${ARCore_jni_LIBRARY})
+ENDIF (ARCore_INCLUDE_DIR AND ARCore_c_LIBRARY AND ARCore_jni_LIBRARY)
+
+IF (ARCore_FOUND)
+   # show which ARCore was found only if not quiet
+   IF (NOT ARCore_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ARCore: ${ARCore_INCLUDE_DIRS}")
+   ENDIF (NOT ARCore_FIND_QUIETLY)
+ELSE (ARCore_FOUND)
+   # fatal error if ARCore is required but not found
+   IF (ARCore_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ARCore (client and/or support libraries)")
+   ENDIF (ARCore_FIND_REQUIRED)
+ENDIF (ARCore_FOUND)
+

cmake_modules/FindAREngine.cmake

@@ -0,0 +1,32 @@
+# - Find AREngine
+# This module finds an installed AREngine client C-API package.
+#
+# It sets the following variables:
+#  AREngine_FOUND       - Set to false, or undefined, if AREngine isn't found.
+#  AREngine_INCLUDE_DIRS - The AREngine include directory.
+#  AREngine_LIBRARIES     - The AREngine library to link against.
+
+FIND_PATH(AREngine_INCLUDE_DIR huawei_arengine_interface.h)
+
+FIND_LIBRARY(AREngine_impl_LIBRARY NAMES huawei_arengine_impl PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(AREngine_jni_LIBRARY NAMES huawei_arengine_jni PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(AREngine_ndk_LIBRARY NAMES huawei_arengine_ndk PATH_SUFFIXES ${ANDROID_ABI})
+
+IF (AREngine_INCLUDE_DIR AND AREngine_impl_LIBRARY AND AREngine_jni_LIBRARY AND AREngine_ndk_LIBRARY)
+   SET(AREngine_FOUND TRUE)
+   SET(AREngine_INCLUDE_DIRS ${AREngine_INCLUDE_DIR})
+   SET(AREngine_LIBRARIES ${AREngine_impl_LIBRARY} ${AREngine_jni_LIBRARY} ${AREngine_ndk_LIBRARY})
+ENDIF (AREngine_INCLUDE_DIR AND AREngine_impl_LIBRARY AND AREngine_jni_LIBRARY AND AREngine_ndk_LIBRARY)
+
+IF (AREngine_FOUND)
+   # show which AREngine was found only if not quiet
+   IF (NOT AREngine_FIND_QUIETLY)
+      MESSAGE(STATUS "Found AREngine: ${AREngine_INCLUDE_DIRS}")
+   ENDIF (NOT AREngine_FIND_QUIETLY)
+ELSE (AREngine_FOUND)
+   # fatal error if AREngine is required but not found
+   IF (AREngine_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find AREngine's impl, jni and ndk libraries)")
+   ENDIF (AREngine_FIND_REQUIRED)
+ENDIF (AREngine_FOUND)
+

cmake_modules/FindFastCV.cmake

@@ -0,0 +1,37 @@
+# - Find FastCV (https://developer.qualcomm.com/software/fastcv-sdk)
+#
+# It sets the following variables:
+#  FastCV_FOUND         - Set to false, or undefined, if FastCV isn't found.
+#  FastCV_INCLUDE_DIRS  - The FastCV include directory.
+#  FastCV_LIBRARIES     - The FastCV library to link against.
+
+IF(NOT WIN32 AND NOT APPLE)
+
+   EXECUTE_PROCESS( COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE )
+   MESSAGE( STATUS "Architecture: ${ARCHITECTURE}" )
+
+   # Currently only tested on aarch64!
+   IF(${ARCHITECTURE} STREQUAL "aarch64")
+      find_path(FastCV_INCLUDE_DIRS NAMES fastcv.h PATH_SUFFIXES fastcv)
+      find_library(FastCV_LIBRARY NAMES fastcvopt fastcv)
+   ENDIF(${ARCHITECTURE} STREQUAL "aarch64")
+ENDIF(NOT WIN32 AND NOT APPLE)
+
+IF (FastCV_INCLUDE_DIRS AND FastCV_LIBRARY)
+   SET(FastCV_FOUND TRUE)
+ENDIF (FastCV_INCLUDE_DIRS AND FastCV_LIBRARY)
+
+IF (FastCV_FOUND)
+   SET(FastCV_LIBRARIES ${FastCV_LIBRARY})
+     
+   # show which RealSense was found only if not quiet
+   IF (NOT FastCV_FIND_QUIETLY)
+      MESSAGE(STATUS "Found FastCV: ${FastCV_LIBRARIES}")
+   ENDIF (NOT FastCV_FIND_QUIETLY)
+ELSE (FastCV_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (FastCV_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find FastCV")
+   ENDIF (FastCV_FIND_REQUIRED)
+ENDIF (FastCV_FOUND)
+

cmake_modules/FindFlyCapture2.cmake

@@ -7,32 +7,39 @@
 #  FlyCapture2_LIBRARIES     - The FlyCapture2 library to link against.
 
 if(CMAKE_CL_64)
-set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib64)
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib64 $ENV{FC2LIB}/lib64)
 else()
-set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib)
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib $ENV{FC2LIB}/lib)
 endif()
 
 if(CMAKE_CL_64)
-set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib64)
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib64 $ENV{TRICLOPSLIB}/lib64)
 else()
-set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib)
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib $ENV{TRICLOPSLIB}/lib)
 endif()
 
 #FlyCapture2 SDK
-find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include $ENV{FlyCapture2_ROOT_DIR}/include/flycapture)
-find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 flycapture NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR})
+find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include $ENV{FlyCapture2_ROOT_DIR}/include/flycapture $ENV{FC2LIB}/include $ENV{FC2LIB}/include/flycapture)
+find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v140 FlyCapture2_v100 FlyCapture2 flycapture2 flycapture NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR}/vs2015 ${FlyCapture2_LIBDIR})
+
+MESSAGE(STATUS "FlyCapture2_INCLUDE_DIR=${FlyCapture2_INCLUDE_DIR}")
+MESSAGE(STATUS "FlyCapture2_LIBRARY=${FlyCapture2_LIBRARY}")
 
 # Triclops SDK
-find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include $ENV{Triclops_ROOT_DIR}/include/triclops)
-find_library(Triclops_LIBRARY NAMES triclops triclops_v100 libtriclops.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 libflycapture2bridge.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include $ENV{Triclops_ROOT_DIR}/include/triclops $ENV{TRICLOPSLIB}/include $ENV{TRICLOPSLIB}/include/triclops)
+find_library(Triclops_LIBRARY NAMES triclops triclops_v140 triclops_v100 libtriclops.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v140 flycapture2bridge_v100 libflycapture2bridge.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v140 pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
 
-IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+MESSAGE(STATUS "Triclops_INCLUDE_DIR=${Triclops_INCLUDE_DIR}")
+MESSAGE(STATUS "Triclops_LIBRARY=${Triclops_LIBRARY}")
+MESSAGE(STATUS "FlyCaptureBridge_LIBRARY=${FlyCaptureBridge_LIBRARY}")
+
+IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY)
    SET(FlyCapture2_FOUND TRUE)
    SET(FlyCapture2_INCLUDE_DIRS ${FlyCapture2_INCLUDE_DIR} ${Triclops_INCLUDE_DIR})
-   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
-ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY})
+ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY)
 
 IF (FlyCapture2_FOUND)
    # show which FlyCapture2 was found only if not quiet

cmake_modules/FindK4A.cmake

@@ -0,0 +1,34 @@
+# - Find K4A 
+# This module finds an kinect 4 azure SDK
+#
+# It sets the following variables:
+#  K4A_FOUND       - Set to false, or undefined, if K4A isn't found.
+#  K4A_INCLUDE_DIRS - The K4A include directory.
+#  K4A_LIBRARIES     - The K4A library to link against.
+
+find_library(K4A_LIBRARY NAMES k4a NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_library(K4ARECORD_LIBRARY NAMES k4arecord NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_path(K4A_INCLUDE_DIR NAMES k4a/k4a.h PATHS $ENV{K4A_ROOT_DIR}/sdk/include)
+
+IF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+   SET(K4A_FOUND TRUE)
+   SET(K4A_INCLUDE_DIRS ${K4A_INCLUDE_DIR})
+   SET(K4A_LIBRARIES ${K4A_LIBRARY} ${K4ARECORD_LIBRARY})
+
+   # Compatibility with linux names
+   SET(k4a_LIBRARIES ${K4A_LIBRARIES})
+   SET(k4a_INCLUDE_DIRS ${K4A_INCLUDE_DIRS})
+   SET(k4a_FOUND ${K4A_FOUND})
+ENDIF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+
+IF (K4A_FOUND)
+   # show which K4A was found only if not quiet
+   IF (NOT K4A_FIND_QUIETLY)
+      MESSAGE(STATUS "Found K4A: ${K4A_LIBRARIES}")
+   ENDIF (NOT K4A_FIND_QUIETLY)
+ELSE (K4A_FOUND)
+   # fatal error if K4A is required but not found
+   IF (K4A_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find K4A (Kinect for Azure SDK)")
+   ENDIF (K4A_FIND_REQUIRED)
+ENDIF (K4A_FOUND)

cmake_modules/FindRealSense2.cmake

@@ -0,0 +1,41 @@
+# - Find librealsense (https://github.com/IntelRealSense/librealsense)
+#
+#  RealSense2_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  RealSense2_FOUND         - Set to false, or undefined, if RealSense2 isn't found.
+#  RealSense2_INCLUDE_DIRS  - The RealSense2 include directory.
+#  RealSense2_LIBRARIES     - The RealSense2 library to link against.
+
+#RealSense library
+
+find_path(RealSense2_INCLUDE_DIRS NAMES librealsense2/rs.hpp PATHS $ENV{RealSense2_ROOT_DIR}/include)
+if(CMAKE_CL_64)
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x64  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x64)
+else()
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x86  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x86)
+endif()
+
+IF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+   SET(RealSense2_FOUND TRUE)
+ENDIF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+
+IF (RealSense2_FOUND)
+   SET(RealSense2_LIBRARIES ${RealSense2_LIBRARY})
+   
+   # Compatibility with linux names
+   SET(realsense2_LIBRARIES ${RealSense2_LIBRARIES})
+   SET(realsense2_INCLUDE_DIRS ${RealSense2_INCLUDE_DIRS})
+   SET(realsense2_FOUND ${RealSense2_FOUND})
+  
+   # show which RealSense was found only if not quiet
+   IF (NOT RealSense2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RealSense: ${RealSense2_LIBRARIES}")
+   ENDIF (NOT RealSense2_FIND_QUIETLY)
+ELSE (RealSense2_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (RealSense2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RealSense2 (librealsense2)")
+   ENDIF (RealSense2_FIND_REQUIRED)
+ENDIF (RealSense2_FOUND)
+

cmake_modules/FindSqlite3.cmake

@@ -2,46 +2,29 @@
 # This module finds an installed Sqlite3 package.
 #
 # It sets the following variables:
-#  SQLITE3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  SQLITE3_INCLUDE_DIR - The Sqlite3 include directory.
-#  SQLITE3_LIBRARY     - The Sqlite3 library to link against.
+#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
+#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
+#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.
 
-SET(SQLITE3_VERSION_REQUIRED "3.6.0")
+FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})
 
-IF(UNIX)
-	FIND_PROGRAM(SQLITE3_EXEC NAME sqlite3 PATHS)  
-	IF(SQLITE3_EXEC)  
-        MESSAGE(STATUS "Found Sqlite3 executable : ${SQLITE3_EXEC}")
-		EXECUTE_PROCESS(COMMAND ${SQLITE3_EXEC} --version 
-				   	    OUTPUT_VARIABLE SQLITE3_VERSION
-						OUTPUT_STRIP_TRAILING_WHITESPACE
-						WORKING_DIRECTORY "./"
-   		)
-		IF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-		    MESSAGE(FATAL_ERROR "Sqlite ${SQLITE3_VERSION} found, but version ${SQLITE3_VERSION_REQUIRED} minimum is required")
-		ENDIF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-	ELSE(SQLITE3_EXEC)
-		MESSAGE(FATAL_ERROR "Could not find Sqlite3 executable")
-	ENDIF(SQLITE3_EXEC)
-ENDIF(UNIX)
+FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
 
-FIND_PATH(SQLITE3_INCLUDE_DIR sqlite3.h)
+IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
+   SET(Sqlite3_FOUND TRUE)
+   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
+   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
+ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
 
-FIND_LIBRARY(SQLITE3_LIBRARY NAMES sqlite3.dll sqlite3)
-
-IF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-   SET(SQLITE3_FOUND TRUE)
-ENDIF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-
-IF (SQLITE3_FOUND)
+IF (Sqlite3_FOUND)
    # show which Sqlite3 was found only if not quiet
    IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3")
+      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
    ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (SQLITE3_FOUND)
+ELSE (Sqlite3_FOUND)
    # fatal error if Sqlite3 is required but not found
    IF (Sqlite3_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Sqlite3")
    ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (SQLITE3_FOUND)
+ENDIF (Sqlite3_FOUND)
 

corelib/include/rtabmap/core/BayesFilter.h

@@ -76,6 +76,7 @@ private:
 	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
 	bool _fullPredictionUpdate;
 	float _totalPredictionLCValues;
+	float _predictionEpsilon;
 	std::map<int, std::map<int, int> > _neighborsIndex;
 };
 

corelib/include/rtabmap/core/CameraInfo.h

@@ -43,6 +43,7 @@ public:
 		timeCapture(0.0f),
 		timeDisparity(0.0f),
 		timeMirroring(0.0f),
+		timeStereoExposureCompensation(0.0f),
 		timeImageDecimation(0.0f),
 		timeScanFromDepth(0.0f),
 		timeUndistortDepth(0.0f),

corelib/include/rtabmap/core/CameraModel.h

@@ -50,7 +50,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
 
 	// minimal
 	CameraModel(
@@ -58,7 +58,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,13 +68,14 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
 	virtual ~CameraModel() {}
 
 	void initRectificationMap();
+	bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
 
 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
@@ -112,8 +113,14 @@ 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;}
+
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
+	std::vector<unsigned char> serialize() const;
+	unsigned int deserialize(const std::vector<unsigned char>& data);
+	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;

corelib/include/rtabmap/core/CameraRGB.h

@@ -27,213 +27,5 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/utilite/UTimer.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
-
-class UDirectory;
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraImages
-/////////////////////////
-class RTABMAP_EXP CameraImages :
-	public Camera
-{
-public:
-	CameraImages();
-	CameraImages(
-			const std::string & path,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const { return odometry_.size() > 0; }
-	std::string getPath() const {return _path;}
-	unsigned int imagesCount() const;
-	std::vector<std::string> filenames() const;
-	bool isImagesRectified() const {return _rectifyImages;}
-	int getBayerMode() const {return _bayerMode;}
-	const CameraModel & cameraModel() const {return _model;}
-
-	void setPath(const std::string & dir) {_path=dir;}
-	virtual void setStartIndex(int index) {_startAt = index;} // negative means last
-	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
-	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
-	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
-
-	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
-	{
-		_filenamesAreTimestamps = fileNamesAreStamps;
-		_timestampsPath=filePath;
-		_syncImageRateWithStamps = syncImageRateWithStamps;
-	}
-
-	void setScanPath(
-			const std::string & dir,
-			int maxScanPts = 0,
-			int downsampleStep = 1,
-			float voxelSize = 0.0f,
-			int normalsK = 0,        // compute normals if > 0
-			float normalsRadius = 0, // compute normals if > 0
-			const Transform & localTransform=Transform::getIdentity(),
-			bool forceGroundNormalsUp = false)
-	{
-		_scanPath = dir;
-		_scanLocalTransform = localTransform;
-		_scanMaxPts = maxScanPts;
-		_scanDownsampleStep = downsampleStep;
-		_scanNormalsK = normalsK;
-		_scanNormalsRadius = normalsRadius;
-		_scanVoxelSize = voxelSize;
-		_scanForceGroundNormalsUp = forceGroundNormalsUp;
-	}
-
-	void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
-	{
-		_depthFromScan = enabled;
-		_depthFromScanFillHoles = fillHoles;
-		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
-	}
-
-	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
-	void setOdometryPath(const std::string & filePath, int format = 0)
-	{
-		_odometryPath = filePath;
-		_odometryFormat = format;
-	}
-
-	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
-	void setGroundTruthPath(const std::string & filePath, int format = 0)
-	{
-		_groundTruthPath = filePath;
-		_groundTruthFormat = format;
-	}
-
-	void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
-	double getMaxPoseTimeDiff() const {return _maxPoseTimeDiff;}
-
-	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
-	{
-		_isDepth = isDepth;
-		_depthScaleFactor=depthScaleFactor;
-	}
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	bool readPoses(
-			std::list<Transform> & outputPoses,
-			std::list<double> & stamps,
-			const std::string & filePath,
-			int format,
-			double maxTimeDiff) const;
-
-private:
-	std::string _path;
-	int _startAt;
-	// If the list of files in the directory is refreshed
-	// on each call of takeImage()
-	bool _refreshDir;
-	bool _rectifyImages;
-	int _bayerMode;
-	bool _isDepth;
-	float _depthScaleFactor;
-	int _count;
-	UDirectory * _dir;
-	std::string _lastFileName;
-
-	int _countScan;
-	UDirectory * _scanDir;
-	std::string _lastScanFileName;
-	std::string _scanPath;
-	Transform _scanLocalTransform;
-	int _scanMaxPts;
-	int _scanDownsampleStep;
-	float _scanVoxelSize;
-	int _scanNormalsK;
-	float _scanNormalsRadius;
-	bool _scanForceGroundNormalsUp;
-
-	bool _depthFromScan;
-	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
-	bool _depthFromScanFillHolesFromBorder;
-
-	bool _filenamesAreTimestamps;
-	std::string _timestampsPath;
-	bool _syncImageRateWithStamps;
-
-	std::string _odometryPath;
-	int _odometryFormat;
-	std::string _groundTruthPath;
-	int _groundTruthFormat;
-	double _maxPoseTimeDiff;
-
-	std::list<double> _stamps;
-	std::list<Transform> odometry_;
-	std::list<Transform> groundTruth_;
-	CameraModel _model;
-
-	UTimer _captureTimer;
-	double _captureDelay;
-};
-
-
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-class RTABMAP_EXP CameraVideo :
-	public Camera
-{
-public:
-	enum Source{kVideoFile, kUsbDevice};
-
-public:
-	CameraVideo(int usbDevice = 0,
-			bool rectifyImages = false,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraVideo(const std::string & filePath,
-			bool rectifyImages = false,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraVideo();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	int getUsbDevice() const {return _usbDevice;}
-	const std::string & getFilePath() const {return _filePath;}
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	// File type
-	std::string _filePath;
-	bool _rectifyImages;
-
-	cv::VideoCapture _capture;
-	Source _src;
-
-	// Usb camera
-	int _usbDevice;
-	std::string _guid;
-
-	CameraModel _model;
-};
-
-
-} // namespace rtabmap
+#include <rtabmap/core/camera/CameraImages.h>
+#include <rtabmap/core/camera/CameraVideo.h>

corelib/include/rtabmap/core/CameraRGBD.h

@@ -27,420 +27,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include <rtabmap/core/camera/CameraFreenect.h>
+#include <rtabmap/core/camera/CameraFreenect2.h>
+#include <rtabmap/core/camera/CameraK4W2.h>
+#include <rtabmap/core/camera/CameraOpenni.h>
+#include <rtabmap/core/camera/CameraOpenNI2.h>
+#include <rtabmap/core/camera/CameraOpenNICV.h>
+#include <rtabmap/core/camera/CameraRealSense.h>
+#include <rtabmap/core/camera/CameraRealSense2.h>
+#include <rtabmap/core/camera/CameraRGBDImages.h>
+#include <rtabmap/core/camera/CameraK4A.h>
 
-#include "rtabmap/utilite/UMutex.h"
-#include "rtabmap/utilite/USemaphore.h"
-#include "rtabmap/core/CameraModel.h"
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraRGB.h"
-#include "rtabmap/core/Version.h"
-
-#include <pcl/pcl_config.h>
-
-#ifdef HAVE_OPENNI
-#if __linux__ && __i386__ && __cplusplus >= 201103L
-#warning "Openni driver is not available on i386 when building with c++11 support"
-#else
-#define RTABMAP_OPENNI
-#include <pcl/io/openni_camera/openni_depth_image.h>
-#include <pcl/io/openni_camera/openni_image.h>
-#endif
-#endif
-
-#include <boost/signals2/connection.hpp>
-
-namespace openni
-{
-class Device;
-class VideoStream;
-}
-
-namespace pcl
-{
-class Grabber;
-}
-
-namespace libfreenect2
-{
-class Freenect2;
-class Freenect2Device;
-class SyncMultiFrameListener;
-class Registration;
-class PacketPipeline;
-}
-
-namespace rs
-{
-	class context;
-	class device;
-	namespace slam {
-	class slam;
-	}
-}
-
-typedef struct _freenect_context freenect_context;
-typedef struct _freenect_device freenect_device;
-
-typedef struct IKinectSensor IKinectSensor;
-typedef struct ICoordinateMapper ICoordinateMapper;
-typedef struct _DepthSpacePoint DepthSpacePoint;
-typedef struct _ColorSpacePoint ColorSpacePoint;
-typedef struct tagRGBQUAD RGBQUAD;
-typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraOpenNIPCL
-/////////////////////////
-class RTABMAP_EXP CameraOpenni :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraOpenni(const std::string & deviceId="",
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenni();
-#ifdef RTABMAP_OPENNI
-    void image_cb (
-    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
-			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
-			float constant);
-#endif
-
-    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-    virtual bool isCalibrated() const;
-    virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-    pcl::Grabber* interface_;
-    std::string deviceId_;
-    boost::signals2::connection connection_;
-    cv::Mat depth_;
-    cv::Mat rgb_;
-    float depthConstant_;
-    UMutex dataMutex_;
-    USemaphore dataReady_;
-};
-
-/////////////////////////
-// CameraOpenNICV
-/////////////////////////
-class RTABMAP_EXP CameraOpenNICV :
-	public Camera
-{
-
-public:
-	static bool available();
-
-public:
-	CameraOpenNICV(bool asus = false,
-					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenNICV();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const {return "";} // unknown with OpenCV
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	bool _asus;
-	cv::VideoCapture _capture;
-	float _depthFocal;
-};
-
-/////////////////////////
-// CameraOpenNI2
-/////////////////////////
-class RTABMAP_EXP CameraOpenNI2 :
-	public Camera
-{
-
-public:
-	static bool available();
-	static bool exposureGainAvailable();
-	enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
-
-public:
-	CameraOpenNI2(const std::string & deviceId = "",
-					Type type = kTypeColorDepth,
-					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenNI2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-	bool setAutoWhiteBalance(bool enabled);
-	bool setAutoExposure(bool enabled);
-	bool setExposure(int value);
-	bool setGain(int value);
-	bool setMirroring(bool enabled);
-	void setOpenNI2StampsAndIDsUsed(bool used);
-	void setIRDepthShift(int horizontal, int vertical);
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_OPENNI2
-	Type _type;
-	openni::Device * _device;
-	openni::VideoStream * _color;
-	openni::VideoStream * _depth;
-	float _depthFx;
-	float _depthFy;
-	std::string _deviceId;
-	bool _openNI2StampsAndIDsUsed;
-	StereoCameraModel _stereoModel;
-	int _depthHShift;
-	int _depthVShift;
-#endif
-};
-
-
-/////////////////////////
-// CameraFreenect
-/////////////////////////
-class FreenectDevice;
-
-class RTABMAP_EXP CameraFreenect :
-	public Camera
-{
-public:
-	static bool available();
-	enum Type {kTypeColorDepth, kTypeIRDepth};
-
-public:
-	// default local transform z in, x right, y down));
-	CameraFreenect(int deviceId= 0,
-					Type type = kTypeColorDepth,
-					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraFreenect();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FREENECT
-	int deviceId_;
-	Type type_;
-	freenect_context * ctx_;
-	FreenectDevice * freenectDevice_;
-	StereoCameraModel stereoModel_;
-#endif
-};
-
-/////////////////////////
-// CameraFreenect2
-/////////////////////////
-
-class RTABMAP_EXP CameraFreenect2 :
-	public Camera
-{
-public:
-	static bool available();
-
-	enum Type{
-		kTypeColor2DepthSD,
-		kTypeDepth2ColorSD,
-		kTypeDepth2ColorHD,
-		kTypeDepth2ColorHD2,
-		kTypeIRDepth,
-		kTypeColorIR
-	};
-
-public:
-	// default local transform z in, x right, y down));
-	CameraFreenect2(int deviceId= 0,
-					Type type = kTypeDepth2ColorSD,
-					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
-					float minDepth = 0.3f,
-					float maxDepth = 12.0f,
-					bool bilateralFiltering = true,
-					bool edgeAwareFiltering = true,
-					bool noiseFiltering = true,
-					const std::string & pipelineName = "");
-	virtual ~CameraFreenect2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FREENECT2
-	int deviceId_;
-	Type type_;
-	StereoCameraModel stereoModel_;
-	libfreenect2::Freenect2 * freenect2_;
-	libfreenect2::Freenect2Device *dev_;
-	libfreenect2::SyncMultiFrameListener * listener_;
-	libfreenect2::Registration * reg_;
-	float minKinect2Depth_;
-	float maxKinect2Depth_;
-	bool bilateralFiltering_;
-	bool edgeAwareFiltering_;
-	bool noiseFiltering_;
-	std::string pipelineName_;
-#endif
-};
-
-/////////////////////////
-// CameraK4W2
-/////////////////////////
-
-class RTABMAP_EXP CameraK4W2 :
-	public Camera
-{
-public:
-	static bool available();
-
-	enum Type {
-		kTypeColor2DepthSD,
-		kTypeDepth2ColorSD,
-		kTypeDepth2ColorHD
-	};
-
-public:
-	static const int        cDepthWidth = 512;
-	static const int        cDepthHeight = 424;
-	static const int        cColorWidth = 1920;
-	static const int        cColorHeight = 1080;
-
-public:
-	// default local transform z in, x right, y down));
-	CameraK4W2(int deviceId = 0, // not used
-		Type type = kTypeDepth2ColorSD,
-		float imageRate = 0.0f,
-		const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraK4W2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	void close();
-
-private:
-#ifdef RTABMAP_K4W2
-	Type type_;
-	IKinectSensor*          pKinectSensor_;
-	ICoordinateMapper*      pCoordinateMapper_;
-	DepthSpacePoint*        pDepthCoordinates_;
-	ColorSpacePoint*        pColorCoordinates_;
-	IMultiSourceFrameReader* pMultiSourceFrameReader_;
-	RGBQUAD * pColorRGBX_;
-	INT_PTR hMSEvent;
-	CameraModel colorCameraModel_;
-#endif
-};
-
-/////////////////////////
-// CameraRealSense
-/////////////////////////
-class slam_event_handler;
-class RTABMAP_EXP CameraRealSense :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraRealSense(
-		int deviceId = 0,
-		int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		bool computeOdometry = false,
-		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRealSense();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_REALSENSE
-	rs::context * ctx_;
-	rs::device * dev_;
-	int deviceId_;
-	int presetRGB_;
-	int presetDepth_;
-	bool computeOdometry_;
-
-	int motionSeq_[2];
-	rs::slam::slam * slam_;
-	UMutex slamLock_;
-
-	std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
-	std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
-	UMutex dataMutex_;
-	USemaphore dataReady_;
-#endif
-};
-
-
-/////////////////////////
-// CameraRGBDImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraRGBDImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraRGBDImages(
-			const std::string & pathRGBImages,
-			const std::string & pathDepthImages,
-			float depthScaleFactor = 1.0f,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRGBDImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages cameraDepth_;
-};
-
-} // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -27,224 +27,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include "rtabmap/core/CameraModel.h"
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraRGB.h"
-#include "rtabmap/core/Version.h"
-#include <list>
-
-namespace FlyCapture2
-{
-class Camera;
-}
-
-namespace sl
-{
-class Camera;
-}
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraStereoDC1394
-/////////////////////////
-class DC1394Device;
-
-class RTABMAP_EXP CameraStereoDC1394 :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoDC1394();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_DC1394
-	DC1394Device *device_;
-	StereoCameraModel stereoModel_;
-#endif
-};
-
-/////////////////////////
-// CameraStereoFlyCapture2
-/////////////////////////
-class RTABMAP_EXP CameraStereoFlyCapture2 :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoFlyCapture2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FLYCAPTURE2
-	FlyCapture2::Camera * camera_;
-	void * triclopsCtx_; // TriclopsContext
-#endif
-};
-
-/////////////////////////
-// CameraStereoZED
-/////////////////////////
-class RTABMAP_EXP CameraStereoZed :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoZed(
-			int deviceId,
-			int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 0,// 0=STANDARD, 1=FILL
-			int confidenceThr = 100,
-			bool computeOdometry = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
-	CameraStereoZed(
-			const std::string & svoFilePath,
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 0,// 0=STANDARD, 1=FILL
-			int confidenceThr = 100,
-			bool computeOdometry = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
-	virtual ~CameraStereoZed();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_ZED
-	sl::Camera * zed_;
-	StereoCameraModel stereoModel_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-	std::string svoFilePath_;
-	int resolution_;
-	int quality_;
-	bool selfCalibration_;
-	int sensingMode_;
-	int confidenceThr_;
-	bool computeOdometry_;
-	bool lost_;
-#endif
-};
-
-/////////////////////////
-// CameraStereoImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoImages(
-			const std::string & pathLeftImages,
-			const std::string & pathRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoImages(
-			const std::string & pathLeftRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages * camera2_;
-	StereoCameraModel stereoModel_;
-};
-
-
-/////////////////////////
-// CameraStereoVideo
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoVideo :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoVideo(
-			const std::string & pathSideBySide,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			const std::string & pathLeft,
-			const std::string & pathRight,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			int device,
-			bool rectifyImages = false,
-			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoVideo();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	cv::VideoCapture capture_;
-	cv::VideoCapture capture2_;
-	std::string path_;
-	std::string path2_;
-	bool rectifyImages_;
-	StereoCameraModel stereoModel_;
-	std::string cameraName_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-};
-
-} // namespace rtabmap
+#include <rtabmap/core/camera/CameraStereoDC1394.h>
+#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
+#include <rtabmap/core/camera/CameraStereoImages.h>
+#include <rtabmap/core/camera/CameraStereoVideo.h>
+#include <rtabmap/core/camera/CameraStereoZed.h>
+#include <rtabmap/core/camera/CameraStereoTara.h>
+#include <rtabmap/core/camera/CameraMyntEye.h>

corelib/include/rtabmap/core/CameraThread.h

@@ -45,6 +45,7 @@ class Camera;
 class CameraInfo;
 class SensorData;
 class StereoDense;
+class IMUFilter;
 
 /**
  * Class CameraThread
@@ -68,21 +69,27 @@ 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 disableIMUFiltering();
 
-	void setScanFromDepth(
-			bool enabled,
-			int decimation=4,
-			float maxDepth=4.0f,
+	void setScanParameters(
+			bool fromDepth,
+			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
+			float rangeMin=0.0f,
+			float rangeMax=0.0f,
 			float voxelSize = 0.0f,
 			int normalsK = 0,
-			int normalsRadius = 0.0f)
+			int normalsRadius = 0.0f,
+			bool forceGroundNormalsUp = false)
 	{
-		_scanFromDepth = enabled;
-		_scanDecimation=decimation;
-		_scanMaxDepth = maxDepth;
+		_scanFromDepth = fromDepth;
+		_scanDownsampleStep=downsampleStep;
+		_scanRangeMin = rangeMin;
+		_scanRangeMax = rangeMax;
 		_scanVoxelSize = voxelSize;
 		_scanNormalsK = normalsK;
 		_scanNormalsRadius = normalsRadius;
+		_scanForceGroundNormalsUp = forceGroundNormalsUp;
 	}
 
 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -106,17 +113,19 @@ private:
 	int _imageDecimation;
 	bool _stereoToDepth;
 	bool _scanFromDepth;
-	int _scanDecimation;
-	float _scanMaxDepth;
-	float _scanMinDepth;
+	int _scanDownsampleStep;
+	float _scanRangeMin;
+	float _scanRangeMax;
 	float _scanVoxelSize;
 	int _scanNormalsK;
 	float _scanNormalsRadius;
+	bool _scanForceGroundNormalsUp;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;
 	float _bilateralSigmaS;
 	float _bilateralSigmaR;
+	IMUFilter * _imuFilter;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/DBDriver.h

@@ -96,14 +96,15 @@ public:
 				float cellSize,
 				const cv::Point3f & viewpoint);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
+	void updateLaserScan(int nodeId, const LaserScan & scan);
 
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
-	void addStatistics(const Statistics & statistics) const;
+	void addStatistics(const Statistics & statistics, bool saveWmState) const;
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
-	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose = 0) const;
 	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
 	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
@@ -156,30 +157,34 @@ public:
 
 	// Load objects
 	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
-	void loadLastNodes(std::list<Signature *> & signatures) const;
-	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
-	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
+	void loadLastNodes(std::list<Signature *> & signatures) const; // returned signatures must be freed after usage
+	Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
+	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0); // returned signatures must be freed after usage
+	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); // returned words must be freed after usage
 
 	// Specific queries...
+	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
-	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
+	void getLastNodeIds(std::set<int> & ids) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
-	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
+	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
+	void getLastMapId(int & mapId) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
+	void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const;
 	void getNodeIdByLabel(const std::string & label, int & id) const;
 	void getAllLabels(std::map<int, std::string> & labels) const;
 
 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
 
-private:
 	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;
@@ -218,22 +223,26 @@ private:
 	virtual void updateLinkQuery(const Link & link) const = 0;
 
 	virtual void updateOccupancyGridQuery(
-				int nodeId,
-				const cv::Mat & ground,
-				const cv::Mat & obstacles,
-				const cv::Mat & empty,
-				float cellSize,
-				const cv::Point3f & viewpoint) const = 0;
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewpoint) const = 0;
 
 	virtual void updateDepthImageQuery(
-					int nodeId,
-					const cv::Mat & image) const = 0;
+			int nodeId,
+			const cv::Mat & image) const = 0;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const = 0;
+
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
-	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const = 0;
 	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(
@@ -259,16 +268,18 @@ private:
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
-	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
+	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
-	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
-	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const = 0;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
 

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
-#include "sqlite3/sqlite3.h"
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;
 
 namespace rtabmap {
 
@@ -48,7 +50,7 @@ public:
 	void setSynchronous(int synchronous);
 	void setTempStore(int tempStore);
 
-private:
+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;
@@ -98,11 +100,15 @@ private:
 			int nodeId,
 			const cv::Mat & image) const;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const;
+
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
-	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const;
 	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
 	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
 	virtual void saveOptimizedMeshQuery(
@@ -128,16 +134,18 @@ private:
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
-	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
+	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
-	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
-	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
 
@@ -146,23 +154,24 @@ private:
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
 	std::string queryStepDepthUpdate() const;
+	std::string queryStepScanUpdate() const;
 	std::string queryStepSensorData() const;
 	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepGlobalDescriptor() const;
 	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
-	void stepImage(
-			sqlite3_stmt * ppStmt,
-			int id,
-			const cv::Mat & imageBytes) const;
+	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
+	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
-	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 			int nodeId,
 			const cv::Mat & ground,
@@ -175,10 +184,12 @@ private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
-private:
+protected:
 	sqlite3 * _ppDb;
-	long _memoryUsedEstimate;
 	std::string _version;
+
+private:
+	long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

corelib/include/rtabmap/core/DBReader.h

@@ -50,15 +50,17 @@ public:
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
-			 int startIndex = 0,
-			 int cameraIndex = -1);
+			 int startId = 0,
+			 int cameraIndex = -1,
+			 int stopId = 0);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
-			 int startIndex = 0,
-			 int cameraIndex = -1);
+			 int startId = 0,
+			 int cameraIndex = -1,
+			 int stopId = 0);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -80,7 +82,8 @@ private:
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
 	bool _goalsIgnored;
-	int _startIndex;
+	int _startId;
+	int _stopId;
 	int _cameraIndex;
 
 	DBDriver * _dbDriver;

corelib/include/rtabmap/core/EnvSensor.h

@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2010-2018, 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_ENVSENSOR_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
+
+namespace rtabmap {
+
+class EnvSensor
+{
+public:
+	enum Type {
+		// built-in types
+		kUndefined = 0,
+		kWifiSignalStrength,      // dBm
+		kAmbientTemperature,      // Celcius
+		kAmbientAirPressure,      // hPa
+		kAmbientLight,            // lx
+		kAmbientRelativeHumidity, // %
+
+		// user types
+		kCustomSensor1 = 100,
+		kCustomSensor2,
+		kCustomSensor3,
+		kCustomSensor4,
+		kCustomSensor5,
+		kCustomSensor6,
+		kCustomSensor7,
+		kCustomSensor8,
+		kCustomSensor9
+	};
+
+public:
+	EnvSensor() :
+		type_(kUndefined),
+		value_(0.0),
+		stamp_(0.0)
+	{}
+	EnvSensor(const Type & type, const double & value,const double & stamp = 0) :
+		type_(type),
+		value_(value),
+		stamp_(stamp)
+	{}
+
+	virtual ~EnvSensor() {}
+
+	const Type & type() const {return type_;}
+	const double & value() const {return value_;}
+	const double & stamp() const {return stamp_;}
+
+private:
+	Type type_;
+	double value_;
+	double stamp_;
+};
+
+typedef std::map<EnvSensor::Type, EnvSensor> EnvSensors;
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_ */

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -75,8 +75,8 @@ public:
 	static cv::Mat findFFromWords(
 			const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
 			std::vector<uchar> & status,
-			double ransacParam1 = 3.0,
-			double ransacParam2 = 0.99);
+			double ransacReprojThreshold = 3.0,
+			double ransacConfidence = 0.99);
 
 	// assume a canonical camera (without K)
 	static void findRTFromP(

corelib/include/rtabmap/core/Features2d.h

@@ -62,7 +62,9 @@ namespace cv{
 namespace xfeatures2d {
 class FREAK;
 class BriefDescriptorExtractor;
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 class SIFT;
+#endif
 class SURF;
 }
 namespace cuda {
@@ -71,7 +73,11 @@ 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))))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
+#else
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.3.0-dev
+#endif
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;
 typedef cv::xfeatures2d::FREAK CV_FREAK;
@@ -84,9 +90,11 @@ typedef cv::cuda::ORB CV_ORB_GPU;
 typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
 #endif
 
-
 namespace rtabmap {
 
+class ORBextractor;
+class SPDetector;
+
 class Stereo;
 #if CV_MAJOR_VERSION < 3
 class CV_ORB;
@@ -105,7 +113,40 @@ public:
 		kFeatureGfttBrief=6,
 		kFeatureBrisk=7,
 		kFeatureGfttOrb=8,  //new 0.10.11
-		kFeatureKaze=9};    //new 0.13.2
+		kFeatureKaze=9,     //new 0.13.2
+		kFeatureOrbOctree=10, //new 0.19.2
+		kFeatureSuperPointTorch=11}; //new 0.19.7
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kFeatureSurf:
+			return "SURF";
+		case kFeatureSift:
+			return "SIFT";
+		case kFeatureOrb:
+			return "ORB";
+		case kFeatureFastFreak:
+			return "FAST+FREAK";
+		case kFeatureFastBrief:
+			return "FAST+BRIEF";
+		case kFeatureGfttFreak:
+			return "GFTT+Freak";
+		case kFeatureGfttBrief:
+			return "GFTT+Brief";
+		case kFeatureBrisk:
+			return "BRISK";
+		case kFeatureGfttOrb:
+			return "GFTT+ORB";
+		case kFeatureKaze:
+			return "KAZE";
+		case kFeatureOrbOctree:
+			return "ORB-OCTREE";
+		case kFeatureSuperPointTorch:
+			return "SUPERPOINT";
+		default:
+			return "Unknown";
+		}
+	}
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -121,6 +162,12 @@ public:
 			const cv::Mat & depth,
 			float minDepth,
 			float maxDepth);
+	static void filterKeypointsByDepth(
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors,
+			std::vector<cv::Point3f> & keypoints3D,
+			float minDepth,
+			float maxDepth);
 
 	static void filterKeypointsByDisparity(
 				std::vector<cv::KeyPoint> & keypoints,
@@ -136,6 +183,7 @@ public:
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
@@ -143,13 +191,15 @@ public:
 	int getMaxFeatures() const {return maxFeatures_;}
 	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
+	int getGridRows() const {return gridRows_;}
+	int getGridCols() const {return gridCols_;}
 
 public:
 	virtual ~Feature2D();
 
 	std::vector<cv::KeyPoint> generateKeypoints(
 			const cv::Mat & image,
-			const cv::Mat & mask = cv::Mat()) const;
+			const cv::Mat & mask = cv::Mat());
 	cv::Mat generateDescriptors(
 			const cv::Mat & image,
 			std::vector<cv::KeyPoint> & keypoints) const;
@@ -165,7 +215,7 @@ protected:
 	Feature2D(const ParametersMap & parameters = ParametersMap());
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const = 0;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) = 0;
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
 
 private:
@@ -194,7 +244,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureSurf;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -221,7 +271,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureSift;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -229,6 +279,7 @@ private:
 	double contrastThreshold_;
 	double edgeThreshold_;
 	double sigma_;
+	bool rootSIFT_;
 
 	cv::Ptr<CV_SIFT> _sift;
 };
@@ -244,7 +295,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureOrb;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -275,8 +326,8 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureUndef;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const {return cv::Mat();}
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat &, std::vector<cv::KeyPoint> &) const {return cv::Mat();}
 
 private:
 	int threshold_;
@@ -287,6 +338,14 @@ private:
 	int maxThreshold_;
 	int gridRows_;
 	int gridCols_;
+	int fastCV_;
+
+	bool fastCVinit_;
+	int fastCVMaxFeatures_;
+	int fastCVLastImageHeight_;
+	uint32_t* fastCVCorners_= NULL;
+	uint32_t* fastCVCornerScores_ = NULL;
+	void* fastCVTempBuf_ = NULL;
 
 	cv::Ptr<cv::FeatureDetector> _fast;
 	cv::Ptr<CV_FAST_GPU> _gpuFast;
@@ -343,7 +402,7 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 
 private:
 	double _qualityLevel;
@@ -424,7 +483,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureBrisk;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -446,7 +505,7 @@ public:
 	virtual Feature2D::Type getType() const { return kFeatureKaze; }
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -462,6 +521,52 @@ private:
 #endif
 };
 
+//ORB OCTREE
+class RTABMAP_EXP ORBOctree : public Feature2D
+{
+public:
+	ORBOctree(const ParametersMap & parameters = ParametersMap());
+	virtual ~ORBOctree();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureOrbOctree;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	float scaleFactor_;
+	int nLevels_;
+	int fastThreshold_;
+	int fastMinThreshold_;
+
+	cv::Ptr<ORBextractor> _orb;
+	cv::Mat descriptors_;
+};
+
+//SuperPointTorch
+class RTABMAP_EXP SuperPointTorch : public Feature2D
+{
+public:
+	SuperPointTorch(const ParametersMap & parameters = ParametersMap());
+	virtual ~SuperPointTorch();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureSuperPointTorch; }
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+	cv::Ptr<SPDetector> superPoint_;
+
+	std::string path_;
+	float threshold_;
+	bool nms_;
+	int minDistance_;
+	bool cuda_;
+};
 
 }
 

corelib/include/rtabmap/core/GPS.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2018, 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_GPS_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
+
+#include <rtabmap/core/GeodeticCoords.h>
+
+namespace rtabmap {
+
+class GPS
+{
+public:
+	GPS():
+		stamp_(0.0),
+		longitude_(0.0),
+		latitude_(0.0),
+		altitude_(0.0),
+		error_(0.0),
+		bearing_(0.0)
+	{}
+	GPS(const double & stamp,
+		const double & longitude,
+		const double & latitude,
+		const double & altitude,
+		const double & error,
+		const double & bearing):
+			stamp_(stamp),
+			longitude_(longitude),
+			latitude_(latitude),
+			altitude_(altitude),
+			error_(error),
+			bearing_(bearing)
+	{}
+	const double & stamp() const {return stamp_;}
+	const double & longitude() const {return longitude_;}
+	const double & latitude() const {return latitude_;}
+	const double & altitude() const {return altitude_;}
+	const double & error() const {return error_;}
+	const double & bearing() const {return bearing_;}
+
+	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
+private:
+	double stamp_;     // in sec
+	double longitude_; // DD
+	double latitude_;  // DD
+	double altitude_;  // m
+	double error_;     // m
+	double bearing_;   // deg (North 0->360 clockwise)
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_ */

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -70,6 +70,10 @@ public:
 	void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
 
 	static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+	static cv::Point3d Geocentric_WGS84ToENU_WGS84(
+			const cv::Point3d & geocentric_WGS84,
+			const cv::Point3d & origin_geocentric_WGS84,
+			const GeodeticCoords & origin);
 
 private:
 	double latitude_;  // deg
@@ -77,47 +81,6 @@ private:
 	double altitude_;  // m
 };
 
-class GPS
-{
-public:
-	GPS():
-		stamp_(0.0),
-		longitude_(0.0),
-		latitude_(0.0),
-		altitude_(0.0),
-		error_(0.0),
-		bearing_(0.0)
-	{}
-	GPS(const double & stamp,
-		const double & longitude,
-		const double & latitude,
-		const double & altitude,
-		const double & error,
-		const double & bearing):
-			stamp_(stamp),
-			longitude_(longitude),
-			latitude_(latitude),
-			altitude_(altitude),
-			error_(error),
-			bearing_(bearing)
-	{}
-	const double & stamp() const {return stamp_;}
-	const double & longitude() const {return longitude_;}
-	const double & latitude() const {return latitude_;}
-	const double & altitude() const {return altitude_;}
-	const double & error() const {return error_;}
-	const double & bearing() const {return bearing_;}
-
-	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
-private:
-	double stamp_;     // in sec
-	double longitude_; // DD
-	double latitude_;  // DD
-	double altitude_;  // m
-	double error_;     // m
-	double bearing_;   // deg (North 0->360 clockwise)
-};
-
 }
 
 #endif /* GEODETICCOORDS_H_ */

corelib/include/rtabmap/core/GlobalDescriptor.h

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class GlobalDescriptor
+{
+
+public:
+	GlobalDescriptor(int type, const cv::Mat & data, const cv::Mat & info = cv::Mat()) :
+		type_(type),
+		info_(info),
+		data_(data)
+	{}
+	GlobalDescriptor() :
+		type_(-1) // Not set
+	{}
+	virtual ~GlobalDescriptor() {}
+
+	int type() const {return type_;}
+	const cv::Mat info() const {return info_;}
+	const cv::Mat data() const {return data_;}
+
+private:
+	int type_;
+	cv::Mat info_;
+	cv::Mat data_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/Graph.h

@@ -32,8 +32,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <map>
 #include <list>
+#include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Link.h>
-#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/core/GPS.h>
 
 namespace rtabmap {
 class Memory;
@@ -44,17 +45,17 @@ namespace graph {
 // Graph utilities
 ////////////////////////////////////////////
 
-bool RTABMAP_EXP exportPoses(
+	bool RTABMAP_EXP exportPoses(
 		const std::string & filePath,
-		int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 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), 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
-		bool g2oRobust = false); // optional for format 4
+		const ParametersMap & parameters = ParametersMap()); // optional for formats 3 and 4
 
 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 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), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 		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
@@ -78,6 +79,19 @@ void RTABMAP_EXP calcKittiSequenceErrors(
 		float & t_err,
 		float & r_err);
 
+/**
+ * Compute average of translation and rotation errors between each poses.
+ * @param poses_gt, Ground Truth poses
+ * @param poses_result, Estimated poses
+ * @param t_err, Output translation error (m)
+ * @param r_err, Output rotation error (deg)
+ */
+void RTABMAP_EXP calcRelativeErrors (
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err);
+
 /**
  * Compute root-mean-square error (RMSE) like the TUM RGBD
  * dataset's evaluation tool (absolute trajectory error).
@@ -102,6 +116,18 @@ Transform RTABMAP_EXP calcRMSE(
 		float & rotational_min,
 		float & rotational_max);
 
+void RTABMAP_EXP computeMaxGraphErrors(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		float & maxLinearErrorRatio,
+		float & maxAngularErrorRatio,
+		float & maxLinearError,
+		float & maxAngularError,
+		const Link ** maxLinearErrorLink = 0,
+		const Link ** maxAngularErrorLink = 0);
+
+std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links);
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
@@ -116,12 +142,16 @@ std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
 		const std::multimap<int, Link> & links,
 		int from,
 		int to,
-		bool checkBothWays = true);
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
 std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		const std::multimap<int, int> & links,
 		int from,
 		int to,
 		bool checkBothWays = true);
+std::list<Link> RTABMAP_EXP findLinks(
+		const std::multimap<int, Link> & links,
+		int from);
 
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);

corelib/include/rtabmap/core/IMU.h

@@ -19,7 +19,7 @@ class IMU
 {
 public:
 	IMU() {}
-	IMU(const cv::Vec4d & orientation,
+	IMU(const cv::Vec4d & orientation, // qx qy qz qw
 		const cv::Mat & orientationCovariance,
 		const cv::Vec3d & angularVelocity,
 		const cv::Mat & angularVelocityCovariance,
@@ -34,9 +34,6 @@ public:
 			linearAccelerationCovariance_(linearAccelerationCovariance),
 			localTransform_(localTransform)
 	{
-		UASSERT(!orientationCovariance.empty() && orientationCovariance.cols == 3 && orientationCovariance.rows == 3 && orientationCovariance.type() == CV_64FC1);
-		UASSERT(!angularVelocityCovariance.empty() && angularVelocityCovariance.cols == 3 && angularVelocityCovariance.rows == 3 && angularVelocityCovariance.type() == CV_64FC1);
-		UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
 	}
 	IMU(const cv::Vec3d & angularVelocity,
 		const cv::Mat & angularVelocityCovariance,
@@ -49,10 +46,9 @@ public:
 			linearAccelerationCovariance_(linearAccelerationCovariance),
 			localTransform_(localTransform)
 	{
-		UASSERT(!angularVelocityCovariance.empty() && angularVelocityCovariance.cols == 3 && angularVelocityCovariance.rows == 3 && angularVelocityCovariance.type() == CV_64FC1);
-		UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
 	}
 
+	// qx qy qz qw
 	const cv::Vec4d & orientation() const {return orientation_;}
 	const cv::Mat & orientationCovariance() const {return orientationCovariance_;} // 3x3 double Row major about x, y, z axes, empty if orientation is not set
 
@@ -66,10 +62,9 @@ public:
 
 	bool empty() const
 	{
-		return orientationCovariance_.empty() && angularVelocityCovariance_.empty() && linearAccelerationCovariance_.empty();
+		return localTransform_.isNull();
 	}
 
-
 private:
 	cv::Vec4d orientation_;
 	cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set

corelib/include/rtabmap/core/IMUFilter.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2019, 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_IMUFILTER_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
+
+#include <rtabmap/core/Parameters.h>
+#include <Eigen/Geometry>
+
+namespace rtabmap {
+
+class IMUFilter
+{
+public:
+	enum Type {
+		kMadgwick=0,
+		kComplementaryFilter=1};
+public:
+	static IMUFilter * create(const ParametersMap & parameters = ParametersMap());
+	static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual ~IMUFilter(){}
+
+	void update(
+			double gx, double gy, double gz,
+			double ax, double ay, double az,
+			double stamp);
+
+	virtual IMUFilter::Type type() const = 0;
+	virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const = 0;
+	virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0) = 0;
+
+protected:
+	IMUFilter(const ParametersMap & parameters = ParametersMap()) : previousStamp_(0) {}
+
+private:
+	// Update from accelerometer and gyroscope data.
+	// [gx, gy, gz]: Angular veloctiy, in rad / s.
+	// [ax, ay, az]: Normalized gravity vector.
+	// dt: time delta, in seconds.
+	virtual void updateImpl(
+			double gx, double gy, double gz,
+			double ax, double ay, double az,
+			double dt) = 0;
+
+private:
+	double previousStamp_;
+};
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ */

corelib/include/rtabmap/core/Landmark.h

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2018, 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_LANDMARK_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap {
+
+class Landmark
+{
+public:
+	Landmark() :
+		id_(0)
+	{}
+	Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
+		id_(id),
+		pose_(pose),
+		covariance_(covariance)
+	{
+		UASSERT(id_>0);
+		UASSERT(!pose_.isNull());
+		UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+	}
+
+	virtual ~Landmark() {}
+
+	const int & id() const {return id_;}
+	const Transform & pose() const {return pose_;}
+	const cv::Mat & covariance() const {return covariance_;}
+
+private:
+	int id_;
+	Transform pose_;
+	cv::Mat covariance_;
+};
+
+typedef std::map<int, Landmark> Landmarks;
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */

corelib/include/rtabmap/core/LaserScan.h

@@ -49,21 +49,52 @@ public:
 		kXYZINormal=9,
 		kXYZRGBNormal=10};
 
-	static int channels(Format format);
+	static std::string formatName(const Format & format);
+	static int channels(const Format & format);
 	static bool isScan2d(const Format & format);
 	static bool isScanHasNormals(const Format & format);
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
-	static LaserScan backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints = 0, int maxRange = 0, const Transform & localTransform = Transform::getIdentity());
+	static LaserScan backwardCompatibility(
+			const cv::Mat & oldScanFormat,
+			int maxPoints = 0,
+			int maxRange = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	static LaserScan backwardCompatibility(
+			const cv::Mat & oldScanFormat,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleInc,
+			const Transform & localTransform = Transform::getIdentity());
 
 public:
 	LaserScan();
-	LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform = Transform::getIdentity());
+	LaserScan(const cv::Mat & data,
+			int maxPoints,
+			float maxRange,
+			Format format,
+			const Transform & localTransform = Transform::getIdentity());
+	LaserScan(const cv::Mat & data,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity());
 
 	const cv::Mat & data() const {return data_;}
-	int maxPoints() const {return maxPoints_;}
-	float maxRange() const {return maxRange_;}
 	Format format() const {return format_;}
+	std::string formatName() const {return formatName(format_);}
+	int channels() const {return data_.channels();}
+	int maxPoints() const {return maxPoints_;}
+	float rangeMin() const {return rangeMin_;}
+	float rangeMax() const {return rangeMax_;}
+	float angleMin() const {return angleMin_;}
+	float angleMax() const {return angleMax_;}
+	float angleIncrement() const {return angleIncrement_;}
 	Transform localTransform() const {return localTransform_;}
 
 	bool isEmpty() const {return data_.empty();}
@@ -74,7 +105,7 @@ public:
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
-	LaserScan clone() const {return LaserScan(data_.clone(), maxPoints_, maxRange_, format_, localTransform_.clone());}
+	LaserScan clone() const;
 
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
@@ -84,9 +115,13 @@ public:
 
 private:
 	cv::Mat data_;
-	int maxPoints_;
-	float maxRange_;
 	Format format_;
+	int maxPoints_;
+	float rangeMin_;
+	float rangeMax_;
+	float angleMin_;
+	float angleMax_;
+	float angleIncrement_;
 	Transform localTransform_;
 };
 

corelib/include/rtabmap/core/Link.h

@@ -46,8 +46,16 @@ public:
 		kUserClosure,
 		kVirtualClosure,
 		kNeighborMerged,
-		kPosePrior,
+		kPosePrior, // Absolute pose in /world frame, From == To
+		kLandmark,  // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To" (landmark is negative id)
+		kGravity,  // Orientation of the base frame accordingly to gravity (From == To)
+		kEnd,
+		kSelfRefLink = 97, // Include kPosePrior and kGravity (all links where From=To)
+		kAllWithLandmarks = 98,
+		kAllWithoutLandmarks = 99,
 		kUndef = 99};
+	static std::string typeName(Type type);
+
 	Link();
 	Link(int from,
 			int to,
@@ -56,15 +64,16 @@ public:
 			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
 			const cv::Mat & userData = cv::Mat());
 
-	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
+	bool isValid() const {return from_ != 0 && to_ != 0 && !transform_.isNull() && type_!=kUndef;}
 
 	int from() const {return from_;}
 	int to() const {return to_;}
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
+	std::string typeName() const {return typeName(type_);}
 	const cv::Mat & infMatrix() const {return infMatrix_;}
-	double rotVariance() const;
-	double transVariance() const;
+	double rotVariance(bool minimum = true) const;
+	double transVariance(bool minimum = true) const;
 
 	void setFrom(int from) {from_ = from;}
 	void setTo(int to) {to_ = to;}

corelib/include/rtabmap/core/MarkerDetector.h

@@ -0,0 +1,60 @@
+/*
+Copyright (c) 2010-2019, 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_MARKERDETECTOR_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/CameraModel.h>
+#include <opencv2/opencv_modules.hpp>
+
+#ifdef HAVE_OPENCV_ARUCO
+#include <opencv2/aruco.hpp>
+#endif
+
+namespace rtabmap {
+
+class MarkerDetector {
+public:
+	MarkerDetector(const ParametersMap & parameters = ParametersMap());
+	virtual ~MarkerDetector();
+	void parseParameters(const ParametersMap & parameters);
+	std::map<int, Transform> detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth = cv::Mat(), float * estimatedMarkerLength = 0, cv::Mat * imageWithDetections = 0);
+
+private:
+#ifdef HAVE_OPENCV_ARUCO
+	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
+	float markerLength_;
+	float maxDepthError_;
+	int dictionaryId_;
+	cv::Ptr<cv::aruco::Dictionary> dictionary_;
+#endif
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_ */

corelib/include/rtabmap/core/Memory.h

@@ -57,6 +57,7 @@ class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
 class OccupancyGrid;
+class MarkerDetector;
 
 class RTABMAP_EXP Memory
 {
@@ -92,7 +93,7 @@ public:
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
 	int cleanup();
-	void saveStatistics(const Statistics & statistics);
+	void saveStatistics(const Statistics & statistics, bool saveWMState);
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -146,21 +147,27 @@ public:
 	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
 	const std::set<int> & getStMem() const {return _stMem;}
 	int getMaxStMemSize() const {return _maxStMemSize;}
-	std::map<int, Link> getNeighborLinks(int signatureId,
+	std::multimap<int, Link> getNeighborLinks(int signatureId,
 			bool lookInDatabase = false) const;
-	std::map<int, Link> getLoopClosureLinks(int signatureId,
+	std::multimap<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
-	std::map<int, Link> getLinks(int signatureId,
-			bool lookInDatabase = false) const;
-	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const;
+	std::multimap<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
+			bool lookInDatabase = false,
+			bool withLandmarks = false) const;
+	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	bool isBinDataKept() const {return _binDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	int getLastSignatureId() const;
 	const Signature * getLastWorkingSignature() const;
+	std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
 	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
 	bool labelSignature(int id, const std::string & label);
-	std::map<int, std::string> getAllLabels() const;
+	const std::map<int, std::string> & getAllLabels() const {return _labels;}
+	const std::map<int, std::set<int> > & getLandmarksIndex() const {return _landmarksIndex;}
+	const std::map<int, std::set<int> > & getLandmarksInvertedIndex() const {return _landmarksInvertedIndex;}
+	bool allNodesInWM() const {return _allNodesInWM;}
+
 	/**
 	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
 	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
@@ -171,9 +178,13 @@ public:
 	bool setUserData(int id, const cv::Mat & data);
 	int getDatabaseMemoryUsed() const; // in bytes
 	std::string getDatabaseVersion() const;
+	std::string getDatabaseUrl() const;
 	double getDbSavingTime() const;
+	int getMapId(int id, bool lookInDatabase = false) const;
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
 	Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
+	const std::map<int, Transform> & getGroundTruths() const {return _groundTruths;} // only those in working+STM memory
+	void getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth = 0) const;
 	bool getNodeInfo(int signatureId,
 			Transform & odomPose,
 			int & mapId,
@@ -183,20 +194,22 @@ public:
 			Transform & groundTruth,
 			std::vector<float> & velocity,
 			GPS & gps,
+			EnvSensors & sensors,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
-	void getNodeWords(int nodeId,
+	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, cv::Mat> & wordsDescriptors,
+			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
-			StereoCameraModel & stereoModel);
-	SensorData getSignatureDataConst(int locationId, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
-	std::set<int> getAllSignatureIds() const;
+			StereoCameraModel & stereoModel) const;
+	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
+	bool isLocalizationDataSaved() const {return _localizationDataSaved;}
 	const Signature * getSignature(int id) const;
 	bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
@@ -205,6 +218,8 @@ public:
 	int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
 	const Feature2D * getFeature2D() const {return _feature2D;}
 	bool isGraphReduced() const {return _reduceGraph;}
+	const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;}
+	bool isOdomGravityUsed() const {return _useOdometryGravity;}
 
 	void dumpMemoryTree(const char * fileNameTree) const;
 	virtual void dumpMemory(std::string directory) const;
@@ -221,7 +236,8 @@ public:
 			const std::set<int> & ids,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
-			bool lookInDatabase = false);
+			bool lookInDatabase = false,
+			bool landmarksAdded = false);
 
 	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
 	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
@@ -274,7 +290,9 @@ private:
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
 	bool _saveIntermediateNodeData;
+	std::string _rgbCompressionFormat;
 	bool _incrementalMemory;
+	bool _localizationDataSaved;
 	bool _reduceGraph;
 	int _maxStMemSize;
 	float _recentWmRatio;
@@ -284,22 +302,29 @@ private:
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
 	bool _depthAsMask;
+	bool _stereoFromMotion;
 	int _imagePreDecimation;
 	int _imagePostDecimation;
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
-	int _laserScanNormalRadius;
+	float _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
+	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
+	bool _useOdometryGravity;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
-	int _visCorType;
 	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
+	bool _covOffDiagonalIgnored;
+	bool _detectMarkers;
+	float _markerLinVariance;
+	float _markerAngVariance;
 
 	int _idCount;
 	int _idMapCount;
@@ -308,16 +333,24 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
+	bool _allNodesInWM;
 	GPS _gpsOrigin;
+	std::vector<CameraModel> _rectCameraModels;
+	StereoCameraModel _rectStereoCameraModel;
+	std::vector<double> _odomMaxInf;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id
 	std::map<int, double> _workingMem; // id,age
+	std::map<int, Transform> _groundTruths;
+	std::map<int, std::string> _labels;
+	std::map<int, std::set<int> > _landmarksIndex;         // <nodeId, landmarkIds>
+	std::map<int, std::set<int> > _landmarksInvertedIndex; // <landmarkId, nodeIds>
 
 	//Keypoint stuff
 	VWDictionary * _vwd;
 	Feature2D * _feature2D;
-	float _badSignRatio;;
+	float _badSignRatio;
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
 
@@ -325,6 +358,8 @@ private:
 	RegistrationIcp * _registrationIcpMulti;
 
 	OccupancyGrid * _occupancy;
+
+	MarkerDetector * _markerDetector;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -39,6 +39,17 @@ namespace rtabmap {
 
 class RTABMAP_EXP OccupancyGrid
 {
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
@@ -53,6 +64,7 @@ public:
 	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
+	const std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > & getCache() const {return cache_;}
 
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -85,8 +97,9 @@ public:
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
 			const cv::Mat & empty);
-	void update(const std::map<int, Transform> & poses);
+	bool update(const std::map<int, Transform> & poses); // return true if map has changed
 	cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getProbMap(float & xMin, float & yMin) const;
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
@@ -125,6 +138,11 @@ private:
 	bool erode_;
 	float footprintRadius_;
 	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;
 
 	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
 	cv::Mat map_;

corelib/include/rtabmap/core/OctoMap.h

@@ -84,6 +84,7 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTr
   public:
     /// Default constructor, sets resolution of leafs
 	RtabmapColorOcTree(double resolution);
+	virtual ~RtabmapColorOcTree() {}
 
     /// virtual constructor: creates a new object of same type
     /// (Covariant return type requires an up-to-date compiler)
@@ -184,7 +185,7 @@ public:
 			const cv::Mat & obstacles,
 			const cv::Mat & empty,
 			const cv::Point3f & viewPoint);
-	void update(const std::map<int, Transform> & poses);
+	bool update(const std::map<int, Transform> & poses); // return true if map has changed
 
 	const RtabmapColorOcTree * octree() const {return octree_;}
 
@@ -193,7 +194,9 @@ public:
 			std::vector<int> * obstacleIndices = 0,
 			std::vector<int> * emptyIndices = 0,
 			std::vector<int> * groundIndices = 0,
-			bool originalRefPoints = true) const;
+			bool originalRefPoints = true,
+			std::vector<int> * frontierIndices = 0,
+			std::vector<double> * cloudProb = 0) const;
 
 	cv::Mat createProjectionMap(
 			float & xMin,
@@ -223,7 +226,6 @@ private:
 	std::map<int, cv::Point3f> cacheViewPoints_;
 	RtabmapColorOcTree * octree_;
 	std::map<int, Transform> addedNodes_;
-	octomap::KeyRay keyRay_;
 	bool hasColor_;
 	bool fullUpdate_;
 	float updateError_;

corelib/include/rtabmap/core/Odometry.h

@@ -50,11 +50,14 @@ public:
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
 		kTypeORBSLAM2 = 5,
-		kTypeOkvis = 6
+		kTypeOkvis = 6,
+		kTypeLOAM = 7,
+		kTypeMSCKF = 8,
+		kTypeVINS = 9
 	};
 
 public:
-	static Odometry * create(const ParametersMap & parameters);
+	static Odometry * create(const ParametersMap & parameters = ParametersMap());
 	static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
 
 public:
@@ -64,15 +67,20 @@ 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;}
 
 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
-	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
+	RTABMAP_DEPRECATED(const Transform & previousVelocityTransform() const, "Use getVelocityGuess() instead.");
+	const Transform & getVelocityGuess() const {return velocityGuess_;}
 	double previousStamp() const {return previousStamp_;}
 	unsigned int framesProcessed() const {return framesProcessed_;}
 	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
 
+protected:
+	const std::map<double, Transform> & imus() const {return imus_;}
+
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
 
@@ -85,6 +93,7 @@ private:
 	bool _force3DoF;
 	bool _holonomic;
 	bool guessFromMotion_;
+	float guessSmoothingDelay_;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -101,13 +110,17 @@ private:
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
-	Transform previousVelocityTransform_;
+	std::list<std::pair<std::vector<float>, double> > previousVelocities_;
+	Transform velocityGuess_;
+	Transform imuLastTransform_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 	unsigned int framesProcessed_;
 
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
+	StereoCameraModel stereoModel_;
+	std::map<double, Transform> imus_;
 
 protected:
 	Odometry(const rtabmap::ParametersMap & parameters);

corelib/include/rtabmap/core/OdometryInfo.h

@@ -56,6 +56,8 @@ public:
 		interval(0),
 		distanceTravelled(0.0f),
 		memoryUsage(0),
+		gravityRollError(0.0),
+		gravityPitchError(0.0),
 		type(0)
 	{}
 
@@ -81,8 +83,11 @@ public:
 		output.transform = transform;
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
+		output.guessVelocity = guessVelocity;
 		output.distanceTravelled = distanceTravelled;
 		output.memoryUsage = memoryUsage;
+		output.gravityRollError = gravityRollError;
+		output.gravityPitchError = gravityPitchError;
 		output.type = type;
 		return output;
 	}
@@ -106,8 +111,11 @@ public:
 	Transform transform;
 	Transform transformFiltered;
 	Transform transformGroundTruth;
+	Transform guessVelocity;
 	float distanceTravelled;
 	int memoryUsage; //MB
+	double gravityRollError;
+	double gravityPitchError;
 
 	int type;
 

corelib/include/rtabmap/core/Optimizer.h

@@ -38,6 +38,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+class FeatureBA
+{
+public:
+	FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat()):
+		kpt(kptIn),
+		depth(depthIn),
+		descriptor(descriptorIn)
+	{}
+	cv::KeyPoint kpt;
+	float depth;
+	cv::Mat descriptor;
+};
+
 ////////////////////////////////////////////
 // Graph optimizers
 ////////////////////////////////////////////
@@ -49,20 +62,20 @@ public:
 		kTypeTORO = 0,
 		kTypeG2O = 1,
 		kTypeGTSAM = 2,
-		kTypeCVSBA = 3
+		kTypeCeres = 3,
+		kTypeCVSBA = 4
 	};
 	static bool isAvailable(Optimizer::Type type);
 	static Optimizer * create(const ParametersMap & parameters);
 	static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
 
 	// Get connected poses and constraints from a set of links
-	static void getConnectedGraph(
+	void getConnectedGraph(
 			int fromId,
 			const std::map<int, Transform> & posesIn,
-			const std::multimap<int, Link> & linksIn, // only one link between two poses
+			const std::multimap<int, Link> & linksIn,
 			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut,
-			int depth = 0);
+			std::multimap<int, Link> & linksOut) const;
 
 public:
 	virtual ~Optimizer() {}
@@ -76,6 +89,8 @@ public:
 	double epsilon() const {return epsilon_;}
 	bool isRobust() const {return robust_;}
 	bool priorsIgnored() const {return priorsIgnored_;}
+	bool landmarksIgnored() const {return landmarksIgnored_;}
+	float gravitySigma() const {return gravitySigma_;}
 
 	// setters
 	void setIterations(int iterations) {iterations_ = iterations;}
@@ -84,6 +99,8 @@ public:
 	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
 	void setRobust(bool enabled) {robust_ = enabled;}
 	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
+	void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
+	void setGravitySigma(float value) {gravitySigma_ = value;}
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
@@ -95,34 +112,53 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 
+	std::map<int, Transform> optimize(
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				cv::Mat & outputCovariance,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
+			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
 			std::set<int> * outliers = 0);
 
 	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, Signature> & signatures);
+			const std::map<int, Signature> & signatures,
+			std::map<int, cv::Point3f> & points3DMap,
+			std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			bool rematchFeatures = false);
+
+	std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			bool rematchFeatures = false);
 
 	Transform optimizeBA(
 			const Link & link,
 			const CameraModel & model,
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
 			std::set<int> * outliers = 0);
 
 	void computeBACorrespondences(
@@ -130,7 +166,8 @@ public:
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
-			std::map<int, std::map<int, cv::Point3f> > & wordReferences); // <ID words, IDs frames + keypoint/depth>
+			std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			bool rematchFeatures = false);
 
 protected:
 	Optimizer(
@@ -139,7 +176,9 @@ protected:
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
 			bool robust            = Parameters::defaultOptimizerRobust(),
-			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored());
+			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored(),
+			bool landmarksIgnored  = Parameters::defaultOptimizerLandmarksIgnored(),
+			float gravitySigma     = Parameters::defaultOptimizerGravitySigma());
 	Optimizer(const ParametersMap & parameters);
 
 private:
@@ -149,6 +188,8 @@ private:
 	double epsilon_;
 	bool robust_;
 	bool priorsIgnored_;
+	bool landmarksIgnored_;
+	float gravitySigma_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Version.h" // DLL export/import defines
 #include <rtabmap/utilite/UConversion.h>
+#include <opencv2/core/version.hpp>
+#include <opencv2/opencv_modules.hpp>
 #include <string>
 #include <map>
 
@@ -62,7 +64,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
 #define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \
     public: \
         static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
-        static TYPE default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+        static TYPE default##PREFIX##NAME() {return (TYPE)DEFAULT_VALUE;} \
         static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
     private: \
         class Dummy##PREFIX##NAME { \
@@ -71,7 +73,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
                                    parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
                                    descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
         }; \
-        Dummy##PREFIX##NAME dummy##PREFIX##NAME;
+        Dummy##PREFIX##NAME dummy##PREFIX##NAME
 // end define PARAM
 
 /**
@@ -105,7 +107,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
                                    parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, "string")); \
                                    descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
         }; \
-        Dummy##PREFIX##NAME dummy##PREFIX##NAME;
+        Dummy##PREFIX##NAME dummy##PREFIX##NAME
 // end define PARAM
 
 /**
@@ -138,7 +140,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
                                    parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
                                    descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
         }; \
-        Dummy##PREFIX##NAME dummy##PREFIX##NAME;
+        Dummy##PREFIX##NAME dummy##PREFIX##NAME
 // end define PARAM
 
 /**
@@ -175,8 +177,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, PublishRAMUsage,              bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
     RTABMAP_PARAM(Rtabmap, ComputeRMSE,                  bool, true,  "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
     RTABMAP_PARAM(Rtabmap, SaveWMState,                  bool, false, "Save working memory state after each update in statistics.");
-    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for the detector (ms) (0 means infinity).");
-    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      "Maximum signatures in the Working Memory (ms) (0 means infinity).");
+    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for map update (ms) (0 means infinity). When map update time exceeds this fixed time threshold, some nodes in Working Memory (WM) are transferred to Long-Term Memory to limit the size of the WM and decrease the update time.");
+    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      uFormat("Maximum nodes in the Working Memory (0 means infinity). Similar to \"%s\", when the number of nodes in Working Memory (WM) exceeds this treshold, some nodes are transferred to Long-Term Memory to keep WM size fixed.", kRtabmapTimeThr().c_str()));
     RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
     RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
     RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
@@ -186,11 +188,14 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
     RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
     RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
+    RTABMAP_PARAM(Rtabmap, StartNewMapOnGoodSignature,   bool, false, uFormat("Start a new map only if the first signature is not bad (i.e., has enough features, see %s).", kKpBadSignRatio().c_str()));
     RTABMAP_PARAM(Rtabmap, ImagesAlreadyRectified,       bool, true,  "Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.");
+    RTABMAP_PARAM(Rtabmap, RectifyOnlyFeatures,          bool, false,  uFormat("If \"%s\" is false and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.", kRtabmapImagesAlreadyRectified().c_str()));
 
     // Hypotheses selection
     RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
     RTABMAP_PARAM(Rtabmap, LoopRatio,         float, 0,         "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+    RTABMAP_PARAM(Rtabmap, LoopGPS,           bool,  true,      uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));
 
     // Memory
     RTABMAP_PARAM(Mem, RehearsalSimilarity,         float, 0.6,     "Rehearsal similarity.");
@@ -201,8 +206,10 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, SaveDepth16Format,           bool, false,    "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
     RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
     RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
+    RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
     RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
     RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
+    RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,     uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
     RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
     RTABMAP_PARAM(Mem, RecentWmRatio,               float, 0.2,     "Ratio of locations after the last loop closure in WM that cannot be transferred.");
     RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
@@ -212,14 +219,17 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
     RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
-    RTABMAP_PARAM(Mem, ImagePreDecimation,          int, 1,         "Image decimation (>=1) before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
-    RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
+    RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
+    RTABMAP_PARAM(Mem, ImagePreDecimation,          int, 1,         "Image decimation (>=1) before features extraction.");
+    RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
     RTABMAP_PARAM(Mem, CompressionParallelized,     bool, true,     "Compression of sensor data is multi-threaded.");
     RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
+    RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()));
     RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       float, 0.0,     "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features instead of regenerating them.");
+    RTABMAP_PARAM(Mem, UseOdomGravity,              bool, false,    uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
+    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
 
     // KeypointMemory (Keypoint-based)
     RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -231,15 +241,11 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
     RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
     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.)");
-#ifndef RTABMAP_NONFREE
-#ifdef RTABMAP_OPENCV3
-    // OpenCV 3 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.");
+#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.");
 #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.");
-#endif
-#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.");
+    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.");
 #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.");
@@ -273,6 +279,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04, "The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector.");
     RTABMAP_PARAM(SIFT, EdgeThreshold,     double, 10,   "The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
     RTABMAP_PARAM(SIFT, Sigma,             double, 1.6,  "The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
+    RTABMAP_PARAM(SIFT, RootSIFT,          bool,  false, "Apply RootSIFT normalization of the descriptors.");
 
     RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
 
@@ -282,8 +289,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, "Used with FAST GPU.");
     RTABMAP_PARAM(FAST, MinThreshold,       int, 7,       "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
     RTABMAP_PARAM(FAST, MaxThreshold,       int, 200,     "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
-    RTABMAP_PARAM(FAST, GridRows,           int, 4,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
-    RTABMAP_PARAM(FAST, GridCols,           int, 4,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridRows,           int, 0,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridCols,           int, 0,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    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,    "");
@@ -311,11 +319,17 @@ class RTABMAP_EXP Parameters
 
     RTABMAP_PARAM(KAZE, Extended,       bool, false,   "Set to enable extraction of extended (128-byte) descriptor.");
     RTABMAP_PARAM(KAZE, Upright,        bool, false,   "Set to enable use of upright descriptors (non rotation-invariant).");
-    RTABMAP_PARAM(KAZE, Threshold,      float, 0.001,  "Detector response threshold to accept point.");
+    RTABMAP_PARAM(KAZE, Threshold,      float, 0.001,  "Detector response threshold to accept keypoint.");
     RTABMAP_PARAM(KAZE, NOctaves,       int, 4,        "Maximum octave evolution of the image.");
     RTABMAP_PARAM(KAZE, NOctaveLayers,  int, 4,        "Default number of sublevels per scale level.");
     RTABMAP_PARAM(KAZE, Diffusivity,    int, 1,        "Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.");
 
+    RTABMAP_PARAM_STR(SuperPoint, ModelPath, "",           "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
+    RTABMAP_PARAM(SuperPoint, Threshold,     float, 0.010, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPoint, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    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.");
+
     // 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, ...}.");
@@ -335,7 +349,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     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, 1.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, 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, 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.");
@@ -348,7 +363,11 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,    "Save scan matching IDs 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, 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.");
+    RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
+    RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
+    RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  0,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false) and when %s!=0. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
 
     // Local/Proximity loop closure detection
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
@@ -359,26 +378,35 @@ class RTABMAP_EXP Parameters
     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.");
 
     // Graph optimization
 #ifdef RTABMAP_GTSAM
-    RTABMAP_PARAM(Optimizer, Strategy,        int, 2,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 2,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
     RTABMAP_PARAM(Optimizer, Iterations,      int, 20,         "Optimization iterations.");
     RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
 #else
 #ifdef RTABMAP_G2O
-    RTABMAP_PARAM(Optimizer, Strategy,        int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
     RTABMAP_PARAM(Optimizer, Iterations,      int, 20,         "Optimization iterations.");
     RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.0,     "Stop optimizing when the error improvement is less than this value.");
 #else
-    RTABMAP_PARAM(Optimizer, Strategy,        int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+#ifdef RTABMAP_CERES
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 3,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
+    RTABMAP_PARAM(Optimizer, Iterations,      int, 20,         "Optimization iterations.");
+    RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.000001, "Stop optimizing when the error improvement is less than this value.");
+#else
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
     RTABMAP_PARAM(Optimizer, Iterations,      int, 100,        "Optimization iterations.");
     RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
 #endif
+#endif
 #endif
     RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
     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.");
+    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()));
 
 #ifdef RTABMAP_ORB_SLAM2
     RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
@@ -393,12 +421,12 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
 
     // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion");
     RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
     RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
     RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
     RTABMAP_PARAM(Odom, ImageBufferSize,     unsigned int, 1, "Data buffer size (0 min inf).");
-    RTABMAP_PARAM(Odom, FilteringStrategy,      int, 0,       "0=No filtering 1=Kalman filtering 2=Particle filtering");
+    RTABMAP_PARAM(Odom, FilteringStrategy,      int, 0,       "0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.");
     RTABMAP_PARAM(Odom, ParticleSize,      unsigned int, 400, "Number of particles of the filter.");
     RTABMAP_PARAM(Odom, ParticleNoiseT,         float, 0.002, "Noise (m) of translation components (x,y,z).");
     RTABMAP_PARAM(Odom, ParticleLambdaT,        float, 100,   "Lambda of translation components (x,y,z).");
@@ -407,6 +435,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Odom, KalmanProcessNoise,     float, 0.001, "Process noise covariance value.");
     RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,  "Process measurement covariance value.");
     RTABMAP_PARAM(Odom, GuessMotion,            bool, true,   "Guess next transformation from the last motion computed.");
+    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate.", kOdomFilteringStrategy().c_str()));
     RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,   "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 150,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
@@ -419,10 +448,12 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
     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)
-    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+    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.");
+    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #endif
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
 
@@ -490,6 +521,45 @@ class RTABMAP_EXP Parameters
     // Odometry OKVIS
     RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
 
+    // Odometry LOAM
+    RTABMAP_PARAM(OdomLOAM, Sensor,     int,    2,    "Velodyne sensor: 0=VLP-16, 1=HDL-32, 2=HDL-64E");
+    RTABMAP_PARAM(OdomLOAM, ScanPeriod, float,  0.1,  "Scan period (s)");
+    RTABMAP_PARAM(OdomLOAM, LinVar,     float,  0.01,  "Linear output variance.");
+    RTABMAP_PARAM(OdomLOAM, AngVar,     float,  0.01,  "Angular output variance.");
+    RTABMAP_PARAM(OdomLOAM, LocalMapping, bool,  true,  "Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.");
+
+    // Odometry MSCKF_VIO
+    RTABMAP_PARAM(OdomMSCKF, GridRow,           int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridCol,           int,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMinFeatureNum, int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMaxFeatureNum, int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, PyramidLevels,     int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, PatchSize,         int,  15,  "");
+    RTABMAP_PARAM(OdomMSCKF, FastThreshold,     int,  10,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxIteration,      int,  30,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackPrecision,    double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, RansacThreshold,   double,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, StereoThreshold,   double,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, PositionStdThreshold,    double,  8.0,  "");
+    RTABMAP_PARAM(OdomMSCKF, RotationThreshold,       double,  0.2618,  "");
+    RTABMAP_PARAM(OdomMSCKF, TranslationThreshold,    double,  0.4,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackingRateThreshold,   double,  0.5,  "");
+    RTABMAP_PARAM(OdomMSCKF, OptTranslationThreshold, double,  0,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyro,     double,  0.005,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAcc,      double,  0.05,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyroBias, double,  0.001,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseFeature,  double,  0.035,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovVel,    double,  0.25,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovGyroBias, double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExRot,    double,  0.00030462,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExTrans,  double,  0.000025,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize,  int,  20,  "");
+
+    // Odometry VINS
+    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");
+
     // Common registration parameters
     RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
     RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -508,18 +578,17 @@ class RTABMAP_EXP Parameters
     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()));
 #endif
 
-    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.1,    uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
+    RTABMAP_PARAM(Vis, MeanInliersDistance,      float, 0.0,    "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.");
+    RTABMAP_PARAM(Vis, MinInliersDistribution,   float, 0.0,    "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled.");
+
     RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
-#ifndef RTABMAP_NONFREE
-#ifdef RTABMAP_OPENCV3
-    // OpenCV 3 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.");
+#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.");
 #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.");
-#endif
-#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.");
+    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.");
 #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).");
@@ -532,8 +601,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     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. 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 features matching approach.", kVisCorType().c_str()));
+    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, 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()));
@@ -541,16 +610,29 @@ class RTABMAP_EXP Parameters
     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)
-    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 1,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+    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.");
+    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(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.");
+	RTABMAP_PARAM_STR(PyMatcher, Model,        "indoor",   "For SuperGlue, set only \"indoor\" or \"outdoor\". For OANet, set path to one of the pth file (e.g., \"OANet/model/gl3d/sift-4000/model_best.pth\").");
+
+	RTABMAP_PARAM(GMS, WithRotation,         bool, false,   "Take rotation transformation into account.");
+	RTABMAP_PARAM(GMS, WithScale,            bool, false,   "Take scale transformation into account.");
+	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");
+
     // ICP registration parameters
     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, 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).");
 #ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1,   "Max distance for point correspondences.");
 #else
@@ -590,6 +672,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
     RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
 
+    RTABMAP_PARAM(Stereo, DenseStrategy,         int, 0,  "0=cv::StereoBM, 1=cv::StereoSGBM");
+
     RTABMAP_PARAM(StereoBM, BlockSize,           int, 15,       "See cv::StereoBM");
     RTABMAP_PARAM(StereoBM, MinDisparity,        int, 0,        "See cv::StereoBM");
     RTABMAP_PARAM(StereoBM, NumDisparities,      int, 128,      "See cv::StereoBM");
@@ -599,6 +683,23 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(StereoBM, TextureThreshold,    int, 10,       "See cv::StereoBM");
     RTABMAP_PARAM(StereoBM, SpeckleWindowSize,   int, 100,      "See cv::StereoBM");
     RTABMAP_PARAM(StereoBM, SpeckleRange,        int, 4,        "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, Disp12MaxDiff,       int, -1,       "See cv::StereoBM");
+
+    RTABMAP_PARAM(StereoSGBM, BlockSize,         int, 15,       "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, MinDisparity,      int, 0,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, NumDisparities,    int, 128,      "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, PreFilterCap,      int, 31,       "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, UniquenessRatio,   int, 20,       "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, SpeckleWindowSize, int, 100,      "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, SpeckleRange,      int, 4,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, Disp12MaxDiff,     int, 1,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, P1,                int, 2,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, P2,                int, 5,        "See cv::StereoSGBM");
+#if CV_MAJOR_VERSION < 3
+	RTABMAP_PARAM(StereoSGBM, Mode,              int, 0,        "See cv::StereoSGBM");
+#else
+	RTABMAP_PARAM(StereoSGBM, Mode,              int, 2,        "See cv::StereoSGBM");
+#endif
 
     // Occupancy Grid
     RTABMAP_PARAM(Grid, FromDepth,               bool,   true,    "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
@@ -639,7 +740,26 @@ 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, OctoMapOccupancyThr,  float,  0.5,     "OctoMap occupancy threshold (value between 0 and 1).");
+    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).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMin,      float,  0.1192,  "Probability clamping minimum (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
+
+    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
+    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
+    RTABMAP_PARAM(Marker, MaxDepthError,          float, 0.01,  uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
+    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(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].");
+
+    RTABMAP_PARAM(ImuFilter, ComplementaryGainAcc,          double, 0.01, "Gain parameter for the complementary filter, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, ComplementaryBiasAlpha,        double, 0.01, "Bias estimation gain parameter, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, ComplementaryDoBiasEstimation, bool,   true, "Parameter whether to do bias estimation or not.");
+    RTABMAP_PARAM(ImuFilter, ComplementaryDoAdpativeGain,   bool,   true, "Parameter whether to do adaptive gain or not.");
 
 public:
     virtual ~Parameters();

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -60,6 +60,8 @@ private:
 	float _maxRotation;
 	float _voxelSize;
 	int _downsamplingStep;
+	float _rangeMin;
+	float _rangeMax;
 	float _maxCorrespondenceDistance;
 	int _maxIterations;
 	float _epsilon;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -37,11 +37,15 @@ public:
 	RegistrationInfo() :
 		totalTime(0.0),
 		inliers(0),
+		inliersMeanDistance(0.0f),
+		inliersDistribution(0.0f),
 		matches(0),
 		icpInliersRatio(0),
 		icpTranslation(0.0f),
 		icpRotation(0.0f),
-		icpStructuralComplexity(0.0f)
+		icpStructuralComplexity(0.0f),
+		icpStructuralDistribution(0.0f),
+		icpCorrespondences(0)
 
 	{
 	}
@@ -53,11 +57,15 @@ public:
 		output.covariance = covariance.clone();
 		output.rejectedMsg = rejectedMsg;
 		output.inliers = inliers;
+		output.inliersMeanDistance = inliersMeanDistance;
+		output.inliersDistribution = inliersDistribution;
 		output.matches = matches;
 		output.icpInliersRatio = icpInliersRatio;
 		output.icpTranslation = icpTranslation;
 		output.icpRotation = icpRotation;
 		output.icpStructuralComplexity = icpStructuralComplexity;
+		output.icpStructuralDistribution = icpStructuralDistribution;
+		output.icpCorrespondences = icpCorrespondences;
 		return output;
 	}
 
@@ -67,6 +75,8 @@ public:
 
 	// RegistrationVis
 	int inliers;
+	float inliersMeanDistance;
+	float inliersDistribution;
 	std::vector<int> inliersIDs;
 	int matches;
 	std::vector<int> matchesIDs;
@@ -77,6 +87,8 @@ public:
 	float icpTranslation;
 	float icpRotation;
 	float icpStructuralComplexity;
+	float icpStructuralDistribution;
+	int   icpCorrespondences;
 };
 
 }

corelib/include/rtabmap/core/RegistrationVis.h

@@ -37,6 +37,10 @@ namespace rtabmap {
 
 class Feature2D;
 
+#ifdef RTABMAP_PYMATCHER
+class PyMatcher;
+#endif
+
 // Visual registration
 class RTABMAP_EXP RegistrationVis : public Registration
 {
@@ -50,8 +54,11 @@ public:
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
 	int getMinInliers() const {return _minInliers;}
+	int getNNType() const {return _nnType;}
+	float getNNDR() const {return _nndr;}
+	int getEstimationType() const {return _estimationType;}
 
-	Feature2D * createFeatureDetector() const; // for convenience
+	const Feature2D * getDetector() const {return _detectorFrom;}
 
 protected:
 	virtual Transform computeTransformationImpl(
@@ -81,13 +88,26 @@ private:
 	float _flowEps;
 	int _flowMaxLevel;
 	float _nndr;
+	int _nnType;
+	bool _gmsWithRotation;
+	bool _gmsWithScale;
+	double _gmsThresholdFactor;
 	int _guessWinSize;
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
 	bool _depthAsMask;
+	float _minInliersDistributionThr;
+	float _maxInliersMeanDistance;
 
 	ParametersMap _featureParameters;
 	ParametersMap _bundleParameters;
+
+	Feature2D * _detectorFrom;
+	Feature2D * _detectorTo;
+
+#ifdef RTABMAP_PYMATCHER
+	PyMatcher * _pyMatcher;
+#endif
 };
 
 }

corelib/include/rtabmap/core/Rtabmap.h

@@ -153,25 +153,40 @@ public:
 	void parseParameters(const ParametersMap & parameters);
 	const ParametersMap & getParameters() const {return _parameters;}
 	void setWorkingDirectory(std::string path);
-	void rejectLoopClosure(int oldId, int newId);
+	void rejectLastLoopClosure();
+	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses);
-	void get3DMap(std::map<int, Signature> & signatures,
-			std::map<int, Transform> & poses,
-			std::multimap<int, Link> & constraints,
-			bool optimized,
-			bool global) const;
+	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	RTABMAP_DEPRECATED(
+		void get3DMap(std::map<int, Signature> & signatures,
+				std::map<int, Transform> & poses,
+				std::multimap<int, Link> & constraints,
+				bool optimized,
+				bool global) const, "Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.");
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			bool optimized,
 			bool global,
-			std::map<int, Signature> * signatures = 0);
-	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1, const ProgressState * state = 0);
+			std::map<int, Signature> * signatures = 0,
+			bool withImages = false,
+			bool withScan = false,
+			bool withUserData = false,
+			bool withGrid = false) const;
+	int detectMoreLoopClosures(
+			float clusterRadius = 0.5f,
+			float clusterAngle = M_PI/6.0f,
+			int iterations = 1,
+			bool intraSession = true,
+			bool interSession = true,
+			const ProgressState * state = 0);
 	int refineLinks();
+	bool addLink(const Link & link);
+	cv::Mat getInformation(const cv::Mat & covariance) const;
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
 	void clearPath(int status); // -1=failed 0=idle/executing 1=success
 	bool computePath(int targetNode, bool global);
-	bool computePath(const Transform & targetPose); // only in current optimized map
+	bool computePath(const Transform & targetPose, float tolerance = -1.0f); // only in current optimized map, tolerance (m) < 0 means RGBD/LocalRadius, 0 means infinite
 	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
 	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
 	std::vector<int> getPathNextNodes() const;
@@ -181,7 +196,7 @@ public:
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
 	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
-	std::map<int, std::map<int, Transform> > getPaths(std::map<int, Transform> poses, const Transform & target, int maxGraphDepth = 0) const;
+	std::map<int, std::map<int, Transform> > getPaths(const std::map<int, Transform> & poses, const Transform & target, int maxGraphDepth = 0) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
@@ -190,6 +205,7 @@ private:
 	void optimizeCurrentMap(int id,
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
+			cv::Mat & covariance,
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
@@ -198,6 +214,7 @@ private:
 			const std::set<int> & ids,
 			const std::map<int, Transform> & guessPoses,
 			bool lookInDatabase,
+			cv::Mat & covariance,
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
@@ -220,6 +237,7 @@ private:
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
 	float _loopRatio;
+	float _maxLoopClosureDistance;
 	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
@@ -245,22 +263,28 @@ private:
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
+	bool _proximityOdomGuess;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
-	float _optimizationMaxLinearError;
+	float _optimizationMaxError;
 	bool _startNewMapOnLoopClosure;
+	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters
 	bool _goalsSavedInUserData;
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
 	bool _savedLocalizationIgnored;
+	bool _loopCovLimited;
+	bool _loopGPS;
+	int _maxOdomCacheSize;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
 	double _lastProcessTime;
 	bool _someNodesHaveBeenTransferred;
 	float _distanceTravelled;
+	bool _optimizeFromGraphEndChanged;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
@@ -286,6 +310,11 @@ private:
 	Transform _mapCorrectionBackup; // used in localization mode when odom is lost
 	Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
 	int _lastLocalizationNodeId; // for localization mode
+	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
+	bool _currentSessionHasGPS;
+	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
+	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
+	std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link
 
 	// Planning stuff
 	int _pathStatus;

corelib/include/rtabmap/core/SensorData.h

@@ -33,11 +33,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/core/GeodeticCoords.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <rtabmap/core/LaserScan.h>
 #include <rtabmap/core/IMU.h>
+#include <rtabmap/core/GPS.h>
+#include <rtabmap/core/EnvSensor.h>
+#include <rtabmap/core/Landmark.h>
+#include <rtabmap/core/GlobalDescriptor.h>
 
 namespace rtabmap
 {
@@ -161,9 +164,23 @@ public:
 	const cv::Mat & imageRaw() const {return _imageRaw;}
 	const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
 	const LaserScan & laserScanRaw() const {return _laserScanRaw;}
-	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
-	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
-	void setLaserScanRaw(const LaserScan & laserScanRaw) {_laserScanRaw =laserScanRaw;}
+
+	/**
+	 * Set image data. Detect automatically if raw or compressed.
+	 * A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * @param clearPreviousData, clear previous raw and compressed images before setting the new ones.
+	 */
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const std::vector<CameraModel> & models, bool clearPreviousData = true);
+	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & stereoCameraModel, bool clearPreviousData = true);
+
+	/**
+	 * Set laser scan data. Detect automatically if raw or compressed.
+	 * A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * @param clearPreviousData, clear previous raw and compressed scans before setting the new one.
+	 */
+	void setLaserScan(const LaserScan & laserScan, bool clearPreviousData = true);
+
 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
 	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
@@ -172,6 +189,11 @@ public:
 	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
 	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
 
+	RTABMAP_DEPRECATED(void setImageRaw(const cv::Mat & image), "Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.");
+	RTABMAP_DEPRECATED(void setDepthOrRightRaw(const cv::Mat & image), "Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.");
+	RTABMAP_DEPRECATED(void setLaserScanRaw(const LaserScan & scan), "Use setLaserScan() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.");
+	RTABMAP_DEPRECATED(void setUserDataRaw(const cv::Mat & data), "Use setUserData() or removeRawData() instead.");
+
 	void uncompressData();
 	void uncompressData(
 			cv::Mat * imageRaw,
@@ -193,15 +215,15 @@ public:
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
 	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
 
-	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
 	/**
 	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
 	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
 	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
 	 * (to have multiple rows instead of multiple columns) in order to be detected as
 	 * not compressed.
+	 * @param clearPreviousData, clear previous raw and compressed user data before setting the new one.
 	 */
-	void setUserData(const cv::Mat & userData);
+	void setUserData(const cv::Mat & userData, bool clearPreviousData = true);
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}
 	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
 
@@ -228,6 +250,11 @@ public:
 	const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
+	void addGlobalDescriptor(const GlobalDescriptor & descriptor) {_globalDescriptors.push_back(descriptor);}
+	void setGlobalDescriptors(const std::vector<GlobalDescriptor> & descriptors) {_globalDescriptors = descriptors;}
+	void clearGlobalDescriptors() {_globalDescriptors.clear();}
+	const std::vector<GlobalDescriptor> & globalDescriptors() const {return _globalDescriptors;}
+
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}
 
@@ -235,20 +262,30 @@ public:
 	const Transform & globalPose() const {return globalPose_;}
 	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
 
-	void setGPS(const GPS & gps)
-	{
-		gps_ = gps;
-	}
+	void setGPS(const GPS & gps) {gps_ = gps;}
 	const GPS & gps() const {return gps_;}
 
-	void setIMU(const IMU & imu)
-	{
-		imu_ = imu;
-	}
+	void setIMU(const IMU & imu) {imu_ = imu; }
 	const IMU & imu() const {return imu_;}
 
+	void setEnvSensors(const EnvSensors & sensors) {_envSensors = sensors;}
+	void addEnvSensor(const EnvSensor & sensor) {_envSensors.insert(std::make_pair(sensor.type(), sensor));}
+	const EnvSensors & envSensors() const {return _envSensors;}
+
+	void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
+	const Landmarks & landmarks() const {return _landmarks;}
+
 	long getMemoryUsed() const; // Return memory usage in Bytes
-	void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
+	/**
+	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
+	 * Raw data are kept is set.
+	 */
+	void clearCompressedData(bool images = true, bool scan = true, bool userData = true);
+	/**
+	 * Clear raw rgb/depth (left/right) images, raw laser scan and raw user data.
+	 * Compressed data are kept is set.
+	 */
+	void clearRawData(bool images = true, bool scan = true, bool userData = true);
 
 	bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
 
@@ -281,11 +318,20 @@ private:
 	float _cellSize;
 	cv::Point3f _viewPoint;
 
+	// environmental sensors
+	EnvSensors _envSensors;
+
+	// landmarks
+	Landmarks _landmarks;
+
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	std::vector<cv::Point3f> _keypoints3D;
 	cv::Mat _descriptors;
 
+	// global descriptors
+	std::vector<GlobalDescriptor> _globalDescriptors;
+
 	Transform groundTruth_;
 
 	Transform globalPose_;

corelib/include/rtabmap/core/Signature.h

@@ -82,18 +82,22 @@ public:
 	void addLinks(const std::map<int, Link> & links);
 	void addLink(const Link & link);
 
-	bool hasLink(int idTo) const;
+	bool hasLink(int idTo, Link::Type type = Link::kUndef) const;
 
 	void changeLinkIds(int idFrom, int idTo);
 
-	void removeLinks();
+	void removeLinks(bool keepSelfReferringLinks = false);
 	void removeLink(int idTo);
 	void removeVirtualLinks();
 
+	void addLandmark(const Link & landmark) {_landmarks.insert(std::make_pair(landmark.to(), landmark));}
+	const std::map<int, Link> & getLandmarks() const {return _landmarks;}
+	void removeLandmarks() {_landmarks.clear();}
+
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}
 
-	const std::map<int, Link> & getLinks() const {return _links;}
+	const std::multimap<int, Link> & getLinks() const {return _links;}
 	bool isSaved() const {return _saved;}
 	bool isModified() const {return _modified || _linksModified;}
 	bool isLinksModified() const {return _linksModified;}
@@ -140,7 +144,8 @@ private:
 	int _id;
 	int _mapId;
 	double _stamp;
-	std::map<int, Link> _links; // id, transform
+	std::multimap<int, Link> _links; // id, transform
+	std::map<int, Link> _landmarks;
 	int _weight;
 	std::string _label;
 	bool _saved; // If it's saved to bd