GUI: added word wrap to transform label in loop closure viewer. Memory: creating a RegistraitonVis for stereo from motion if memory's registration pipeline is not vis only.

Great! thx!

.appveyor.yml

@@ -33,36 +33,38 @@ 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
   # 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 PCL:"
+  - 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 PCL:"
+  - 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 PCL:"
+  - 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 PCL:"
+  - ECHO "Installed Eigen:"
   - ps: "ls \"C:/Program Files/Eigen\""
   # PCL
-  - ps: wget 'https://dl.dropboxusercontent.com/s/r9tvi9md54zlul2/PCL-1_8_1-July2018-msvc140.exe?dl=0' -outfile PCL_1.8.1.exe
+  - 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\""
@@ -85,12 +87,40 @@ install:
   - 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\cmake" -DZLIB_ROOT="C:\Program Files\zlib" -DBUILD_AS_BUNDLE=ON ..
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -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
@@ -101,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

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 6)
+SET(RTABMAP_MINOR_VERSION 19)
+SET(RTABMAP_PATCH_VERSION 3)
 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)
 
@@ -144,6 +162,7 @@ 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_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
@@ -169,7 +188,9 @@ 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_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(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
@@ -179,10 +200,18 @@ 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)
@@ -205,6 +234,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)
@@ -316,7 +346,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)
@@ -349,21 +380,7 @@ 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}")
@@ -455,8 +472,8 @@ 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)
 
@@ -467,6 +484,16 @@ IF(WITH_MSCKF_VIO)
     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_ORB_SLAM2 AND NOT G2O_FOUND)
     FIND_PACKAGE(ORB_SLAM2 QUIET)
@@ -495,7 +522,17 @@ IF(loam_velodyne_FOUND)
         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 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)
+ELSEIF(G2O_FOUND OR 
+       GTSAM_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)
@@ -547,7 +584,17 @@ SET(CONF_DEPENDENCIES
 )
 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()
+
 IF(NOT G2O_FOUND)
    SET(G2O "//")
 ELSE()
@@ -666,11 +713,19 @@ IF(NOT msckf_vio_FOUND)
 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(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
@@ -680,6 +735,9 @@ 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 NOT (OpenCV_VERSION_MAJOR LESS 3)))
    SET(OPENCV3 "//")
@@ -768,7 +826,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")
@@ -866,7 +926,11 @@ IF(APPLE OR WIN32)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
 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}")
+IF(PCL_COMPILE_OPTIONS)
+MESSAGE(STATUS "  PCL_COMPILE_OPTIONS = ${PCL_COMPILE_OPTIONS}")
+ENDIF(PCL_COMPILE_OPTIONS)
 
 MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
@@ -878,9 +942,9 @@ IF(OpenCV_FOUND)
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION_MAJOR} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION_MAJOR} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
     ENDIF()
   ENDIF()
 ENDIF(OpenCV_FOUND)
@@ -891,6 +955,18 @@ 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(WITH_MADGWICK)
+MESSAGE(STATUS "  With Madgwick             = YES (License: GPL)")
+ELSE()
+MESSAGE(STATUS "  With Madgwick             = NO (WITH_MADGWICK=OFF)")
+ENDIF()
+
 IF(Freenect_FOUND)
 MESSAGE(STATUS "  With Freenect             = YES (License: Apache v2 and/or GPLv2)")
 ELSEIF(NOT WITH_FREENECT)
@@ -1094,6 +1170,14 @@ 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)

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>

Version.h.in

@@ -65,7 +65,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @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
+@MADGWICK@#define RTABMAP_MADGWICK
+
 
 #endif /* VERSION_H_ */
 

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- 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" />
@@ -13,6 +13,7 @@
     <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" />
 

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-2018<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
 <b>http://introlab.github.io/rtabmap</b><br><br>
 

app/android/jni/CameraTango.cpp

@@ -438,6 +438,7 @@ void CameraTango::close()
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
 	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
 	originOffset_ = Transform();
 	originUpdate_ = false;
 }
@@ -545,6 +546,11 @@ void CameraTango::setGPS(const GPS & gps)
 	lastKnownGPS_ = gps;
 }
 
+void CameraTango::addEnvSensor(int type, float value)
+{
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
@@ -885,7 +891,6 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			{
 				//UTimer t;
 				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
-				data.setDepthOrRightRaw(depth);
 				//LOGD("Bilateral filtering, time=%fs", t.ticks());
 			}
 
@@ -907,6 +912,12 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			{
 				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
 			}
+
+			if(lastEnvSensors_.size())
+			{
+				data.setEnvSensors(lastEnvSensors_);
+				lastEnvSensors_.clear();
+			}
 		}
 		else
 		{
@@ -950,7 +961,15 @@ void CameraTango::mainLoop()
 				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;

app/android/jni/CameraTango.h

@@ -92,6 +92,7 @@ public:
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
+	void addEnvSensor(int type, float value);
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
@@ -130,6 +131,7 @@ private:
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
 	GPS lastKnownGPS_;
+	EnvSensors lastEnvSensors_;
 	Transform originOffset_;
 	bool originUpdate_;
 };

app/android/jni/RTABMapApp.cpp

@@ -95,28 +95,33 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("1")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("3")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), std::string("0.05")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMarkerLength(), std::string("0.0")));
 
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemUseOdomGravity(), "true"));
 	if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end())
 	{
 		if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("2") == 0) // GTSAM
 		{
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerGravitySigma(), "0.2"));
 		}
 		else if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("1") == 0) // g2o
 		{
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.0"));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerGravitySigma(), "0"));
 		}
 		else // TORO
 		{
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"100":"0"));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerGravitySigma(), "0"));
 		}
 	}
 
@@ -126,7 +131,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxRotation(), std::string("0.17"))); // 10 degrees
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.5")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.49")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
 
 	parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures()));
@@ -1138,10 +1143,15 @@ int RTABMapApp::Render()
 				int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
 				int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
 				int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
+				int landmark = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
 				if(!paused_ && loopClosure>0)
 				{
 					main_scene_.setBackgroundColor(0, 0.5f, 0); // green
 				}
+				else if(!paused_ && landmark!=0)
+				{
+					main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+				}
 				else if(!paused_ && rejected>0)
 				{
 					main_scene_.setBackgroundColor(0, 0.2f, 0); // dark green
@@ -1334,11 +1344,11 @@ int RTABMapApp::Render()
 						int smallMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
 						int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
 						int rehearsalMerged = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f);
-						if(!localizationMode_ && stats.getSignatures().size() &&
+						if(!localizationMode_ && stats.getLastSignatureData().id() > 0 &&
 							smallMovement == 0 && rehearsalMerged == 0 && fastMovement == 0)
 						{
-							int id = stats.getSignatures().rbegin()->first;
-							const rtabmap::Signature & s = stats.getSignatures().rbegin()->second;
+							int id = stats.getLastSignatureData().id();
+							const rtabmap::Signature & s = stats.getLastSignatureData();
 
 							if(!trajectoryMode_ &&
 							   !s.sensorData().imageRaw().empty() &&
@@ -1352,10 +1362,15 @@ int RTABMapApp::Render()
 
 						int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
 						int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
+						int landmark = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
 						if(!paused_ && loopClosure>0)
 						{
 							main_scene_.setBackgroundColor(0, 0.5f, 0); // green
 						}
+						else if(!paused_ && landmark!=0)
+						{
+							main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
+						}
 						else if(!paused_ && rejected>0)
 						{
 							main_scene_.setBackgroundColor(0, 0.2f, 0); // dark green
@@ -1379,14 +1394,14 @@ int RTABMapApp::Render()
 				LOGW("Looking fo data to load (%d) %fs", bufferedSensorData.size(), time.ticks());
 #endif
 
-				std::map<int, rtabmap::Transform> poses = rtabmapEvents.back()->getStats().poses();
+				std::map<int, rtabmap::Transform> posesWithMarkers = rtabmapEvents.back()->getStats().poses();
 				if(!rtabmapEvents.back()->getStats().mapCorrection().isNull())
 				{
 					mapToOdom_ = rtabmapEvents.back()->getStats().mapCorrection();
 				}
 
 				// Transform pose in OpenGL world
-				for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				for(std::map<int, rtabmap::Transform>::iterator iter=posesWithMarkers.begin(); iter!=posesWithMarkers.end(); ++iter)
 				{
 					if(!graphOptimization_)
 					{
@@ -1402,6 +1417,7 @@ int RTABMapApp::Render()
 					}
 				}
 
+				std::map<int, rtabmap::Transform> poses(posesWithMarkers.lower_bound(0), posesWithMarkers.end());
 				const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back()->getStats().constraints();
 				if(poses.size())
 				{
@@ -1533,7 +1549,7 @@ int RTABMapApp::Render()
 					}
 				}
 
-				if(poses.size())
+				if(!poses.empty())
 				{
 					//update cloud visibility
 					boost::mutex::scoped_lock  lock(meshesMutex_);
@@ -1551,6 +1567,33 @@ int RTABMapApp::Render()
 						}
 					}
 				}
+
+				// Update markers
+				std::set<int> addedMarkers = main_scene_.getAddedMarkers();
+				for(std::set<int>::const_iterator iter=addedMarkers.begin();
+					iter!=addedMarkers.end();
+					++iter)
+				{
+					if(posesWithMarkers.find(*iter) == posesWithMarkers.end())
+					{
+						main_scene_.removeMarker(*iter);
+					}
+				}
+				for(std::map<int, rtabmap::Transform>::const_iterator iter=posesWithMarkers.begin();
+					iter!=posesWithMarkers.end() && iter->first<0;
+					++iter)
+				{
+					int id = iter->first;
+					if(main_scene_.hasMarker(id))
+					{
+						//just update pose
+						main_scene_.setMarkerPose(id, iter->second);
+					}
+					else
+					{
+						main_scene_.addMarker(id, iter->second);
+					}
+				}
 			}
 			else
 			{
@@ -2064,6 +2107,14 @@ void RTABMapApp::setGPS(const rtabmap::GPS & gps)
 	}
 }
 
+void RTABMapApp::addEnvSensor(int type, float value)
+{
+	if(camera_)
+	{
+		camera_->addEnvSensor(type, value);
+	}
+}
+
 void RTABMapApp::resetMapping()
 {
 	LOGW("Reset!");
@@ -3086,7 +3137,7 @@ int RTABMapApp::postProcessing(int approach)
 			{
 				progressionStatus_.reset(6);
 			}
-			returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, approach==-1?&progressionStatus_:0);
+			returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, true, true, approach==-1?&progressionStatus_:0);
 			if(approach == -1 && progressionStatus_.isCanceled())
 			{
 				postProcessing_ = false;
@@ -3295,7 +3346,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
 		{
 			const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getRtabmapEvent()->getStats();
 			loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
-			featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
+			featuresExtracted = stats.getLastSignatureData().getWords().size();
 
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryWorking_memory_size(), uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryShort_time_memory_size(), uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f)));
@@ -3312,6 +3363,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryFast_movement(), uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f)));
+			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopLandmark_detected(), uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f)));
 		}
 		// else use last data
 
@@ -3330,6 +3382,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
 		float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
 		float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
 		int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
+		int landmarkDetected = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
 		rtabmap::Transform currentPose = main_scene_.GetCameraPose();
 		float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f;
 		if(!currentPose.isNull())
@@ -3349,7 +3402,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFFFFIFFFFFF)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFFFFIIFFFFFF)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -3373,6 +3426,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
 								optimizationMaxErrorRatio,
 								distanceTravelled,
 								fastMovement,
+								landmarkDetected,
 								x,
 								y,
 								z,

app/android/jni/RTABMapApp.h

@@ -148,6 +148,7 @@ 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);

app/android/jni/jni_interface.cpp

@@ -357,6 +357,15 @@ Java_com_introlab_rtabmap_RTABMapLib_setGPS(
 			bearing));
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_addEnvSensor(
+		JNIEnv*, jobject,
+		int type,
+		float value)
+{
+	return app.addEnvSensor(type, value);
+}
+
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
 		JNIEnv*, jobject)

app/android/jni/scene.cpp

@@ -102,7 +102,7 @@ Scene::Scene() :
 {
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
-	      tango_gl::GestureCamera::kFirstPerson);
+	      tango_gl::GestureCamera::kThirdPersonFollow);
 }
 
 Scene::~Scene() {
@@ -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);
@@ -551,6 +556,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();
 }
 
@@ -648,6 +659,53 @@ void Scene::setTraceVisible(bool 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,

app/android/jni/scene.h

@@ -102,6 +102,12 @@ class Scene {
   void setGridVisible(bool visible);
   void setTraceVisible(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,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -167,6 +173,8 @@ class Scene {
   bool gridVisible_;
   bool traceVisible_;
 
+  std::map<int, tango_gl::Axis*> markers_;
+
   TangoSupportRotation color_camera_to_display_rotation_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;

app/android/res/layout/activity_settings.xml

@@ -191,6 +191,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 +223,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/values/strings.xml

@@ -36,6 +36,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 -->
@@ -91,7 +92,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 +103,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>
     
@@ -344,12 +351,18 @@
     <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>
 
@@ -591,6 +604,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/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -1,11 +1,8 @@
 package com.introlab.rtabmap;
 
-import java.io.BufferedInputStream;
-import java.io.BufferedOutputStream;
 import java.io.File;
 import java.io.FileInputStream;
 import java.io.FileOutputStream;
-import java.io.FilenameFilter;
 import java.io.IOException;
 import java.io.InputStream;
 import java.io.OutputStream;
@@ -16,16 +13,13 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Date;
 import java.util.HashMap;
-import java.util.List;
-import java.util.zip.ZipEntry;
-import java.util.zip.ZipOutputStream;
+import java.util.Timer;
+import java.util.TimerTask;
 
-import android.app.ActionBar;
 import android.app.Activity;
 import android.app.ActivityManager;
 import android.app.ActivityManager.MemoryInfo;
 import android.app.AlertDialog;
-import android.app.Dialog;
 import android.app.Notification;
 import android.app.NotificationManager;
 import android.app.PendingIntent;
@@ -41,46 +35,30 @@ import android.content.pm.ApplicationInfo;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
-import android.content.res.Configuration;
-import android.database.Cursor;
-import android.database.sqlite.SQLiteDatabase;
-import android.graphics.Bitmap;
-import android.graphics.Canvas;
-import android.graphics.Color;
-import android.graphics.Paint;
-import android.graphics.Rect;
-import android.graphics.Typeface;
 import android.hardware.Camera;
 import android.hardware.Sensor;
 import android.hardware.SensorEvent;
 import android.hardware.SensorEventListener;
 import android.hardware.SensorManager;
+import android.graphics.Matrix;
 import android.graphics.Point;
 import android.hardware.display.DisplayManager;
 import android.location.Location;
 import android.location.LocationListener;
 import android.location.LocationManager;
-import android.net.ConnectivityManager;
-import android.net.NetworkInfo;
 import android.net.Uri;
+import android.net.wifi.WifiInfo;
+import android.net.wifi.WifiManager;
 import android.opengl.GLSurfaceView;
-import android.os.AsyncTask;
 import android.os.Bundle;
 import android.os.Environment;
 import android.os.Handler;
-import android.os.Debug;
 import android.os.IBinder;
 import android.os.Message;
-import android.preference.ListPreference;
 import android.preference.PreferenceManager;
-import android.text.Editable;
 import android.text.Html;
 import android.text.InputType;
-import android.text.SpannableString;
-import android.text.TextPaint;
-import android.text.method.HideReturnsTransformationMethod;
 import android.text.method.LinkMovementMethod;
-import android.text.util.Linkify;
 import android.util.Log;
 import android.util.TypedValue;
 import android.view.ContextMenu;
@@ -91,30 +69,23 @@ import android.view.MenuItem;
 import android.view.MenuItem.OnMenuItemClickListener;
 import android.view.MenuInflater;
 import android.view.MotionEvent;
-import android.view.Surface;
 import android.view.View;
 import android.view.ContextMenu.ContextMenuInfo;
 import android.view.View.OnClickListener;
 import android.view.View.OnCreateContextMenuListener;
 import android.view.View.OnTouchListener;
-import android.view.Window;
 import android.view.WindowManager;
 import android.view.inputmethod.EditorInfo;
-import android.webkit.WebView;
-import android.webkit.WebViewClient;
 import android.widget.AdapterView;
-import android.widget.AdapterView.OnItemLongClickListener;
 import android.widget.AdapterView.OnItemSelectedListener;
 import android.widget.ArrayAdapter;
 import android.widget.Button;
 import android.widget.EditText;
-import android.widget.LinearLayout;
 import android.widget.ListView;
 import android.widget.NumberPicker;
 import android.widget.RelativeLayout;
 import android.widget.SeekBar;
 import android.widget.SeekBar.OnSeekBarChangeListener;
-import android.widget.Spinner;
 import android.widget.TextView;
 import android.widget.Toast;
 import android.widget.ToggleButton;
@@ -155,6 +126,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	private boolean mHudVisible = true;
 	private int mSavedRenderingType = 0;
 	private boolean mMenuOpened = false;
+	private long mSavedStamp = 0;
 
 	// UI states
 	private static enum State {
@@ -224,12 +196,34 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	private String mMinInliers;
 	private String mMaxOptimizationError;
 	private boolean mGPSSaved = false;
+	private boolean mEnvSensorsSaved = false;
 	
 	private LocationManager mLocationManager;
 	private LocationListener mLocationListener;
 	private Location mLastKnownLocation;
 	private SensorManager mSensorManager;
+	private WifiManager mWifiManager;
+	private Timer mEnvSensorsTimer = new Timer();
+	Sensor mAccelerometer;
+	Sensor mMagnetometer;
+	Sensor mAmbientTemperature;
+	Sensor mAmbientLight;
+	Sensor mAmbientAirPressure;
+	Sensor mAmbientRelativeHumidity;
 	private float mCompassDeg = 0.0f;
+	private float[] mLastEnvSensors = new float[5];
+	private boolean[] mLastEnvSensorsSet = new boolean[5];
+	
+	private float[] mLastAccelerometer = new float[3];
+    private float[] mLastMagnetometer = new float[3];
+    private boolean mLastAccelerometerSet = false;
+    private boolean mLastMagnetometerSet = false;
+
+    private Matrix mDeviceToCamera = new Matrix();
+    private Matrix mRMat = new Matrix();
+    private Matrix mNewR = new Matrix();
+    private float[] mR = new float[9];
+    private float[] mOrientation = new float[3];
 
 	private int mTotalLoopClosures = 0;
 	private boolean mMapIsEmpty = false;
@@ -239,8 +233,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	
 	private AlertDialog mMemoryWarningDialog = null;
 	
-	private final int STATUS_TEXTS_SIZE = 19;
-	private final int STATUS_TEXTS_POSE_INDEX = 5;
+	private final int STATUS_TEXTS_SIZE = 20;
+	private final int STATUS_TEXTS_POSE_INDEX = 6;
 	private String[] mStatusTexts = new String[STATUS_TEXTS_SIZE];
 	
 	GestureDetector mGesDetect = null;
@@ -534,14 +528,69 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		};
 
 		mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
+		mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+	    mMagnetometer = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
+	    mAmbientTemperature = mSensorManager.getDefaultSensor(Sensor.TYPE_AMBIENT_TEMPERATURE);
+	    mAmbientLight = mSensorManager.getDefaultSensor(Sensor.TYPE_LIGHT);
+	    mAmbientAirPressure = mSensorManager.getDefaultSensor(Sensor.TYPE_PRESSURE);
+	    mAmbientRelativeHumidity = mSensorManager.getDefaultSensor(Sensor.TYPE_RELATIVE_HUMIDITY);
+	    float [] values = {1,0,0,0,0,1,0,-1,0};
+	    mDeviceToCamera.setValues(values);
+	    mWifiManager = (WifiManager) getSystemService(Context.WIFI_SERVICE);
+		
+		setCamera(1);
 
 		DISABLE_LOG =  !( 0 != ( getApplicationInfo().flags & ApplicationInfo.FLAG_DEBUGGABLE ) );
 	}
 
 	@Override
 	public void onSensorChanged(SensorEvent event) {
-		// get the angle around the z-axis rotated
-		mCompassDeg = event.values[0];
+		if(event.sensor == mAccelerometer || event.sensor == mMagnetometer)
+		{
+			if (event.sensor == mAccelerometer) {
+	            System.arraycopy(event.values, 0, mLastAccelerometer, 0, event.values.length);
+	            mLastAccelerometerSet = true;
+	        } else if (event.sensor == mMagnetometer) {
+	            System.arraycopy(event.values, 0, mLastMagnetometer, 0, event.values.length);
+	            mLastMagnetometerSet = true;
+	        }
+	        if (mLastAccelerometerSet && mLastMagnetometerSet) {
+	            SensorManager.getRotationMatrix(mR, null, mLastAccelerometer, mLastMagnetometer);
+	            mRMat.setValues(mR);
+	            mNewR.setConcat(mRMat, mDeviceToCamera) ;
+	            mNewR.getValues(mR);
+	            SensorManager.getOrientation(mR, mOrientation);                 
+	            mCompassDeg = mOrientation[0] * 180.0f/(float)Math.PI;
+	            if(mCompassDeg<0.0f)
+	            {
+	            	mCompassDeg += 360.0f;
+	            }
+	        }
+		}
+		else if(event.sensor == mAmbientTemperature)
+		{
+			mLastEnvSensors[1] = event.values[0];
+			mLastEnvSensorsSet[1] = true;
+			RTABMapLib.addEnvSensor(2, event.values[0]);
+		}
+		else if(event.sensor == mAmbientAirPressure)
+		{
+			mLastEnvSensors[2] = event.values[0];
+			mLastEnvSensorsSet[2] = true;
+			RTABMapLib.addEnvSensor(3, event.values[0]);
+		}
+		else if(event.sensor == mAmbientLight)
+		{
+			mLastEnvSensors[3] = event.values[0];
+			mLastEnvSensorsSet[3] = true;
+			RTABMapLib.addEnvSensor(4, event.values[0]);
+		}
+		else if(event.sensor == mAmbientRelativeHumidity)
+		{
+			mLastEnvSensors[4] = event.values[0];
+			mLastEnvSensorsSet[4] = true;
+			RTABMapLib.addEnvSensor(5, event.values[0]);
+		}
 	}
 
 	@Override
@@ -660,6 +709,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		
 		mLocationManager.removeUpdates(mLocationListener);
 		mSensorManager.unregisterListener(this);
+		mLastAccelerometerSet = false;
+		mLastMagnetometerSet= false;
+		mLastEnvSensorsSet[0] = mLastEnvSensorsSet[1]= mLastEnvSensorsSet[2]= mLastEnvSensorsSet[3]= mLastEnvSensorsSet[4]=false;
 
 		RTABMapLib.onPause();
 		
@@ -724,11 +776,37 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			boolean keepAllDb = sharedPref.getBoolean(getString(R.string.pref_key_keep_all_db), Boolean.parseBoolean(getString(R.string.pref_default_keep_all_db)));
 			boolean optimizeFromGraphEnd = sharedPref.getBoolean(getString(R.string.pref_key_optimize_end), Boolean.parseBoolean(getString(R.string.pref_default_optimize_end)));
 			String optimizer = sharedPref.getString(getString(R.string.pref_key_optimizer), getString(R.string.pref_default_optimizer));
+			String markerDetection = sharedPref.getString(getString(R.string.pref_key_marker_detection), getString(R.string.pref_default_marker_detection));
+			String markerDetectionDepthError = sharedPref.getString(getString(R.string.pref_key_marker_detection_depth_error), getString(R.string.pref_default_marker_detection_depth_error));
 			mGPSSaved = sharedPref.getBoolean(getString(R.string.pref_key_gps_saved), Boolean.parseBoolean(getString(R.string.pref_default_gps_saved)));
 			if(mGPSSaved)
 			{
 				mLocationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0, 0, mLocationListener);
-				mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ORIENTATION), SensorManager.SENSOR_DELAY_GAME);
+				mSensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_UI);
+			    mSensorManager.registerListener(this, mMagnetometer, SensorManager.SENSOR_DELAY_UI);
+			}
+			mEnvSensorsSaved = sharedPref.getBoolean(getString(R.string.pref_key_env_sensors_saved), Boolean.parseBoolean(getString(R.string.pref_default_env_sensors_saved)));
+			if(mEnvSensorsSaved)
+			{
+				mSensorManager.registerListener(this, mAmbientTemperature, SensorManager.SENSOR_DELAY_NORMAL);
+				mSensorManager.registerListener(this, mAmbientAirPressure, SensorManager.SENSOR_DELAY_NORMAL);
+				mSensorManager.registerListener(this, mAmbientLight, SensorManager.SENSOR_DELAY_NORMAL);
+				mSensorManager.registerListener(this, mAmbientRelativeHumidity, SensorManager.SENSOR_DELAY_NORMAL);
+				mEnvSensorsTimer.schedule(new TimerTask() {
+
+			        @Override
+			        public void run() {
+			        	WifiInfo wifiInfo = mWifiManager.getConnectionInfo();
+			        	int dbm = 0;
+			        	if(wifiInfo != null && (dbm = wifiInfo.getRssi()) > -127)
+			        	{
+			        		mLastEnvSensors[0] = (float)dbm;
+			        		mLastEnvSensorsSet[0] = true;
+			        		RTABMapLib.addEnvSensor(1, mLastEnvSensors[0]);
+			        	}
+			        }
+
+			    },0,200);
 			}
 			
 			if(!DISABLE_LOG) Log.d(TAG, "set mapping parameters");
@@ -755,6 +833,17 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			RTABMapLib.setMappingParameter("Mem/NotLinkedNodesKept", String.valueOf(keepAllDb));
 			RTABMapLib.setMappingParameter("RGBD/OptimizeFromGraphEnd", String.valueOf(optimizeFromGraphEnd));
 			RTABMapLib.setMappingParameter("Optimizer/Strategy", optimizer);
+			if(Integer.parseInt(markerDetection) == -1)
+			{
+				RTABMapLib.setMappingParameter("RGBD/MarkerDetection", "false");
+			}
+			else
+			{
+				RTABMapLib.setMappingParameter("RGBD/MarkerDetection", "true");
+				RTABMapLib.setMappingParameter("Marker/Dictionary", markerDetection);
+                                RTABMapLib.setMappingParameter("Marker/CornerRefinementMethod", Integer.parseInt(markerDetection) > 16?"3":"0");
+			}
+			RTABMapLib.setMappingParameter("Marker/MaxDepthError", markerDetectionDepthError);
 	
 			if(!DISABLE_LOG) Log.d(TAG, "set exporting parameters...");
 			RTABMapLib.setCloudDensityLevel(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_density), getString(R.string.pref_default_density))));
@@ -1035,6 +1124,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			float optimizationMaxError,
 			float optimizationMaxErrorRatio,
 			boolean fastMovement,
+			int landmarkDetected,
 			String[] statusTexts)
 	{
 		mStatusTexts = statusTexts;
@@ -1078,6 +1168,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						}
 					})
 					.create();
+					mMemoryWarningDialog.setCanceledOnTouchOutside(false);
 					mMemoryWarningDialog.show();
 				}
 				else if(mMemoryWarningDialog == null && memoryUsed*3 > memoryFree && (mItemDataRecorderMode == null || !mItemDataRecorderMode.isChecked()))
@@ -1101,6 +1192,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						}
 					})
 					.create();
+					mMemoryWarningDialog.setCanceledOnTouchOutside(false);
 					mMemoryWarningDialog.show();
 				}
 			}
@@ -1115,6 +1207,11 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 				mToast.setText(String.format("Loop closure detected! (%d/%d inliers)", inliers, matches));
 				mToast.show();
 			}
+			else if(landmarkDetected != 0)
+			{
+				mToast.setText(String.format("Marker %d detected!", landmarkDetected));
+				mToast.show();
+			}
 			else if(rejected > 0)
 			{
 				if(inliers >= Integer.parseInt(mMinInliers))
@@ -1130,7 +1227,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 				}
 				else
 				{
-					mToast.setText(String.format("Loop closure rejected, not enough inliers (%d/%d < %s).", inliers, matches, mMinInliers));
+					mToast.setText(String.format("Loop closure rejected, not enough inliers (%d/%d < %s).", inliers, matches, mMinInliers));
 				}
 				mToast.show();
 			}
@@ -1172,6 +1269,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			final float optimizationMaxErrorRatio,
 			final float distanceTravelled,
 			final int fastMovement,
+			final int landmarkDetected,
 			final float x,
 			final float y,
 			final float z,
@@ -1225,12 +1323,42 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			}
 			else
 			{
-				statusTexts[3] = getString(R.string.gps)+"[not yet available]";
+				statusTexts[3] = getString(R.string.gps)+String.format("[not yet available, %.0fdeg]", mCompassDeg);
+			}
+		}
+		if(mEnvSensorsSaved)
+		{
+			statusTexts[4] = getString(R.string.env_sensors);
+			
+			if(mLastEnvSensorsSet[0])
+			{
+				statusTexts[4] += String.format(" %.0f dbm", mLastEnvSensors[0]);
+				mLastEnvSensorsSet[0] = false;
+			}
+			if(mLastEnvSensorsSet[1])
+			{
+				statusTexts[4] += String.format(" %.1f %cC", mLastEnvSensors[1], '\u00B0');
+				mLastEnvSensorsSet[1] = false;
+			}
+			if(mLastEnvSensorsSet[2])
+			{
+				statusTexts[4] += String.format(" %.1f hPa", mLastEnvSensors[2]);
+				mLastEnvSensorsSet[2] = false;
+			}
+			if(mLastEnvSensorsSet[3])
+			{
+				statusTexts[4] += String.format(" %.0f lx", mLastEnvSensors[3]);
+				mLastEnvSensorsSet[3] = false;
+			}
+			if(mLastEnvSensorsSet[4])
+			{
+				statusTexts[4] += String.format(" %.0f %%", mLastEnvSensors[4]);
+				mLastEnvSensorsSet[4] = false;
 			}
 		}
 		
 		String formattedDate = new SimpleDateFormat("HH:mm:ss.SSS").format(new Date());
-		statusTexts[4] = getString(R.string.time)+formattedDate;
+		statusTexts[5] = getString(R.string.time)+formattedDate;
 		
 		int index = STATUS_TEXTS_POSE_INDEX;
 		statusTexts[index++] = getString(R.string.nodes)+nodes+" (" + nodesDrawn + " shown)";
@@ -1250,7 +1378,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			
 		runOnUiThread(new Runnable() {
 				public void run() {
-					updateStatsUI(loopClosureId, inliers, matches, rejected, optimizationMaxError, optimizationMaxErrorRatio, fastMovement!=0, statusTexts);
+					updateStatsUI(loopClosureId, inliers, matches, rejected, optimizationMaxError, optimizationMaxErrorRatio, fastMovement!=0, landmarkDetected, statusTexts);
 				} 
 		});
 	}
@@ -1525,6 +1653,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 
     public void stopDisconnectTimer(){
         notouchHandler.removeCallbacks(notouchCallback);
+        Timer timer = new Timer();
+        timer.cancel();
     }
 		
 	private void updateState(State state)
@@ -1637,7 +1767,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			if(!mOnPause && !mItemLocalizationMode.isChecked() && !mItemDataRecorderMode.isChecked() && memoryFree >= 100 && mMapNodes>2)
 			{
 				// Do standard post processing?
-				new AlertDialog.Builder(getActivity())
+				AlertDialog d2 = new AlertDialog.Builder(getActivity())
+				.setCancelable(false)
 				.setTitle("Mapping Paused! Optimize Now?")
 				.setMessage("Do you want to do standard map optimization now? This can be also done later using \"Optimize\" menu.")
 				.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
@@ -1650,7 +1781,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						// do nothing...
 					}
 				})
-				.show();
+				.create();
+				d2.setCanceledOnTouchOutside(false);
+				d2.show();
 			} 
 		}
 		else
@@ -1879,6 +2012,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		else if (itemId == R.id.save)
 		{
 			AlertDialog.Builder builder = new AlertDialog.Builder(this);
+			builder.setCancelable(false);
 			builder.setTitle("RTAB-Map Database Name (*.db):");
 			final EditText input = new EditText(this);
 			input.setInputType(InputType.TYPE_CLASS_TEXT); 
@@ -1908,7 +2042,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						File newFile = new File(mWorkingDirectory + fileName + ".db");
 						if(newFile.exists())
 						{
-							new AlertDialog.Builder(getActivity())
+							AlertDialog d2 = new AlertDialog.Builder(getActivity())
+							.setCancelable(false)
 							.setTitle("File Already Exists")
 							.setMessage("Do you want to overwrite the existing file?")
 							.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
@@ -1922,7 +2057,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 									resetNoTouchTimer(true);
 								}
 							})
-							.show();
+							.create();
+							d2.setCanceledOnTouchOutside(false);
+							d2.show();
 						}
 						else
 						{
@@ -1932,6 +2069,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 				}
 			});
 			AlertDialog alertToShow = builder.create();
+			alertToShow.setCanceledOnTouchOutside(false);
 			alertToShow.getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_VISIBLE);
 			alertToShow.show();
 		}
@@ -1972,7 +2110,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		else if(itemId == R.id.data_recorder)
 		{
 			final boolean dataRecorderOldState = item.isChecked();
-			new AlertDialog.Builder(getActivity())
+			AlertDialog d2 = new AlertDialog.Builder(getActivity())
+			.setCancelable(false)
 			.setTitle("Data Recorder Mode")
 			.setMessage("Changing from/to data recorder mode will close the current session. Do you want to continue?")
 			.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
@@ -2027,7 +2166,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 					dialog.dismiss();
 				}
 			})
-			.show();
+			.create();
+			d2.setCanceledOnTouchOutside(false);
+			d2.show();
 		}
 		else if(itemId == R.id.export_point_cloud ||
 				itemId == R.id.export_point_cloud_highrez)
@@ -2082,27 +2223,26 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	        linearLayout.setLayoutParams(params);
 	        linearLayout.addView(aNumberPicker,numPicerParams);
 
-	        AlertDialog.Builder alertDialogBuilder = new AlertDialog.Builder(this);
-	        alertDialogBuilder.setTitle("Maximum polygons");
-	        alertDialogBuilder.setView(linearLayout);
-	        alertDialogBuilder
-	                .setCancelable(false)
-	                .setPositiveButton("Ok",
-	                        new DialogInterface.OnClickListener() {
-	                            public void onClick(DialogInterface dialog,
-	                                                int id) {
-	                                export(isOBJ, true, false, true, aNumberPicker.getValue()*100000);
-	                            }
-	                        })
-	                .setNegativeButton("Cancel",
-	                        new DialogInterface.OnClickListener() {
-	                            public void onClick(DialogInterface dialog,
-	                                                int id) {
-	                                dialog.cancel();
-	                            }
-	                        });
-	        AlertDialog alertDialog = alertDialogBuilder.create();
-	        alertDialog.show();
+	        AlertDialog ad = new AlertDialog.Builder(this)
+	        		.setTitle("Maximum polygons")
+	        		.setView(linearLayout)
+	        		.setCancelable(false)
+	        		.setPositiveButton("Ok",
+	        				new DialogInterface.OnClickListener() {
+	        			public void onClick(DialogInterface dialog,
+	        					int id) {
+	        				export(isOBJ, true, false, true, aNumberPicker.getValue()*100000);
+	        			}
+	        		})
+	        		.setNegativeButton("Cancel",
+	        				new DialogInterface.OnClickListener() {
+	        			public void onClick(DialogInterface dialog,
+	        					int id) {
+	        				dialog.cancel();
+	        			}
+	        		}).create();
+	        ad.setCanceledOnTouchOutside(false);
+	        ad.show();
 		}
 		else if(itemId == R.id.open)
 		{
@@ -2149,13 +2289,15 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	        DatabaseListArrayAdapter simpleAdapter = new DatabaseListArrayAdapter(this, arrayList, R.layout.database_list, from, to);//Create object and set the parameters for simpleAdapter
 
 			AlertDialog.Builder builder = new AlertDialog.Builder(this);
+			builder.setCancelable(false);
 			builder.setTitle("Choose Your File (*.db)");
 			builder.setAdapter(simpleAdapter, new DialogInterface.OnClickListener() {
 			//builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
 				public void onClick(DialogInterface dialog, final int which) {
 											
 					// Adjust color now?
-					new AlertDialog.Builder(getActivity())
+					AlertDialog d2 = new AlertDialog.Builder(getActivity())
+					.setCancelable(false)
 					.setTitle("Opening database...")
 					.setMessage("Do you want to adjust colors now?\nThis can be done later under Optimize menu.")
 					.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
@@ -2168,12 +2310,15 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 							openDatabase(files[which], false);
 						}
 					})
-					.show();
+					.create();
+					d2.setCanceledOnTouchOutside(false);
+					d2.show();
 					return;
 				}
 			});
 
 			final AlertDialog ad = builder.create(); //don't show dialog yet
+			ad.setCanceledOnTouchOutside(false);
 			ad.setOnShowListener(new OnShowListener() 
 			{   			
 				@Override
@@ -2194,6 +2339,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 									@Override
 									public boolean onMenuItemClick(MenuItem item) {
 										AlertDialog.Builder builderRename = new AlertDialog.Builder(getActivity());
+										builderRename.setCancelable(false);
 										builderRename.setTitle("RTAB-Map Database Name (*.db):");
 										final EditText input = new EditText(getActivity());
 										input.setInputType(InputType.TYPE_CLASS_TEXT); 
@@ -2213,16 +2359,32 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 													File newFile = new File(mWorkingDirectory + fileName + ".db");
 													if(newFile.exists())
 													{
-														new AlertDialog.Builder(getActivity())
+														AlertDialog d2 = new AlertDialog.Builder(getActivity())
+														.setCancelable(false)
 														.setTitle("File Already Exists")
 														.setMessage(String.format("Name %s already used, choose another name.", fileName))
-														.show();
+														.create();
+														d2.setCanceledOnTouchOutside(false);
+														d2.show();
 													}
 													else
 													{
 														File from = new File(mWorkingDirectory, files[position]);
 												        File to   = new File(mWorkingDirectory, fileName + ".db");
 												        from.renameTo(to);
+												        
+												        long stamp = System.currentTimeMillis();
+												        if(stamp-mSavedStamp < 10000)
+												        {
+												        	try {
+																Thread.sleep(10000 - (stamp-mSavedStamp));
+															}
+															catch(InterruptedException e){}
+												        }
+												        
+												        refreshSystemMediaScanDataBase(getActivity(), files[position]);
+												        refreshSystemMediaScanDataBase(getActivity(), to.getAbsolutePath());
+												        
 												        ad.dismiss();
 												        resetNoTouchTimer(true);
 													}
@@ -2230,6 +2392,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 											}
 										});
 										AlertDialog alertToShow = builderRename.create();
+										alertToShow.setCanceledOnTouchOutside(false);
 										alertToShow.getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_VISIBLE);
 										alertToShow.show();
 										return true;
@@ -2245,6 +2408,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 										        case DialogInterface.BUTTON_POSITIVE:
 										        	Log.e(TAG, String.format("Yes delete %s!", files[position]));
 										        	(new File(mWorkingDirectory+files[position])).delete();
+										        	refreshSystemMediaScanDataBase(getActivity(), mWorkingDirectory+files[position]);
 										        	ad.dismiss();
 										        	resetNoTouchTimer(true);
 										            break;
@@ -2255,11 +2419,14 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 										        }
 										    }
 										};
-										AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
-										builder.setTitle(String.format("Delete %s", files[position]))
+										AlertDialog dialog = new AlertDialog.Builder(getActivity())
+										.setCancelable(false)
+										.setTitle(String.format("Delete %s", files[position]))
 										    .setMessage("Are you sure?")
 										    .setPositiveButton("Yes", dialogClickListener)
-										    .setNegativeButton("No", dialogClickListener).show();
+										    .setNegativeButton("No", dialogClickListener).create();
+										dialog.setCanceledOnTouchOutside(false);
+										dialog.show();
 										return true;
 									}
 								});
@@ -2429,14 +2596,15 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 											public void onClick(DialogInterface dialog, int which) {
 												resetNoTouchTimer(true);
 											}
-										})
-										.create();
+										}).create();
+										d2.setCanceledOnTouchOutside(false);
 										d2.show();
 										// Make the textview clickable. Must be called after show()
 									    ((TextView)d2.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
 									}
 								})
 								.create();
+								d.setCanceledOnTouchOutside(false);
 								d.show();
 								// Make the textview clickable. Must be called after show()
 							    ((TextView)d.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
@@ -2461,6 +2629,18 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		exportThread.start();
 	}
 	
+	/** 
+	@param context : it is the reference where this method get called
+	@param docPath : absolute path of file for which broadcast will be send to refresh media database
+	@see https://stackoverflow.com/a/36051318/6163336
+	**/
+	public static void refreshSystemMediaScanDataBase(Context context, String docPath){
+	   Intent mediaScanIntent = new Intent(Intent.ACTION_MEDIA_SCANNER_SCAN_FILE);
+	   Uri contentUri = Uri.fromFile(new File(docPath));
+	   mediaScanIntent.setData(contentUri);
+	   context.sendBroadcast(mediaScanIntent);
+	}
+	
 	private void saveDatabase(String fileName)
 	{
 		final String newDatabasePath = mWorkingDirectory + fileName + ".db";
@@ -2489,6 +2669,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						}
 						else
 						{
+							refreshSystemMediaScanDataBase(getActivity(), newDatabasePath);
+							mSavedStamp = System.currentTimeMillis();
 							msg = String.format("Database saved to \"%s\".", newDatabasePathHuman);	
 						}
 						
@@ -2514,7 +2696,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						final File f = new File(newDatabasePath);
 						final int fileSizeMB = (int)f.length()/(1024 * 1024);
 						
-						new AlertDialog.Builder(getActivity())
+						AlertDialog d2 = new AlertDialog.Builder(getActivity())
+						.setCancelable(false)
 						.setTitle("Database saved!")
 						.setMessage(String.format("Database \"%s\" (%d MB) successfully saved on the SD-CARD! Share it?", newDatabasePathHuman, fileSizeMB))
 						.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
@@ -2546,7 +2729,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 								updateState(previousState);
 							}
 						})
-						.show();
+						.create();
+						d2.setCanceledOnTouchOutside(false);
+						d2.show();
 					}
 				});
 			} 
@@ -2595,7 +2780,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 					File newFile = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + fileName + ".zip");
 					if(newFile.exists())
 					{
-						new AlertDialog.Builder(getActivity())
+						AlertDialog ad = new AlertDialog.Builder(getActivity())
+						.setCancelable(false)
 						.setTitle("File Already Exists")
 						.setMessage("Do you want to overwrite the existing file?")
 						.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
@@ -2607,8 +2793,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 							public void onClick(DialogInterface dialog, int which) {
 								saveOnDevice();
 							}
-						})
-						.show();
+						}).create();
+						ad.setCanceledOnTouchOutside(false);
+						ad.show();
 					}
 					else
 					{
@@ -2618,6 +2805,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			}
 		});
 		AlertDialog alertToShow = builder.create();
+		alertToShow.setCanceledOnTouchOutside(false);
 		alertToShow.getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_VISIBLE);
 		alertToShow.show();
 	}
@@ -2695,7 +2883,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 							final File f = new File(zipOutput);
 							final int fileSizeMB = (int)f.length()/(1024 * 1024);
 							
-							new AlertDialog.Builder(getActivity())
+							AlertDialog d = new AlertDialog.Builder(getActivity())
+							.setCancelable(false)
 							.setTitle("Database saved!")
 							.setMessage(String.format("Mesh \"%s\" (%d MB) successfully exported on the SD-CARD! Share it?", pathHuman, fileSizeMB))
 							.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
@@ -2714,8 +2903,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 								public void onClick(DialogInterface dialog, int which) {
 									resetNoTouchTimer(true);
 								}
-							})
-							.show();
+							}).create();
+							d.setCanceledOnTouchOutside(false);
+							d.show();
 						}
 					});
 				}
@@ -2761,7 +2951,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						{
 							updateState(State.STATE_IDLE);
 							mProgressDialog.dismiss();
-							new AlertDialog.Builder(getActivity())
+							AlertDialog d = new AlertDialog.Builder(getActivity())
 							.setCancelable(false)
 							.setTitle("Error")
 							.setMessage("The map is loaded but optimization of the map's graph has "
@@ -2778,14 +2968,15 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 							.setNegativeButton("Close", new DialogInterface.OnClickListener() {
 								public void onClick(DialogInterface dialog, int which) {
 								}
-							})
-							.show();
+							}).create();
+							d.setCanceledOnTouchOutside(false);
+							d.show();
 						}
 						else if(status == -2)
 						{
 							updateState(State.STATE_IDLE);
 							mProgressDialog.dismiss();
-							new AlertDialog.Builder(getActivity())
+							AlertDialog d = new AlertDialog.Builder(getActivity())
 							.setCancelable(false)
 							.setTitle("Error")
 							.setMessage("Failed to open database: Out of memory! Try "
@@ -2800,8 +2991,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 							.setNegativeButton("Close", new DialogInterface.OnClickListener() {
 								public void onClick(DialogInterface dialog, int which) {
 								}
-							})
-							.show();
+							}).create();
+							d.setCanceledOnTouchOutside(false);
+							d.show();
 						}
 						else
 						{

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

@@ -99,6 +99,7 @@ public class RTABMapLib
 			double altitude,  
 			double accuracy,
 			double bearing);
+    public static native void addEnvSensor(int type, float value);
 
     public static native void resetMapping();
     public static native void save(String outputDatabasePath);

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

@@ -206,6 +206,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));
@@ -265,6 +267,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));

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(false);
+			mAuthDialog.setCanceledOnTouchOutside(false);
 			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() {
@@ -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/TextManager.java

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

app/src/CMakeLists.txt

@@ -154,11 +154,28 @@ IF(BUILD_AS_BUNDLE AND (APPLE 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

app/src/main.cpp

@@ -43,7 +43,7 @@ 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);
@@ -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);
 
@@ -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/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

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

@@ -115,6 +115,9 @@ public:
 
 	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,483 +27,13 @@ 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/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;
-	}
-}
-
-namespace rs2
-{
-	class context;
-	class device;
-	class syncer;
-}
-struct rs2_intrinsics;
-struct rs2_extrinsics;
-
-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();
-	enum RGBSource {kColor, kInfrared, kFishEye};
-
-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();
-
-	void setDepthScaledToRGBSize(bool enabled);
-	void setRGBSource(RGBSource source);
-	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_;
-	bool depthScaledToRGBSize_;
-	RGBSource rgbSource_;
-	CameraModel cameraModel_;
-	std::vector<int> rsRectificationTable_;
-
-	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
-};
-/////////////////////////
-// CameraRealSense2
-/////////////////////////
-class slam_event_handler;
-class RTABMAP_EXP CameraRealSense2 :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraRealSense2(
-		const std::string & deviceId = "",
-		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRealSense2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-	// parameters are set during initialization
-	void setEmitterEnabled(bool enabled);
-	void setIRDepthFormat(bool enabled);
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_REALSENSE2
-	rs2::context * ctx_;
-	rs2::device * dev_;
-	std::string deviceId_;
-	rs2::syncer * syncer_;
-	float depth_scale_meters_;
-	rs2_intrinsics * depthIntrinsics_;
-	rs2_intrinsics * rgbIntrinsics_;
-	rs2_extrinsics * depthToRGBExtrinsics_;
-	cv::Mat depthBuffer_;
-	cv::Mat rgbBuffer_;
-	CameraModel model_;
-
-	bool emitterEnabled_;
-	bool irDepth_;
-#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,231 +27,9 @@ 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 = true);
-	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 = true);
-	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());
-	CameraStereoVideo(
-			int deviceLeft,
-			int deviceRight,
-			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_;
-	int usbDevice2_;
-};
-
-} // 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>

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

@@ -103,7 +103,7 @@ public:
 	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(
@@ -165,14 +165,16 @@ public:
 	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 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;
 
@@ -232,7 +234,7 @@ protected:
 	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(
@@ -258,16 +260,18 @@ protected:
 	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 getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & 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

@@ -104,7 +104,7 @@ protected:
 	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(
@@ -130,16 +130,18 @@ protected:
 	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 getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & 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;
 
@@ -155,10 +157,7 @@ private:
 	std::string queryStepKeypoint() 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 stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;

corelib/include/rtabmap/core/DBReader.h

@@ -51,14 +51,16 @@ public:
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
 			 int startIndex = 0,
-			 int cameraIndex = -1);
+			 int cameraIndex = -1,
+			 int maxFrames = 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 cameraIndex = -1,
+			 int maxFrames = 0);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -81,6 +83,7 @@ private:
 	bool _ignoreGoalDelay;
 	bool _goalsIgnored;
 	int _startIndex;
+	int _maxFrames;
 	int _cameraIndex;
 
 	DBDriver * _dbDriver;
@@ -92,6 +95,7 @@ private:
 	double _previousStamp;
 	int _previousMapID;
 	bool _calibrated;
+	int _framesPublished;
 };
 
 } /* namespace rtabmap */

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

@@ -84,9 +84,10 @@ typedef cv::cuda::ORB CV_ORB_GPU;
 typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
 #endif
 
-
 namespace rtabmap {
 
+class ORBextractor;
+
 class Stereo;
 #if CV_MAJOR_VERSION < 3
 class CV_ORB;
@@ -105,7 +106,8 @@ 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
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -149,7 +151,7 @@ public:
 
 	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 +167,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 +196,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 +223,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:
@@ -244,7 +246,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,7 +277,7 @@ 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 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 {return cv::Mat();}
 
 private:
@@ -343,7 +345,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 +426,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 +448,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 +464,30 @@ 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_;
+};
+
 
 }
 

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/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,
@@ -48,6 +48,7 @@ public:
 	{
 	}
 
+	// 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
 
@@ -64,7 +65,6 @@ public:
 		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,7 +46,13 @@ 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};
 	Link();
 	Link(int from,
@@ -56,7 +62,7 @@ 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_;}

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
 {
@@ -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.
@@ -173,8 +180,11 @@ public:
 	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,
@@ -184,6 +194,7 @@ 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;
@@ -206,6 +217,7 @@ 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;}
 
 	void dumpMemoryTree(const char * fileNameTree) const;
 	virtual void dumpMemory(std::string directory) const;
@@ -222,7 +234,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);
@@ -275,6 +288,7 @@ private:
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
 	bool _saveIntermediateNodeData;
+	std::string _rgbCompressionFormat;
 	bool _incrementalMemory;
 	bool _reduceGraph;
 	int _maxStMemSize;
@@ -298,12 +312,15 @@ private:
 	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;
@@ -312,13 +329,19 @@ 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;
@@ -331,6 +354,8 @@ private:
 	RegistrationIcp * _registrationIcpMulti;
 
 	OccupancyGrid * _occupancy;
+
+	MarkerDetector * _markerDetector;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/OccupancyGrid.h

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

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_;}
 
@@ -223,7 +224,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

@@ -52,11 +52,12 @@ public:
 		kTypeORBSLAM2 = 5,
 		kTypeOkvis = 6,
 		kTypeLOAM = 7,
-		kTypeMSCKF = 8
+		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:
@@ -66,11 +67,13 @@ 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;}
@@ -87,6 +90,7 @@ private:
 	bool _force3DoF;
 	bool _holonomic;
 	bool guessFromMotion_;
+	bool guessSmoothingDelay_;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -103,7 +107,8 @@ private:
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
-	Transform previousVelocityTransform_;
+	std::list<std::pair<std::vector<float>, double> > previousVelocities_;
+	Transform velocityGuess_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 	unsigned int framesProcessed_;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -106,6 +106,7 @@ public:
 	Transform transform;
 	Transform transformFiltered;
 	Transform transformGroundTruth;
+	Transform guessVelocity;
 	float distanceTravelled;
 	int memoryUsage; //MB
 

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
 ////////////////////////////////////////////
@@ -56,13 +69,12 @@ public:
 	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 +88,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 +98,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);
 
@@ -118,20 +134,30 @@ public:
 			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(
@@ -139,7 +165,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(
@@ -148,7 +175,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:
@@ -158,6 +187,8 @@ private:
 	double epsilon_;
 	bool robust_;
 	bool priorsIgnored_;
+	bool landmarksIgnored_;
+	float gravitySigma_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -193,6 +193,7 @@ class RTABMAP_EXP Parameters
     // 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.");
@@ -203,6 +204,7 @@ 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, ReduceGraph,                 bool, false,    "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
@@ -218,10 +220,11 @@ class RTABMAP_EXP Parameters
     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, 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,       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.");
+    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)
@@ -237,12 +240,12 @@ class RTABMAP_EXP Parameters
 #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.");
+    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.");
 #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.");
 #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.");
 #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.");
@@ -338,7 +341,7 @@ 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, 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.");
@@ -353,6 +356,9 @@ class RTABMAP_EXP Parameters
     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.");
@@ -383,6 +389,8 @@ class RTABMAP_EXP Parameters
     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 GTSAM optimization strategy (see %s).", kOptimizerStrategy().c_str()));
 
 #ifdef RTABMAP_ORB_SLAM2
     RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
@@ -402,7 +410,7 @@ class RTABMAP_EXP Parameters
     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).");
@@ -411,6 +419,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.");
@@ -423,6 +432,7 @@ 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, 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.");
 #else
@@ -530,6 +540,9 @@ class RTABMAP_EXP Parameters
     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");
@@ -554,12 +567,12 @@ class RTABMAP_EXP Parameters
 #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.");
+    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.");
 #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.");
 #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.");
 #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).");
@@ -630,6 +643,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");
@@ -639,6 +654,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.");
@@ -685,6 +717,21 @@ class RTABMAP_EXP Parameters
     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/Rtabmap.h

@@ -166,8 +166,15 @@ public:
 			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);
+	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();
+	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
@@ -182,7 +189,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;
@@ -259,6 +266,9 @@ private:
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
 	bool _savedLocalizationIgnored;
+	bool _loopCovLimited;
+	bool _loopGPS;
+	int _maxOdomCacheSize;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -290,6 +300,10 @@ 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
 
 	// Planning stuff
 	int _pathStatus;

corelib/include/rtabmap/core/SensorData.h

@@ -33,11 +33,13 @@ 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>
 
 namespace rtabmap
 {
@@ -161,9 +163,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 +188,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 +214,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;}
 
@@ -235,20 +256,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,6 +312,12 @@ private:
 	float _cellSize;
 	cv::Point3f _viewPoint;
 
+	// environmental sensors
+	EnvSensors _envSensors;
+
+	// landmarks
+	Landmarks _landmarks;
+
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	std::vector<cv::Point3f> _keypoints3D;

corelib/include/rtabmap/core/Signature.h

@@ -82,7 +82,7 @@ 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);
 
@@ -90,10 +90,13 @@ public:
 	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 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 +143,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

corelib/include/rtabmap/core/Statistics.h

@@ -69,6 +69,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Optimization_iterations, );
 	RTABMAP_STATS(Loop, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
+	RTABMAP_STATS(Loop, Landmark_detected,);
+	RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
 
 	RTABMAP_STATS(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);
@@ -127,6 +129,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Timing, Forgetting, ms);
 	RTABMAP_STATS(Timing, Joining_trash, ms);
 	RTABMAP_STATS(Timing, Emptying_trash, ms);
+	RTABMAP_STATS(Timing, Finalizing_statistics, ms);
 
 	RTABMAP_STATS(TimingMem, Pre_update, ms);
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);
@@ -144,6 +147,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Post_decimation, ms);
 	RTABMAP_STATS(TimingMem, Scan_filtering, ms);
 	RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
+	RTABMAP_STATS(TimingMem, Markers_detection, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
 	RTABMAP_STATS(Keypoint, Indexed_words, words);
@@ -176,18 +180,22 @@ public:
 
 	// setters
 	void setExtended(bool extended) {_extended = extended;}
-	void setRefImageId(int refImageId) {_refImageId = refImageId;}
-	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
+	void setRefImageId(int id) {_refImageId = id;}
+	void setRefImageMapId(int id) {_refImageMapId = id;}
+	void setLoopClosureId(int id) {_loopClosureId = id;}
+	void setLoopClosureMapId(int id) {_loopClosureMapId = id;}
 	void setProximityDetectionId(int id) {_proximiyDetectionId = id;}
+	void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
 	void setStamp(double stamp) {_stamp = stamp;}
 
-	void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
+	void setLastSignatureData(const Signature & data) {_lastSignatureData = data;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
 	void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
 	void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
 	void setLocalizationCovariance(const cv::Mat & covariance) {_localizationCovariance = covariance;}
+	void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
 	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
@@ -200,17 +208,21 @@ public:
 	// getters
 	bool extended() const {return _extended;}
 	int refImageId() const {return _refImageId;}
+	int refImageMapId() const {return _refImageMapId;}
 	int loopClosureId() const {return _loopClosureId;}
+	int loopClosureMapId() const {return _loopClosureMapId;}
 	int proximityDetectionId() const {return _proximiyDetectionId;}
+	int proximityDetectionMapId() const {return _proximiyDetectionMapId;}
 	double stamp() const {return _stamp;}
 
-	const std::map<int, Signature> & getSignatures() const {return _signatures;}
+	const Signature & getLastSignatureData() const {return _lastSignatureData;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
 	const Transform & mapCorrection() const {return _mapCorrection;}
 	const Transform & loopClosureTransform() const {return _loopClosureTransform;}
 	const cv::Mat & localizationCovariance() const {return _localizationCovariance;}
+	const std::map<int, std::string> & labels() const {return _labels;}
 	const std::map<int, int> & weights() const {return _weights;}
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
@@ -226,11 +238,14 @@ private:
 	bool _extended; // 0 -> only loop closure and last signature ID fields are filled
 
 	int _refImageId;
+	int _refImageMapId;
 	int _loopClosureId;
+	int _loopClosureMapId;
 	int _proximiyDetectionId;
+	int _proximiyDetectionMapId;
 	double _stamp;
 
-	std::map<int, Signature> _signatures;
+	Signature _lastSignatureData;
 
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
@@ -238,6 +253,7 @@ private:
 	Transform _loopClosureTransform;
 	cv::Mat _localizationCovariance;
 
+	std::map<int, std::string> _labels;
 	std::map<int, int> _weights;
 	std::map<int, float> _posterior;
 	std::map<int, float> _likelihood;

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -97,6 +97,9 @@ public:
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
 	bool saveStereoTransform(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);
 
 	double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}
 

corelib/include/rtabmap/core/StereoDense.h

@@ -36,6 +36,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 class RTABMAP_EXP StereoDense {
+public:
+	enum Type {
+		kTypeBM = 0,
+		kTypeSGBM = 1
+	};
+	static StereoDense * create(const ParametersMap & parameters);
+	static StereoDense * create(StereoDense::Type type, const ParametersMap & parameters = ParametersMap());
+
 public:
 	virtual ~StereoDense() {}
 
@@ -48,29 +56,6 @@ protected:
 	StereoDense(const ParametersMap & parameters = ParametersMap()) {}
 };
 
-class RTABMAP_EXP StereoBM : public StereoDense {
-public:
-	StereoBM(int blockSize, int numDisparities);
-	StereoBM(const ParametersMap & parameters = ParametersMap());
-	virtual ~StereoBM() {}
-
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat computeDisparity(
-			const cv::Mat & leftImage,
-			const cv::Mat & rightImage) const;
-
-private:
-	int blockSize_;         //15
-	int minDisparity_;      //0
-	int numDisparities_;    //64
-	int preFilterSize_;     //9
-	int preFilterCap_;      //31
-	int uniquenessRatio_;   //15
-	int textureThreshold_;  //10
-	int speckleWindowSize_; //100
-	int speckleRange_;      //4
-};
-
 } /* namespace rtabmap */
 
 #endif /* STEREODENSE_H_ */

corelib/include/rtabmap/core/Transform.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RtabmapExp.h>
 #include <vector>
 #include <string>
+#include <map>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <opencv2/core/core.hpp>
@@ -149,12 +150,32 @@ public:
 	static Transform fromString(const std::string & string);
 	static bool canParseString(const std::string & string);
 
+	static Transform getClosestTransform(
+				const std::map<double, Transform> & tfBuffer,
+				const double & stamp,
+				double * stampDiff = 0);
+
 private:
 	cv::Mat data_;
 };
 
 RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
 
+class TransformStamped
+{
+public:
+	TransformStamped(const Transform & transform, const double & stamp) :
+		transform_(transform),
+		stamp_(stamp)
+	{}
+	const Transform & transform() const {return transform_;}
+	const double & stamp() const {return stamp_;}
+
+private:
+	Transform transform_;
+	double stamp_;
+};
+
 }
 
 #endif /* TRANSFORM_H_ */

corelib/include/rtabmap/core/VWDictionary.h

@@ -99,6 +99,10 @@ public:
 	void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
 	void deleteUnusedWords();
 
+public:
+	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn);
+	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn);
+
 protected:
 	int getNextId();
 

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

@@ -0,0 +1,77 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+typedef struct _freenect_context freenect_context;
+typedef struct _freenect_device freenect_device;
+
+namespace rtabmap
+{
+
+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
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,103 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace libfreenect2
+{
+class Freenect2;
+class Freenect2Device;
+class SyncMultiFrameListener;
+class Registration;
+class PacketPipeline;
+}
+
+namespace rtabmap
+{
+
+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
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,173 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/utilite/UTimer.h"
+#include <list>
+
+class UDirectory;
+
+namespace rtabmap
+{
+
+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
+	virtual void setMaxFrames(int value) {_maxFrames = value;}
+	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,
+			const Transform & localTransform=Transform::getIdentity())
+	{
+		_scanPath = dir;
+		_scanLocalTransform = localTransform;
+		_scanMaxPts = maxScanPts;
+	}
+
+	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;
+	int _maxFrames;
+	// 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;
+	int _framesPublished;
+	UDirectory * _dir;
+	std::string _lastFileName;
+
+	int _countScan;
+	UDirectory * _scanDir;
+	std::string _lastScanFileName;
+	std::string _scanPath;
+	Transform _scanLocalTransform;
+	int _scanMaxPts;
+
+	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;
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,97 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+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
+{
+
+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
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,93 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace openni
+{
+class Device;
+class VideoStream;
+}
+
+namespace rtabmap
+{
+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
+};
+
+
+
+} // namespace rtabmap

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

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

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

@@ -0,0 +1,96 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include "rtabmap/core/Camera.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 pcl
+{
+class Grabber;
+}
+
+namespace rtabmap
+{
+
+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_;
+};
+
+} // namespace rtabmap

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

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

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

@@ -0,0 +1,104 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace rs
+{
+	class context;
+	class device;
+	namespace slam {
+	class slam;
+	}
+}
+
+namespace rtabmap
+{
+
+class slam_event_handler;
+class RTABMAP_EXP CameraRealSense :
+	public Camera
+{
+public:
+	static bool available();
+	enum RGBSource {kColor, kInfrared, kFishEye};
+
+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();
+
+	void setDepthScaledToRGBSize(bool enabled);
+	void setRGBSource(RGBSource source);
+	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_;
+	bool depthScaledToRGBSize_;
+	RGBSource rgbSource_;
+	CameraModel cameraModel_;
+	std::vector<int> rsRectificationTable_;
+
+	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
+};
+
+} // namespace rtabmap

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

@@ -0,0 +1,132 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+#include <pcl/pcl_config.h>
+
+#ifdef RTABMAP_REALSENSE2
+#include <librealsense2/hpp/rs_frame.hpp>
+#endif
+
+
+namespace rs2
+{
+	class context;
+	class device;
+	class syncer;
+}
+struct rs2_intrinsics;
+struct rs2_extrinsics;
+
+namespace rtabmap
+{
+
+class RTABMAP_EXP CameraRealSense2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraRealSense2(
+		const std::string & deviceId = "",
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	bool odomProvided() const;
+
+	// parameters are set during initialization
+	// D400 series
+	void setEmitterEnabled(bool enabled);
+	void setIRDepthFormat(bool enabled);
+	void setResolution(int width, int height, int fps = 30);
+	// T265 related parameters
+	void setImagesRectified(bool enabled);
+	void setOdomProvided(bool enabled);
+
+#ifdef RTABMAP_REALSENSE2
+	void imu_callback(rs2::frame frame);
+	void pose_callback(rs2::frame frame);
+	void frame_callback(rs2::frame frame);
+	void multiple_message_callback(rs2::frame frame);
+	void getPoseAndIMU(
+			const double & stamp,
+			Transform & pose,
+			unsigned int & poseConfidence,
+			IMU & imu) const;
+#endif
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE2
+	rs2::context * ctx_;
+	rs2::device * dev_;
+	std::string deviceId_;
+	rs2::syncer * syncer_;
+	float depth_scale_meters_;
+	rs2_intrinsics * depthIntrinsics_;
+	rs2_intrinsics * rgbIntrinsics_;
+	rs2_extrinsics * depthToRGBExtrinsics_;
+	cv::Mat depthBuffer_;
+	cv::Mat rgbBuffer_;
+	CameraModel model_;
+	StereoCameraModel stereoModel_;
+	Transform imuLocalTransform_;
+	std::map<double, cv::Vec3f> accBuffer_;
+	std::map<double, cv::Vec3f> gyroBuffer_;
+	std::map<double, std::pair<Transform, unsigned int> > poseBuffer_; // <stamp, <Pose, confidence: 1=lost, 2=low, 3=high> >
+	UMutex poseMutex_;
+	UMutex imuMutex_;
+
+	bool emitterEnabled_;
+	bool irDepth_;
+	bool rectifyImages_;
+	bool odometryProvided_;
+	int cameraWidth_;
+	int cameraHeight_;
+	int cameraFps_;
+
+	static Transform realsense2PoseRotation_;
+	static Transform realsense2PoseRotationInv_;
+#endif
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,66 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace rtabmap
+{
+
+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
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,68 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace FlyCapture2
+{
+class Camera;
+}
+
+namespace rtabmap
+{
+
+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
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,76 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/camera/CameraImages.h>
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Version.h"
+
+namespace rtabmap
+{
+
+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
+	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);}
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+	CameraImages * camera2_;
+	StereoCameraModel stereoModel_;
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,75 @@
+/*
+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.
+*/
+
+/**
+ * Contributed by e-consystemgit
+ * https://www.e-consystems.com/opensource-linux-webcam-software-application.asp
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/camera/CameraVideo.h"
+#include "rtabmap/core/Version.h"
+
+
+namespace rtabmap
+{
+
+class RTABMAP_EXP CameraStereoTara :
+public Camera
+{
+public:
+	static bool available();
+public:
+
+	CameraStereoTara(
+			int device,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+
+	virtual ~CameraStereoTara();
+
+	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_;
+	bool rectifyImages_;
+	StereoCameraModel stereoModel_;
+	std::string cameraName_;
+	int usbDevice_;
+};
+
+} // namespace rtabmap

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

@@ -0,0 +1,89 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/camera/CameraVideo.h"
+
+namespace rtabmap
+{
+
+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());
+	CameraStereoVideo(
+			int deviceLeft,
+			int deviceRight,
+			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_;
+	int usbDevice2_;
+};
+
+} // namespace rtabmap

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

@@ -0,0 +1,102 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/camera/CameraVideo.h"
+#include "rtabmap/core/Version.h"
+
+namespace sl
+{
+class Camera;
+}
+
+namespace rtabmap
+{
+
+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 = true,
+			bool odomForce3DoF = 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 = true,
+			bool odomForce3DoF = 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_;
+	Transform imuLocalTransform_;
+	CameraVideo::Source src_;
+	int usbDevice_;
+	std::string svoFilePath_;
+	int resolution_;
+	int quality_;
+	bool selfCalibration_;
+	int sensingMode_;
+	int confidenceThr_;
+	bool computeOdometry_;
+	bool lost_;
+	bool force3DoF_;
+#endif
+};
+
+
+} // namespace rtabmap

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

@@ -0,0 +1,89 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include "rtabmap/core/Camera.h"
+
+namespace rtabmap
+{
+
+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;}
+
+	/**
+	 * Set wanted usb resolution, should be set before initialization. 0 means
+	 * default resolution. It won't be applied if a valid camera calibration
+	 * has been loaded, thus resolution from calibration is used.
+	 * */
+	void setResolution(int width, int height) {_width=width, _height=height;}
+
+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;
+	int _width;
+	int _height;
+
+	CameraModel _model;
+};
+
+
+} // namespace rtabmap

corelib/include/rtabmap/core/impl/OccupancyGrid.hpp

@@ -44,6 +44,13 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		pcl::IndicesPtr & obstaclesIndices,
 		pcl::IndicesPtr * flatObstacles) const
 {
+	groundIndices.reset(new std::vector<int>);
+	obstaclesIndices.reset(new std::vector<int>);
+	if(flatObstacles)
+	{
+		flatObstacles->reset(new std::vector<int>);
+	}
+
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 	pcl::IndicesPtr indices(new std::vector<int>);
 

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

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYF2M_H_
 
 #include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/Optimizer.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
@@ -49,6 +50,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	const Signature & getMap() const {return *map_;}
 	const Signature & getLastFrame() const {return *lastFrame_;}
+	virtual bool canProcessIMU() const;
 
 	virtual Odometry::Type getType() {return Odometry::kTypeF2M;}
 
@@ -67,16 +69,20 @@ private:
 	float scanSubtractAngle_;
 	int bundleAdjustment_;
 	int bundleMaxFrames_;
+	float validDepthRatio_;
 
 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
 	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
+	std::map<double, Transform> imus_;
+	bool initGravity_;
 
-	std::map<int, std::map<int, cv::Point3f> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
+	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
 	std::map<int, Transform> bundlePoses_;
 	std::multimap<int, Link> bundleLinks_;
+	std::multimap<int, Link> bundleIMUOrientations_;
 	std::map<int, CameraModel> bundleModels_;
 	std::map<int, int> bundlePoseReferences_;
 	int bundleSeq_;

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

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

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

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

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

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

corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h

@@ -57,7 +57,7 @@ public:
 			const std::multimap<int, Link> & links,
 			const std::map<int, CameraModel> & models,
 			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(x,y,depth)>
 			std::set<int> * outliers = 0);
 };
 

corelib/include/rtabmap/core/optimizer/OptimizerG2O.h

@@ -40,18 +40,13 @@ public:
 	static bool available();
 	static bool isCSparseAvailable();
 	static bool isCholmodAvailable();
-	static bool saveGraph(
-			const std::string & fileName,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
-			bool useRobustConstraints = false);
 
 public:
 	OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
 		Optimizer(parameters),
 		solver_(Parameters::defaultg2oSolver()),
 		optimizer_(Parameters::defaultg2oOptimizer()),
-        pixelVariance_(Parameters::defaultg2oPixelVariance()),
+		pixelVariance_(Parameters::defaultg2oPixelVariance()),
 		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
 		baseline_(Parameters::defaultg2oBaseline())
 	{
@@ -78,9 +73,14 @@ public:
 			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(x,y,depth)>
 			std::set<int> * outliers = 0);
 
+	bool saveGraph(
+		const std::string & fileName,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints);
+
 private:
 	int solver_;
 	int optimizer_;

corelib/include/rtabmap/core/optimizer/OptimizerTORO.h

@@ -40,10 +40,6 @@ public:
 	static bool available();
 
 public:
-	static bool saveGraph(
-			const std::string & fileName,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints);
 	static bool loadGraph(
 			const std::string & fileName,
 			std::map<int, Transform> & poses,
@@ -70,6 +66,11 @@ public:
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 			double * finalError = 0,
 			int * iterationsDone = 0);
+
+	bool saveGraph(
+		const std::string & fileName,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints);
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/stereo/StereoBM.h

@@ -0,0 +1,65 @@
+/*
+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 STEREOBM_H_
+#define STEREOBM_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/StereoDense.h>
+#include <rtabmap/core/Parameters.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_EXP StereoBM : public StereoDense {
+public:
+	StereoBM(int blockSize, int numDisparities);
+	StereoBM(const ParametersMap & parameters = ParametersMap());
+	virtual ~StereoBM() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual cv::Mat computeDisparity(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage) const;
+
+private:
+	int blockSize_;         //15
+	int minDisparity_;      //0
+	int numDisparities_;    //64
+	int preFilterSize_;     //9
+	int preFilterCap_;      //31
+	int uniquenessRatio_;   //15
+	int textureThreshold_;  //10
+	int speckleWindowSize_; //100
+	int speckleRange_;      //4
+	int disp12MaxDiff_;     //-1
+};
+
+} /* namespace rtabmap */
+
+#endif /* STEREOBM_H_ */

corelib/include/rtabmap/core/stereo/StereoSGBM.h

@@ -0,0 +1,65 @@
+/*
+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 STEREOSGBM_H_
+#define STEREOSGBM_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/StereoDense.h>
+#include <rtabmap/core/Parameters.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_EXP StereoSGBM : public StereoDense {
+public:
+	StereoSGBM(const ParametersMap & parameters = ParametersMap());
+	virtual ~StereoSGBM() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual cv::Mat computeDisparity(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage) const;
+
+private:
+	int blockSize_;         //15
+	int minDisparity_;      //0
+	int numDisparities_;    //64
+	int preFilterCap_;      //31
+	int uniquenessRatio_;   //15
+	int speckleWindowSize_; //100
+	int speckleRange_;      //4
+	int P1_;                //0
+	int P2_;                //0
+	int disp12MaxDiff_;     //0
+	int mode_;              //0=cv::StereoSGBM::MODE_SGBM;
+};
+
+} /* namespace rtabmap */
+
+#endif /* STEREOSGBM_H_ */

corelib/include/rtabmap/core/util3d.h

@@ -234,12 +234,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const Lase
 // For laserScan without normals, normals are set to null.
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform = Transform());
 // For laserScan without rgb, rgb is set to default r,g,b parameters.
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);
 // For laserScan without intensity, intensity is set to intensity parameter.
 pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
 // For laserScan without rgb, rgb is set to default r,g,b parameters.
 // For laserScan without normals, normals are set to null.
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);
 // For laserScan without intensity, intensity is set to default intensity parameter.
 // For laserScan without normals, normals are set to null.
 pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
@@ -249,7 +249,7 @@ pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const LaserScan & laserScan, int inde
 // For laserScan without normals, normals are set to null.
 pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const LaserScan & laserScan, int index);
 // For laserScan without rgb, rgb is set to default r,g,b parameters.
-pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 100, unsigned char g = 100, unsigned char b = 100);
 // For laserScan without intensity, intensity is set to intensity parameter.
 pcl::PointXYZI RTABMAP_EXP laserScanToPointI(const LaserScan & laserScan, int index, float intensity);
 // For laserScan without rgb, rgb is set to default r,g,b parameters.

corelib/include/rtabmap/core/util3d_filtering.h

@@ -72,12 +72,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP downsample(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int step);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		int step);
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP downsample(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		int step);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP downsample(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		int step);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		int step);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -316,12 +322,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud);
 
 
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud);
 
 /**
  * For convenience.

corelib/src/BayesFilter.cpp

@@ -43,7 +43,8 @@ namespace rtabmap {
 BayesFilter::BayesFilter(const ParametersMap & parameters) :
 	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
 	_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
-	_totalPredictionLCValues(0.0f)
+	_totalPredictionLCValues(0.0f),
+	_predictionEpsilon(0.0f)
 {
 	this->setPredictionLC(Parameters::defaultBayesPredictionLC());
 	this->parseParameters(parameters);
@@ -103,6 +104,14 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
 	for(unsigned int j=0; j<_predictionLC.size(); ++j)
 	{
 		_totalPredictionLCValues += _predictionLC[j];
+		if(j==0 || _predictionLC[j] < _predictionEpsilon)
+		{
+			_predictionEpsilon = _predictionLC[j];
+		}
+	}
+	if(!_predictionLC.empty())
+	{
+		UDEBUG("predictionEpsilon = %f", _predictionEpsilon);
 	}
 }
 
@@ -263,9 +272,16 @@ float addNeighborProb(cv::Mat & prediction,
 
 cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
 {
+	std::vector<int> oldIds = uKeys(_posterior);
+	if(oldIds.size() == ids.size() &&
+		memcmp(oldIds.data(), ids.data(), oldIds.size()*sizeof(int)) == 0)
+	{
+		return _prediction;
+	}
+
 	if(!_fullPredictionUpdate && !_prediction.empty())
 	{
-		return updatePrediction(_prediction, memory, uKeys(_posterior), ids);
+		return updatePrediction(_prediction, memory, oldIds, ids);
 	}
 	UDEBUG("");
 
@@ -441,6 +457,10 @@ void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float adde
 		for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
 		{
 			((float*)prediction.data)[index + j*cols] *= maxNorm / addedProbabilitiesSum;
+			if(((float*)prediction.data)[index + j*cols] < _predictionEpsilon)
+			{
+				((float*)prediction.data)[index + j*cols] = 0.0f;
+			}
 		}
 		addedProbabilitiesSum = maxNorm;
 	}
@@ -518,8 +538,17 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	}
 	UDEBUG("time getting removed ids = %fs", timer.restart());
 
+	bool oldAllCopied = false;
+	if(removedIds.empty() &&
+		newIds.size() > oldIds.size() &&
+		memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) == 0)
+	{
+		oldPrediction.copyTo(cv::Mat(prediction, cv::Range(0, oldPrediction.rows), cv::Range(0, oldPrediction.cols)));
+		oldAllCopied = true;
+		UDEBUG("Copied all old prediction: = %fs", timer.ticks());
+	}
+
 	int added = 0;
-	float epsilon = 0.00001f;
 	// get ids to update
 	std::set<int> idsToUpdate;
 	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
@@ -532,9 +561,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 				int count = 0;
 				for(unsigned int j=0; j<cols; ++j)
 				{
-					if(((const float *)oldPrediction.data)[i + j*cols] > epsilon &&
-					   j!=i &&
-					   removedIds.find(oldIds[j]) == removedIds.end())
+					if(j!=i && removedIds.find(oldIds[j]) == removedIds.end())
 					{
 						//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
 						idsToUpdate.insert(oldIds[j]);
@@ -565,6 +592,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 
 			float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap);
 			this->normalize(prediction, i, sum, newIds[0]<0);
+
 			++added;
 			int count = 0;
 			for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
@@ -609,36 +637,39 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	}
 	UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
 
-	//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
-	//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
-	// copy not changed probabilities
 	int copied = 0;
-	for(unsigned int i=0; i<oldIds.size(); ++i)
+	if(!oldAllCopied)
 	{
-		if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
+		//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
+		//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
+		// copy not changed probabilities
+		for(unsigned int i=0; i<oldIds.size(); ++i)
 		{
-			for(int j=i; j<oldPrediction.cols; ++j)
+			if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
 			{
-				if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] > epsilon)
+				for(int j=0; j<oldPrediction.cols; ++j)
 				{
-					//UDEBUG("i=%d, j=%d", i, j);
-					//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
-					//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
-					//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
-					float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
-					int ii = newIdToIndexMap.at(oldIds[i]);
-					int jj = newIdToIndexMap.at(oldIds[j]);
-					((float *)prediction.data)[ii + jj*prediction.cols] = v;
-					if(ii != jj)
+					if(oldIds[j]>0 && removedIds.find(oldIds[j]) == removedIds.end())
 					{
-						((float *)prediction.data)[jj + ii*prediction.cols] = v;
+						//UDEBUG("i=%d, j=%d", i, j);
+						//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
+						//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
+						//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
+						float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
+						int ii = newIdToIndexMap.at(oldIds[i]);
+						int jj = newIdToIndexMap.at(oldIds[j]);
+						((float *)prediction.data)[ii + jj*prediction.cols] = v;
+						//if(ii != jj)
+						//{
+						//	((float *)prediction.data)[jj + ii*prediction.cols] = v;
+						//}
 					}
 				}
+				++copied;
 			}
-			++copied;
 		}
+		UDEBUG("time copying = %fs", timer.restart());
 	}
-	UDEBUG("time copying = %fs", timer.restart());
 
 	//update virtual place
 	if(newIds[0] < 0)

corelib/src/CMakeLists.txt

@@ -15,11 +15,26 @@ SET(SRC_FILES
 	
     Camera.cpp
     CameraThread.cpp
-    CameraRGB.cpp
-    CameraRGBD.cpp
-    CameraStereo.cpp
     CameraModel.cpp
     
+    camera/CameraFreenect.cpp
+    camera/CameraFreenect2.cpp
+    camera/CameraImages.cpp
+    camera/CameraK4W2.cpp
+    camera/CameraOpenni.cpp
+    camera/CameraOpenNI2.cpp
+    camera/CameraOpenNICV.cpp
+    camera/CameraRealSense.cpp
+    camera/CameraRealSense2.cpp
+    camera/CameraRGBDImages.cpp
+    camera/CameraStereoDC1394.cpp
+    camera/CameraStereoFlyCapture2.cpp
+    camera/CameraStereoImages.cpp
+    camera/CameraStereoVideo.cpp
+    camera/CameraStereoZed.cpp
+	camera/CameraStereoTara.cpp
+    camera/CameraVideo.cpp
+    
     EpipolarGeometry.cpp
 	VisualWord.cpp
 	VWDictionary.cpp
@@ -49,10 +64,10 @@ SET(SRC_FILES
 	LaserScan.cpp
 	
 	Optimizer.cpp
-	OptimizerTORO.cpp
-	OptimizerG2O.cpp
-	OptimizerGTSAM.cpp
-	OptimizerCVSBA.cpp
+	optimizer/OptimizerTORO.cpp
+	optimizer/OptimizerG2O.cpp
+	optimizer/OptimizerGTSAM.cpp
+	optimizer/OptimizerCVSBA.cpp
 	
 	Registration.cpp
 	RegistrationIcp.cpp
@@ -60,25 +75,32 @@ SET(SRC_FILES
 	
 	Odometry.cpp
 	OdometryThread.cpp
-	OdometryF2M.cpp
-	OdometryMono.cpp
-	OdometryF2F.cpp
-	OdometryFovis.cpp
-	OdometryViso2.cpp
-	OdometryDVO.cpp
-	OdometryOkvis.cpp
-	OdometryORBSLAM2.cpp
-	OdometryLOAM.cpp
-	OdometryMSCKF.cpp
+	odometry/OdometryF2M.cpp
+	odometry/OdometryMono.cpp
+	odometry/OdometryF2F.cpp
+	odometry/OdometryFovis.cpp
+	odometry/OdometryViso2.cpp
+	odometry/OdometryDVO.cpp
+	odometry/OdometryOkvis.cpp
+	odometry/OdometryORBSLAM2.cpp
+	odometry/OdometryLOAM.cpp
+	odometry/OdometryMSCKF.cpp
+	odometry/OdometryVINS.cpp
 	
 	IMUThread.cpp
+	IMUFilter.cpp
+	imufilter/ComplementaryFilter.cpp
 	
 	Stereo.cpp
 	StereoDense.cpp
 	StereoCameraModel.cpp
+	stereo/StereoBM.cpp
+	stereo/StereoSGBM.cpp
 	
 	OccupancyGrid.cpp
 	
+	MarkerDetector.cpp
+	
 	GainCompensator.cpp
 		
 	rtflann/ext/lz4.c
@@ -90,6 +112,8 @@ SET(SRC_FILES
 	clams/discrete_depth_distortion_model.cpp
 	clams/frame_projector.cpp
 	clams/slam_calibrator.cpp
+	
+	opencv/ORBextractor.cc
 )
 
 IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -248,12 +272,12 @@ ENDIF(FlyCapture2_FOUND)
 IF(WITH_TORO)
 	SET(SRC_FILES
     	${SRC_FILES}
-		toro3d/posegraph3.cpp
-		toro3d/treeoptimizer3_iteration.cpp
-		toro3d/treeoptimizer3.cpp
+		optimizer/toro3d/posegraph3.cpp
+		optimizer/toro3d/treeoptimizer3_iteration.cpp
+		optimizer/toro3d/treeoptimizer3.cpp
 	
-		toro3d/posegraph2.cpp
-		toro3d/treeoptimizer2.cpp
+		optimizer/toro3d/posegraph2.cpp
+		optimizer/toro3d/treeoptimizer2.cpp
 	)
 ENDIF(WITH_TORO)
 
@@ -266,15 +290,18 @@ IF(G2O_FOUND)
 		${LIBRARIES}
 		${G2O_LIBRARIES}
 	)
-	
+	SET(SRC_FILES
+    		${SRC_FILES}
+		optimizer/g2o/edge_se3_xyzprior.cpp
+	)
 	IF(WITH_VERTIGO)
 		SET(SRC_FILES
-    		${SRC_FILES}
-			vertigo/g2o/edge_se2Switchable.cpp
-			vertigo/g2o/edge_se3Switchable.cpp
-			vertigo/g2o/edge_switchPrior.cpp
-			vertigo/g2o/types_g2o_robust.cpp
-			vertigo/g2o/vertex_switchLinear.cpp
+    			${SRC_FILES}
+			optimizer/vertigo/g2o/edge_se2Switchable.cpp
+			optimizer/vertigo/g2o/edge_se3Switchable.cpp
+			optimizer/vertigo/g2o/edge_switchPrior.cpp
+			optimizer/vertigo/g2o/types_g2o_robust.cpp
+			optimizer/vertigo/g2o/vertex_switchLinear.cpp
 		)
 	ENDIF(WITH_VERTIGO)
 ENDIF(G2O_FOUND)
@@ -409,6 +436,17 @@ IF(msckf_vio_FOUND)
 	)
 ENDIF(msckf_vio_FOUND)
 
+IF(vins_FOUND)
+    SET(INCLUDE_DIRS
+		${vins_INCLUDE_DIRS}
+		${INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${vins_LIBRARIES}
+		${LIBRARIES}
+	)
+ENDIF(vins_FOUND)
+
 IF(ORB_SLAM2_FOUND)
     SET(INCLUDE_DIRS
         ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
@@ -433,6 +471,10 @@ IF(GTSAM_FOUND)
 			${GTSAM_INCLUDE_DIRS}
 		)
 	ENDIF()
+	SET(SRC_FILES
+    	${SRC_FILES}
+		optimizer/gtsam/GravityFactor.cpp
+	)
 	IF(WIN32)
 		# GTSAM should be built in STATIC on Windows to avoid "error C2338: THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS" when building GTSAM
 		add_definitions("-DGTSAM_IMPORT_STATIC") 
@@ -443,6 +485,13 @@ IF(GTSAM_FOUND)
 	)
 ENDIF(GTSAM_FOUND)
 
+IF(WITH_MADGWICK)
+	SET(SRC_FILES
+		${SRC_FILES}
+		imufilter/MadgwickFilter.cpp
+	)
+ENDIF(WITH_MADGWICK)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/Camera.cpp

@@ -68,9 +68,10 @@ SensorData Camera::takeImage(CameraInfo * info)
 {
 	bool warnFrameRateTooHigh = false;
 	float actualFrameRate = 0;
-	if(_imageRate>0)
+	float imageRate = _imageRate;
+	if(imageRate>0)
 	{
-		int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
+		int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
 		if(sleepTime > 2)
 		{
 			uSleep(sleepTime-2);
@@ -82,14 +83,14 @@ SensorData Camera::takeImage(CameraInfo * info)
 		}
 
 		// Add precision at the cost of a small overhead
-		while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
+		while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
 		{
 			//
 		}
 
 		double slept = _frameRateTimer->getElapsedTime();
 		_frameRateTimer->start();
-		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
+		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
 	}
 
 	UTimer timer;
@@ -98,7 +99,7 @@ SensorData Camera::takeImage(CameraInfo * info)
 	if(warnFrameRateTooHigh)
 	{
 		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
-				_imageRate, actualFrameRate, captureTime);
+				imageRate, actualFrameRate, captureTime);
 	}
 	else
 	{

corelib/src/CameraModel.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Version.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
@@ -165,8 +166,8 @@ void CameraModel::initRectificationMap()
 		// Equidistant / FishEye
 		// get only k parameters (k1,k2,p1,p2,k3,k4)
 		cv::Mat D(1, 4, CV_64FC1);
-		D.at<double>(0,0) = D_.at<double>(0,1);
-		D.at<double>(0,1) = D_.at<double>(0,2);
+		D.at<double>(0,0) = D_.at<double>(0,0);
+		D.at<double>(0,1) = D_.at<double>(0,1);
 		D.at<double>(0,2) = D_.at<double>(0,4);
 		D.at<double>(0,3) = D_.at<double>(0,5);
 		cv::fisheye::initUndistortRectifyMap(K_, D, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
@@ -449,6 +450,124 @@ bool CameraModel::save(const std::string & directory) const
 	return false;
 }
 
+std::vector<unsigned char> CameraModel::serialize() const
+{
+	const int headerSize = 11;
+	int header[headerSize] = {
+			RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
+			0, //mono                                                            // 3,
+			imageSize_.width, imageSize_.height,                                 // 4,5
+			(int)K_.total(), (int)D_.total(), (int)R_.total(), (int)P_.total(),  // 6,7,8,9
+			localTransform_.isNull()?0:localTransform_.size()};                  // 10
+	UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
+	std::vector<unsigned char> data(
+			sizeof(int)*headerSize +
+			sizeof(double)*(K_.total()+D_.total()+R_.total()+P_.total()) +
+			(localTransform_.isNull()?0:sizeof(float)*localTransform_.size()));
+	memcpy(data.data(), header, sizeof(int)*headerSize);
+	int index = sizeof(int)*headerSize;
+	if(!K_.empty())
+	{
+		memcpy(data.data()+index, K_.data, sizeof(double)*(K_.total()));
+		index+=sizeof(double)*(K_.total());
+	}
+	if(!D_.empty())
+	{
+		memcpy(data.data()+index, D_.data, sizeof(double)*(D_.total()));
+		index+=sizeof(double)*(D_.total());
+	}
+	if(!R_.empty())
+	{
+		memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
+		index+=sizeof(double)*(R_.total());
+	}
+	if(!P_.empty())
+	{
+		memcpy(data.data()+index, P_.data, sizeof(double)*(P_.total()));
+		index+=sizeof(double)*(P_.total());
+	}
+	if(!localTransform_.isNull())
+	{
+		memcpy(data.data()+index, localTransform_.data(), sizeof(float)*(localTransform_.size()));
+		index+=sizeof(float)*(localTransform_.size());
+	}
+	return data;
+}
+
+unsigned int CameraModel::deserialize(const std::vector<unsigned char>& data)
+{
+	return deserialize(data.data(), data.size());
+}
+unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
+{
+	*this = CameraModel();
+	int headerSize = 11;
+	if(dataSize >= sizeof(int)*headerSize)
+	{
+		UASSERT(data != 0);
+		const int * header = (const int *)data;
+		int type = header[3];
+		if(type == 0)
+		{
+			imageSize_.width = header[4];
+			imageSize_.height = header[5];
+			int iK = 6;
+			int iD = 7;
+			int iR = 8;
+			int iP = 9;
+			int iL = 10;
+			UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
+			unsigned int requiredDataSize = sizeof(int)*headerSize +
+					sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
+					sizeof(float)*header[iL];
+			UASSERT_MSG(dataSize >= requiredDataSize,
+					uFormat("dataSize=%d != required=%d (header: version %d.%d.%d %dx%d type=%d K=%d D=%d R=%d P=%d L=%d)",
+							dataSize,
+							requiredDataSize,
+							header[0], header[1], header[2], header[4], header[5], header[3],
+							header[iK], header[iD], header[iR],header[iP], header[iL]).c_str());
+			unsigned int index = sizeof(int)*headerSize;
+			if(header[iK] != 0)
+			{
+				UASSERT(header[iK] == 9);
+				K_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(K_.total());
+			}
+			if(header[iD] != 0)
+			{
+				D_ = cv::Mat(1, header[iD], CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(D_.total());
+			}
+			if(header[iR] != 0)
+			{
+				UASSERT(header[iR] == 9);
+				R_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(R_.total());
+			}
+			if(header[iP] != 0)
+			{
+				UASSERT(header[iP] == 12);
+				P_ = cv::Mat(3, 4, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(P_.total());
+			}
+			if(header[iL] != 0)
+			{
+				UASSERT(header[iL] == 12);
+				memcpy(localTransform_.data(), data+index, sizeof(float)*localTransform_.size());
+				index+=sizeof(float)*localTransform_.size();
+			}
+			UASSERT(index <= dataSize);
+			return index;
+		}
+		else
+		{
+			UERROR("Serialized calibration is not mono (type=%d), use the appropriate class matching the type to deserialize.", type);
+		}
+	}
+	UERROR("Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (int)dataSize);
+	return 0;
+}
+
 CameraModel CameraModel::scaled(double scale) const
 {
 	CameraModel scaledModel = *this;