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.

RegICP: Updated warning when complexity is too low. LoopClosureViewer: fixed scan transform.
Reduced local bundle on loop closure iterations to 5. Updated graph::filterLinks() to handle selfRef link type. DbViewer: removed gravity constraints from constraints view.
2026-09-17 00:37:45 +08:00 · 2019-06-02 14:59:11 -04:00 · 2019-06-02 12:38:09 -04:00 · 2019-06-01 21:43:13 -04:00 · 2019-05-31 19:31:11 -04:00 · 2019-05-31 16:51:37 -04:00
444 changed files with 81493 additions and 25998 deletions
@@ -0,0 +1,137 @@
+
+branches:
+  only:
+    - master
+    - devel
+
+os: Visual Studio 2015
+
+clone_folder: c:\projects\rtabmap
+
+platform: x64
+configuration: Release
+
+init:
+  - cmake --version
+  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
+  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
+
+install:
+  # Qt
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
+  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
+  - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
+  # Openni2
+  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
+  - cmd: OpenNI2.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenNI2:"
+  - ps: "ls \"C:/Program Files/OpenNI2\""
+  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
+  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
+  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
+  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
+  # OpenCV
+  #- ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
+  #- cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
+  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe' -outfile opencv-2.4.13.6-vc14.exe
+  - cmd: opencv-2.4.13.6-vc14.exe -o"C:\Program Files" -y
+  - 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 VTK:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed QHull:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed FLANN:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed Eigen:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe
+  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
+  # zlib
+  - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
+  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
+  - ECHO "Installed zlib:"
+  - ps: "ls \"C:/Program Files/zlib\""
+  - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
+  # OctoMap
+  - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe
+  - cmd: octomap.exe -o"C:\Program Files" -y
+  - ECHO "Installed OctoMap:"
+  - ps: "ls \"C:/Program Files/octomap-distribution\""
+  - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin
+  # CPU-TSDF
+  - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe
+  - cmd: cpu_tsdf.exe -o"C:\Program Files" -y
+  - ECHO "Installed CPU-TSDF:"
+  - ps: "ls \"C:/Program Files/cpu_tsdf\""
+  - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin
+  # Open Chisel
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe
+  - cmd: open_chisel.exe -o"C:\Program Files" -y
+  - ECHO "Installed Open Chisel:"
+  - ps: "ls \"C:/Program Files/open_chisel\""
+  - set PATH=%PATH%;C:\Program Files\open_chisel\bin
+  # cvsba
+  - ps: wget 'https://dl.dropboxusercontent.com/s/4ey8ergerx46zvj/cvsba_x64_vc14.exe?dl=0' -outfile cvsba.exe
+  - cmd: cvsba.exe -o"C:\Program Files" -y
+  - ECHO "Installed cvsba:"
+  - ps: "ls \"C:/Program Files/cvsba\""
+  - set PATH=%PATH%;C:\Program Files\cvsba\bin
+  - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe
+  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
+  - ECHO "Installed yaml-cpp:"
+  - ps: "ls \"C:/Program Files/yaml-cpp\""
+  
+before_build:
+  - cd c:\projects\rtabmap\build
+  - ECHO %PROGRAMFILES%
+  - ECHO %PATH%
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\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
+
+artifacts:
+  - path: build\RTABMap-*
+  
+notifications:
+  - provider: Email
+    to:
+      - matlabbe@gmail.com
+    on_build_success: false
+    on_build_failure: false
+    on_build_status_changed: true
@@ -1,6 +1,9 @@
 /lib
 .DS_Store
 .settings/language.settings.xml
+.idea/
+.vscode
+cmake-build-debug/
 app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml
@@ -1,6 +1,7 @@
 sudo: true
 dist: trusty
 language: cpp
+group: deprecated-2017Q3

 compiler:
  - gcc
@@ -13,11 +14,13 @@ addons:
      - libopencv-dev
      - libqt4-dev
      - libsqlite3-dev
+      - libyaml-cpp-dev

 install:
    - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
    - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
    - sudo apt-get update
+    - sudo apt-get update && sudo apt-get install dpkg
    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev

 script: 
@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 12)
-SET(RTABMAP_PATCH_VERSION 5)
+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)

@@ -113,20 +131,27 @@ SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
 SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")

 ####### INSTALL DIR #######
-set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH
-  "Installation directory for header files")
+set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 if(WIN32 AND NOT CYGWIN)
  set(DEF_INSTALL_CMAKE_DIR CMake)
 else()
  set(DEF_INSTALL_CMAKE_DIR ${CMAKE_INSTALL_LIBDIR}/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 endif()
-set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
-  "Installation directory for CMake files")
+set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR})
  
 ####### BUILD OPTIONS #######
-IF(APPLE)
-	OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
-ENDIF(APPLE)
+
+# ANDROID_PREBUILD (early exit if true)
+OPTION( ANDROID_PREBUILD "Set to ON to build rtabmap resource build tool (required for android build)" OFF )
+IF(ANDROID_PREBUILD)
+   MESSAGE("Option ANDROID_PREBUILD is set, only rtabmap resource tool will be built. You can use android toolchain after that.")
+   ADD_SUBDIRECTORY( utilite )
+   return()
+ENDIF(ANDROID_PREBUILD)
+
+IF(APPLE OR WIN32)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle with all embedded dependencies (DragNDrop for Mac, installer for Windows)" OFF)
+ENDIF(APPLE OR WIN32)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
@@ -137,8 +162,10 @@ 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)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
@@ -146,20 +173,56 @@ option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
+option(WITH_LOAM          "Include LOAM support"                 ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
+option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
+option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
+option(WITH_FOVIS         "Include FOVIS support"                ON)
+option(WITH_VISO2         "Include VISO2 support"                ON)
+option(WITH_DVO           "Include DVO support"                  ON)
+option(WITH_ORB_SLAM2     "Include ORB_SLAM2 support"            ON)
+option(WITH_OKVIS         "Include OKVIS support"                ON)
+option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"           OFF)
+option(WITH_VINS          "Include VINS-Fusion support"          ON)
+option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(PCL_OMP            "With PCL OMP implementations"         ON)

 FIND_PACKAGE(OpenCV REQUIRED QUIET)
-FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
+
+IF(WITH_QT)
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
+ELSE()
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation )
+ENDIF()
+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()
+  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)

-# fix libproj.so not found on Xenial
+FIND_PACKAGE(Sqlite3 QUIET)
+IF(Sqlite3_FOUND)
+   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
+ENDIF(Sqlite3_FOUND)
+
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
+   # fix libproj.so not found on Xenial
   list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
 endif()

@@ -171,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)
@@ -201,12 +265,12 @@ IF(WITH_QT)
    IF("${VTK_MAJOR_VERSION}" GREATER 5)
        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
        IF(Qt5_FOUND)
-             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
        ENDIF(Qt5_FOUND)
    ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)

    IF(NOT Qt5_FOUND)
-        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
    ENDIF(NOT Qt5_FOUND)
        
    IF(QT4_FOUND OR Qt5_FOUND)
@@ -218,7 +282,12 @@ IF(WITH_QT)
                SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
                SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
            ENDIF(value EQUAL -1)
+			MESSAGE(STATUS "VTK_RENDERING_BACKEND=${VTK_RENDERING_BACKEND}")
+			IF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
+				ADD_DEFINITIONS("-DVTK_OPENGL2")
+			ENDIF(VTK_RENDERING_BACKEND STREQUAL "OpenGL2")
        ENDIF()
+        ADD_DEFINITIONS(-DQT_NO_KEYWORDS) # To avoid conflicts with boost signals/foreach and Qt macros
    ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)

@@ -247,6 +316,13 @@ IF(WITH_FREENECT2)
    ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)

+IF(WITH_K4W2 AND WIN32)
+    FIND_PACKAGE(KinectSDK2 QUIET)
+	IF(KinectSDK2_FOUND)
+       MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}")
+    ENDIF(KinectSDK2_FOUND)
+ENDIF(WITH_K4W2 AND WIN32)
+
 # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
 IF(WITH_OPENNI2 OR OpenNI2_FOUND)
    FIND_PACKAGE(OpenNI2 QUIET)
@@ -270,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)
@@ -287,22 +364,23 @@ IF(WITH_CVSBA)
    ENDIF(cvsba_FOUND)
 ENDIF(WITH_CVSBA)

+IF(WITH_POINTMATCHER)
+    find_package(libpointmatcher QUIET)
+    IF(libpointmatcher_FOUND)
+       MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
+    ENDIF(libpointmatcher_FOUND)
+ENDIF(WITH_POINTMATCHER)
+
+IF(WITH_LOAM)
+    find_package(loam_velodyne QUIET)
+    IF(loam_velodyne_FOUND)
+       MESSAGE(STATUS "Found loam_velodyne: ${loam_velodyne_INCLUDE_DIRS}")
+    ENDIF(loam_velodyne_FOUND)
+ENDIF(WITH_LOAM)
+
+SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
-	IF(WIN32) # Windows
-		SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
-		if (CMAKE_CL_64) # 64 bits
-			SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
-		else(CMAKE_CL_64) # 32 bits
-			message("32bits compilation is no more available with CUDA7.0")
-		endif(CMAKE_CL_64)
-		SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
-		IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
-			SET(ZED_FOUND TRUE)
-			LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
-		ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
-	ELSE() # Linux
-		find_package(ZED 1 QUIET)
-	ENDIF(WIN32)
+    find_package(ZED 2 QUIET)
    
    IF(ZED_FOUND)
       MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}")
@@ -330,10 +408,24 @@ IF(WITH_REALSENSE)
    ENDIF(RealSenseSlam_FOUND)
 ENDIF(WITH_REALSENSE)

+IF(WITH_REALSENSE2)
+    IF(WIN32)
+        FIND_PACKAGE(RealSense2 QUIET)
+	ELSE()
+        FIND_PACKAGE(realsense2 QUIET)
+    ENDIF()
+    IF(realsense2_FOUND)
+       MESSAGE(STATUS "Found RealSense2: ${realsense2_INCLUDE_DIRS}")
+    ENDIF(realsense2_FOUND)
+ENDIF(WITH_REALSENSE2)
+
 IF(WITH_OCTOMAP)
    FIND_PACKAGE(OCTOMAP QUIET)
    IF(OCTOMAP_FOUND)
-       MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
+       MESSAGE(STATUS "Found octomap ${OCTOMAP_VERSION}: ${OCTOMAP_INCLUDE_DIRS}")
+       IF(OCTOMAP_VERSION VERSION_LESS 1.8)
+          ADD_DEFINITIONS("-DOCTOMAP_PRE_18")
+       ENDIF(OCTOMAP_VERSION VERSION_LESS 1.8)
    ENDIF(OCTOMAP_FOUND)
 ENDIF(WITH_OCTOMAP)

@@ -344,7 +436,103 @@ IF(WITH_CPUTSDF)
    ENDIF(CPUTSDF_FOUND)
 ENDIF(WITH_CPUTSDF)

-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
+IF(WITH_OPENCHISEL)
+    find_package(open_chisel QUIET)
+    if(open_chisel_FOUND)
+        MESSAGE(STATUS "Found open_chisel: ${open_chisel_INCLUDE_DIRS}")
+    endif(open_chisel_FOUND)
+ENDIF(WITH_OPENCHISEL)
+
+IF(WITH_FOVIS)
+    FIND_PACKAGE(libfovis QUIET)
+    IF(libfovis_FOUND)
+       MESSAGE(STATUS "Found libfovis: ${libfovis_INCLUDE_DIRS}")
+    ENDIF(libfovis_FOUND)
+ENDIF(WITH_FOVIS)
+
+IF(WITH_VISO2)
+    FIND_PACKAGE(libviso2 QUIET)
+    IF(libviso2_FOUND)
+       MESSAGE(STATUS "Found libviso2: ${libviso2_INCLUDE_DIRS}")
+    ENDIF(libviso2_FOUND)
+ENDIF(WITH_VISO2)
+
+IF(WITH_DVO)
+    FIND_PACKAGE(dvo_core QUIET)
+    IF(dvo_core_FOUND)
+       MESSAGE(STATUS "Found dvo_core: ${dvo_core_INCLUDE_DIRS}")
+    ENDIF(dvo_core_FOUND)
+ENDIF(WITH_DVO)
+
+IF(WITH_OKVIS)
+    FIND_PACKAGE(okvis 1.1 QUIET)
+    IF(okvis_FOUND)
+       MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
+       find_package(brisk 2 REQUIRED)
+       MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
+       find_package(opengv REQUIRED)
+       MESSAGE(STATUS "Found opengv: ${OPENGV_INCLUDE_DIRS}")
+       find_package(Ceres 1.9.0 REQUIRED EXACT) # OKVIS requires this specific version
+       MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
+    ENDIF(okvis_FOUND)
+ENDIF(WITH_OKVIS)
+
+IF(WITH_MSCKF_VIO)
+    FIND_PACKAGE(msckf_vio QUIET)
+    IF(msckf_vio_FOUND)
+       MESSAGE(STATUS "Found msckf_vio: ${msckf_vio_INCLUDE_DIRS}")
+    ENDIF(msckf_vio_FOUND)
+ENDIF(WITH_MSCKF_VIO)
+
+IF(WITH_VINS)
+    FIND_PACKAGE(vins QUIET)
+    IF(vins_FOUND)
+       MESSAGE(STATUS "Found vins: ${vins_INCLUDE_DIRS}")
+       IF(okvis_FOUND)
+           MESSAGE(WARNING "VINS and OKVIS will be both linked to project, make sure VINS has been built with against same Ceres version than OKVIS to avoid some crashes.")
+       ENDIF(okvis_FOUND)
+    ENDIF(vins_FOUND)
+ENDIF(WITH_VINS)
+
+
+IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
+    FIND_PACKAGE(ORB_SLAM2 QUIET)
+    IF(ORB_SLAM2_FOUND)
+       MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
+       FIND_PACKAGE(Pangolin QUIET)
+       IF(NOT Pangolin_FOUND)
+          SET(ORB_SLAM2_FOUND FALSE)
+          MESSAGE(STATUS "Found ORB_SLAM2 but not Pangolin, disabling ORB_SLAM2.")
+       ELSE()
+          MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
+          SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
+          SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
+       ENDIF()
+    ENDIF(ORB_SLAM2_FOUND)
+ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
+
+IF(loam_velodyne_FOUND)
+   #LOAM requires c++14
+   IF(NOT MSVC)
+     include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+      IF(COMPILER_SUPPORTS_CXX14)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM (set \"-DWITH_LOAM=OFF\" to build without LOAM).")
+      ENDIF()
+   ENDIF()
+ELSEIF(G2O_FOUND OR 
+       GTSAM_FOUND OR 
+       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)
@@ -355,10 +543,10 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
      ELSEIF(COMPILER_SUPPORTS_CXX0X)
        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
      ELSE()
-		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+      ENDIF()
   ENDIF()
 ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)

 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -396,11 +584,24 @@ 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()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
+   IF(NOT G2O_CPP11)
+   SET(G2O_CPP_CONF "//")
+   ENDIF(NOT G2O_CPP11)
 ENDIF()
 IF(NOT GTSAM_FOUND)
   SET(GTSAM "//")
@@ -418,6 +619,12 @@ IF(NOT cvsba_FOUND)
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES})
 ENDIF()
+IF(NOT libpointmatcher_FOUND)
+   SET(POINTMATCHER "//")
+ENDIF(NOT libpointmatcher_FOUND)
+IF(NOT loam_velodyne_FOUND)
+   SET(LOAM "//")
+ENDIF(NOT loam_velodyne_FOUND)
 IF(NOT Freenect_FOUND)
   SET(FREENECT "//")
 ELSE()
@@ -428,6 +635,11 @@ IF(NOT freenect2_FOUND)
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
 ENDIF()
+IF(NOT KinectSDK2_FOUND)
+   SET(K4W2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES})
+ENDIF()
 IF(NOT OpenNI2_FOUND)
   SET(OPENNI2 "//")
 ELSE()
@@ -456,6 +668,11 @@ ENDIF()
 IF(NOT RealSenseSlam_FOUND)
   SET(REALSENSESLAM "//")
 ENDIF(NOT RealSenseSlam_FOUND)
+IF(NOT realsense2_FOUND)
+   SET(REALSENSE2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) 
+ENDIF()
 IF(NOT OCTOMAP_FOUND)
   SET(OCTOMAP "//")
 ELSE()
@@ -466,15 +683,65 @@ IF(NOT CPUTSDF_FOUND)
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) 
 ENDIF()
+IF(NOT open_chisel_FOUND)
+   SET(OPENCHISEL "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) 
+ENDIF()
+IF(NOT libfovis_FOUND)
+   SET(FOVIS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_LIBRARIES}) 
+ENDIF()
+IF(NOT libviso2_FOUND)
+   SET(VISO2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES}) 
+ENDIF()
+IF(NOT dvo_core_FOUND)
+   SET(DVO "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) 
+ENDIF()
+IF(NOT okvis_FOUND)
+   SET(OKVIS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) 
+ENDIF()
+IF(NOT msckf_vio_FOUND)
+   SET(MSCKF_VIO "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${msckf_vio_LIBRARIES}) 
+ENDIF()
+IF(NOT vins_FOUND)
+   SET(VINS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${vins_LIBRARIES}) 
+ENDIF()
+IF(NOT ORB_SLAM2_FOUND)
+   SET(ORB_SLAM2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) 
+ENDIF()
+IF(NOT WITH_ORB_OCTREE)
+   SET(ORB_OCTREE "//")
+ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
   SET(CONF_VTK_QT true)
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
 ELSE()
   SET(CONF_VTK_QT false)
 ENDIF()
-IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+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 "//")
-ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+ENDIF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)

 ADD_SUBDIRECTORY( utilite )
@@ -559,7 +826,9 @@ install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PR
 #######################
 # CPACK (Packaging)
 #######################
+IF(BUILD_AS_BUNDLE)
  INCLUDE(InstallRequiredSystemLibraries)
+ENDIF(BUILD_AS_BUNDLE)

 SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}")
 SET(CPACK_PACKAGE_VENDOR "${PROJECT_NAME} project")
@@ -594,7 +863,11 @@ IF(WIN32)
  ELSE() 
    SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES") 
  ENDIF()
+  IF(BUILD_AS_BUNDLE)
    SET(CPACK_GENERATOR "ZIP;NSIS")
+  ELSE()
+    SET(CPACK_GENERATOR "ZIP")
+  ENDIF()
  SET(CPACK_SOURCE_GENERATOR "ZIP")
  SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}")
  SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/src/${PROJECT_NAME}.ico")
@@ -649,27 +922,51 @@ IF(NOT WIN32)
    # see comment above for the BUILD_SHARED_LIBS option on Windows
    MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
 ENDIF(NOT WIN32)
-IF(APPLE)
+IF(APPLE OR WIN32)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
-ENDIF(APPLE)
+ENDIF(APPLE OR WIN32)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
+MESSAGE(STATUS "  FLANN_KDTREE_MEM_OPT = ${FLANN_KDTREE_MEM_OPT}")
+MESSAGE(STATUS "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")
+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)
  IF(OpenCV_VERSION_MAJOR EQUAL 2)
    IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
    ELSE()
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
    ENDIF()
  ELSE()
    IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      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)

+IF(Sqlite3_FOUND)
+MESSAGE(STATUS "  With external SQLite3     = YES (License: Public Domain)")
+ELSE()
+MESSAGE(STATUS "  With external SQLite3     = NO (sqlite3 not found, internal version is used for convenience)")
+ENDIF()
+
+IF(WITH_ORB_OCTREE)
+MESSAGE(STATUS "  With ORB OcTree           = YES (License: GPLv3)")
+ELSE()
+MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
+ENDIF()
+
+IF(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)
@@ -694,6 +991,14 @@ ELSE()
 MESSAGE(STATUS "  With Freenect2            = NO (libfreenect2 not found)")
 ENDIF()

+IF(KinectSDK2_FOUND)
+MESSAGE(STATUS "  With Kinect for Windows 2 = YES (License: Apache v2 and/or GPLv2)")
+ELSEIF(NOT WITH_K4W2)
+MESSAGE(STATUS "  With Kinect for Windows 2 = NO (WITH_K4W2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)")
+ENDIF()
+
 IF(DC1394_FOUND)
 MESSAGE(STATUS "  With dc1394               = YES (License: LGPL)")
 ELSEIF(NOT WITH_DC1394)
@@ -717,26 +1022,30 @@ MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
 ENDIF()

 IF(G2O_FOUND)
-MESSAGE(STATUS "  With g2o                  = YES (License: BSD)")
+MESSAGE(STATUS " *With g2o                  = YES (License: BSD)")
 ELSEIF(NOT WITH_G2O)
-MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
+MESSAGE(STATUS " *With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
-MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
+MESSAGE(STATUS " *With g2o                  = NO (g2o not found)")
 ENDIF()

 IF(GTSAM_FOUND)
-MESSAGE(STATUS "  With GTSAM                = YES (License: BSD)")
+MESSAGE(STATUS " *With GTSAM                = YES (License: BSD)")
 ELSEIF(NOT WITH_GTSAM)
-MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
+MESSAGE(STATUS " *With GTSAM                = NO (WITH_GTSAM=OFF)")
 ELSE()
-MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
+MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()

+IF(G2O_FOUND OR GTSAM_FOUND)
 IF(WITH_VERTIGO)
 MESSAGE(STATUS "  With VERTIGO              = YES (License: GPLv3)")
 ELSE()
 MESSAGE(STATUS "  With VERTIGO              = NO (WITH_VERTIGO=OFF)")
 ENDIF()
+ELSE()
+MESSAGE(STATUS "  With VERTIGO              = NO (GTSAM or g2o required)")
+ENDIF()

 IF(cvsba_FOUND)
 MESSAGE(STATUS "  With cvsba                = YES (License: GPLv2)")
@@ -746,6 +1055,22 @@ ELSE()
 MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()

+IF(libpointmatcher_FOUND)
+MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
+ELSEIF(NOT WITH_POINTMATCHER)
+MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
+ELSE()
+MESSAGE(STATUS " *With libpointmatcher      = NO (libpointmatcher not found)")
+ENDIF()
+
+IF(loam_velodyne_FOUND)
+MESSAGE(STATUS "  With loam_velodyne        = YES (License: BSD)")
+ELSEIF(NOT WITH_LOAM)
+MESSAGE(STATUS "  With loam_velodyne        = NO (WITH_LOAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With loam_velodyne        = NO (loam_velodyne not found)")
+ENDIF()
+
 IF(ZED_FOUND)
 IF(CUDA_FOUND)
 MESSAGE(STATUS "  With ZED                  = YES (With CUDA)")
@@ -773,6 +1098,14 @@ ELSE()
 MESSAGE(STATUS "  With RealSense            = NO (librealsense not found)")
 ENDIF()

+IF(realsense2_FOUND)
+MESSAGE(STATUS "  With RealSense2           = YES (License: Apache-2)")
+ELSEIF(NOT WITH_REALSENSE2)
+MESSAGE(STATUS "  With RealSense2           = NO (WITH_REALSENSE2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With RealSense2           = NO (librealsense2 not found)")
+ENDIF()
+
 IF(OCTOMAP_FOUND)
 MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
 ELSEIF(NOT WITH_OCTOMAP)
@@ -789,6 +1122,72 @@ ELSE()
 MESSAGE(STATUS "  With CPUTSDF              = NO (CPUTSDF not found)")
 ENDIF()

+IF(open_chisel_FOUND)
+MESSAGE(STATUS "  With OpenChisel           = YES (License: ???)")
+ELSEIF(NOT WITH_OPENCHISEL)
+MESSAGE(STATUS "  With OpenChisel           = NO (WITH_OPENCHISEL=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
+ENDIF()
+
+IF(libfovis_FOUND)
+MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
+ELSEIF(NOT WITH_FOVIS)
+MESSAGE(STATUS "  With libfovis             = NO (WITH_FOVIS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libfovis             = NO (libfovis not found)")
+ENDIF()
+
+IF(libviso2_FOUND)
+MESSAGE(STATUS "  With libviso2             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_VISO2)
+MESSAGE(STATUS "  With libviso2             = NO (WITH_VISO2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libviso2             = NO (libviso2 not found)")
+ENDIF()
+
+IF(dvo_core_FOUND)
+MESSAGE(STATUS "  With dvo_core             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_DVO)
+MESSAGE(STATUS "  With dvo_core             = NO (WITH_DVO=OFF)")
+ELSE()
+MESSAGE(STATUS "  With dvo_core             = NO (dvo_core not found)")
+ENDIF()
+
+IF(okvis_FOUND)
+MESSAGE(STATUS "  With okvis                = YES (License: BSD)")
+ELSEIF(NOT WITH_OKVIS)
+MESSAGE(STATUS "  With okvis                = NO (WITH_OKVIS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With okvis                = NO (okvis not found)")
+ENDIF()
+
+IF(msckf_vio_FOUND)
+MESSAGE(STATUS "  With msckf_vio            = YES (License: Penn Software License)")
+ELSEIF(NOT WITH_MSCKF_VIO)
+MESSAGE(STATUS "  With msckf_vio            = NO (WITH_MSCKF_VIO=OFF)")
+ELSE()
+MESSAGE(STATUS "  With msckf_vio            = NO (msckf_vio not found)")
+ENDIF()
+
+IF(vins_FOUND)
+MESSAGE(STATUS "  With VINS-Fusion          = YES (License: GPLv3)")
+ELSEIF(NOT WITH_VINS)
+MESSAGE(STATUS "  With VINS-Fusion          = NO (WITH_VINS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With VINS-Fusion          = NO (VINS-Fusion not found)")
+ENDIF()
+
+IF(ORB_SLAM2_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
+ELSEIF(NOT WITH_ORB_SLAM2)
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_ORB_SLAM2=OFF)")
+ELSEIF(G2O_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
+ELSE()
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)
@@ -807,3 +1206,5 @@ MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of th
       GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
       set -DWITH_TORO=ON")
 ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
+
+# vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :
@@ -1,8 +1,27 @@
-rtabmap [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap)
+rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pixel) 
 =======

+[![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
+
+[![Release][release-image]][releases]
+[![License][license-image]][license]
+Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
+
+[release-image]: https://img.shields.io/badge/release-0.18.0-green.svg?style=flat
+[releases]: https://github.com/introlab/rtabmap/releases
+
+[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
+[license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
+
 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>
@@ -1,4 +1,8 @@
 # - Config file for the RTABMap package
+# Components:
+#  core    (required)
+#  gui     (optional)
+#  utilite (required)
 # It defines the following variables
 #  RTABMap_INCLUDE_DIRS - include directories for RTABMap
 #  RTABMap_LIBRARIES    - libraries to link against
@@ -42,8 +46,18 @@ ENDIF()

 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})

+list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
+set(WITH_GUI ON)
+if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
+   if (${_index} EQUAL -1)
+      set(WITH_GUI OFF)
+   endif()
+endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+
+
 #gui lib (OFF if RTAB-Map is not built with Qt)
-if(@CONF_WITH_GUI@)
+if(@CONF_WITH_GUI@ AND ${WITH_GUI})
   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
   
@@ -59,13 +73,15 @@ if(@CONF_WITH_GUI@)
   ENDIF()
   
   set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
-endif(@CONF_WITH_GUI@)
+elseif(${WITH_GUI})
+   MESSAGE(ERROR "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
+endif()

 # Dependencies
-if(@CONF_VTK_QT@)
+if(@CONF_VTK_QT@ AND ${WITH_GUI})
   find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
-endif(@CONF_VTK_QT@)
-set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
+endif(@CONF_VTK_QT@ AND ${WITH_GUI})
+SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")

 #backward compatibilities
 set(RTABMAP_CORE ${RTABMap_CORE})
@@ -74,3 +90,7 @@ if(RTABMap_GUI)
   set(RTABMAP_GUI ${RTABMap_GUI})
   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
 endif(RTABMap_GUI)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
+mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)
@@ -40,20 +40,36 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@NONFREE@#define RTABMAP_NONFREE
@TORO@#define RTABMAP_TORO
@G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
@GTSAM@#define RTABMAP_GTSAM
@VERTIGO@#define RTABMAP_VERTIGO
@OPENCV3@#define RTABMAP_OPENCV3
@OPENNI2@#define RTABMAP_OPENNI2
@FREENECT@#define RTABMAP_FREENECT
@FREENECT2@#define RTABMAP_FREENECT2
+@K4W2@#define RTABMAP_K4W2
@CVSBA@#define RTABMAP_CVSBA
+@POINTMATCHER@#define RTABMAP_POINTMATCHER
+@LOAM@#define RTABMAP_LOAM
@DC1394@#define RTABMAP_DC1394
@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
@ZED@#define RTABMAP_ZED
@REALSENSE@#define RTABMAP_REALSENSE
@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
@OCTOMAP@#define RTABMAP_OCTOMAP
@CPUTSDF@#define RTABMAP_CPUTSDF
+@OPENCHISEL@#define RTABMAP_OPENCHISEL
+@FOVIS@#define RTABMAP_FOVIS
+@VISO2@#define RTABMAP_VISO2
+@DVO@#define RTABMAP_DVO
+@OKVIS@#define RTABMAP_OKVIS
+@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
+@VINS@#define RTABMAP_VINS
+@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
+@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
+@MADGWICK@#define RTABMAP_MADGWICK
+

 #endif /* VERSION_H_ */

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
        package="com.introlab.rtabmap"
-        android:versionCode="50"
+        android:versionCode="72"
        android:versionName="@RTABMAP_VERSION@">

    <uses-permission android:name="android.permission.CAMERA" />
@@ -12,6 +12,9 @@
    <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
    <uses-permission android:name="android.permission.INTERNET" />
    <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
+    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-permission android:name="android.permission.ACCESS_WIFI_STATE" />
+    <uses-feature android:name="android.hardware.location.gps" />
    <uses-feature android:glEsVersion="0x00020000" />

    <!-- This is the platform API where NativeActivity was introduced. -->
@@ -31,7 +34,8 @@
                android:label="@string/app_name"
                android:launchMode="singleTask"
 		android:screenOrientation="fullSensor"
-		android:configChanges="orientation|screenSize|keyboardHidden">
+		android:configChanges="orientation|screenSize|keyboardHidden"
+		android:theme="@style/ThemeApp">
            <!-- Tell NativeActivity the name of our .so -->
            <meta-data android:name="android.app.lib_name"
                    android:value="NativeRTABMap" />
@@ -1,7 +1,7 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
 Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
-Copyright 2016<br>
+Copyright 2016-2018<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
 <b>http://introlab.github.io/rtabmap</b><br><br>

@@ -46,7 +46,7 @@ const int scanDownsampling = 1;
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(point_cloud->num_points>0)
+	if(app->isRunning() && point_cloud->num_points>0)
 	{
 		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 	}
@@ -55,6 +55,8 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
+	if(app->isRunning())
+	{
 		cv::Mat tangoImage;
 		if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 		{
@@ -82,6 +84,7 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
 			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
 		}
 	}
+}

 void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 {
@@ -107,7 +110,7 @@ const float CameraTango::bilateralFilteringSigmaR = 0.075f;
 CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
 		Camera(0),
 		tango_config_(0),
-		firstFrame_(true),
+		previousStamp_(0.0),
 		stampEpochOffset_(0.0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),
@@ -116,7 +119,8 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan,
 		cloudStamp_(0),
 		tangoColorType_(0),
 		tangoColorStamp_(0),
-		colorCameraToDisplayRotation_(ROTATION_0)
+		colorCameraToDisplayRotation_(ROTATION_0),
+		originUpdate_(false)
 {
 	UASSERT(decimation >= 1);
 }
@@ -189,7 +193,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();

-	TangoSupport_initializeLibrary();
+	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);

 	// Connect to Tango
 	LOGI("NativeRTABMap: Setup tango config");
@@ -425,11 +429,23 @@ void CameraTango::close()
 	{
 		TangoConfig_free(tango_config_);
 		tango_config_ = nullptr;
+		LOGI("TangoService_disconnect()");
 		TangoService_disconnect();
+		LOGI("TangoService_disconnect() done.");
 	}
-	firstFrame_ = true;
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	originOffset_ = Transform();
+	originUpdate_ = false;
+}
+
+void CameraTango::resetOrigin()
+{
+	originUpdate_ = true;
 }

 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -490,10 +506,23 @@ static rtabmap::Transform opticalRotation(
 								0.0f,  0.0f, -1.0f, 0.0f);
 void CameraTango::poseReceived(const Transform & pose)
 {
-	if(!pose.isNull() && pose.getNormSquared() < 100000)
+	if(!pose.isNull())
 	{
 		// send pose of the camera (without optical rotation), not the device
-		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotation));
+		Transform p = pose*deviceTColorCamera_*opticalRotation;
+		if(originUpdate_)
+		{
+			originOffset_ = p.translation().inverse();
+			originUpdate_ = false;
+		}
+		if(!originOffset_.isNull())
+		{
+			this->post(new PoseEvent(originOffset_*p));
+		}
+		else
+		{
+			this->post(new PoseEvent(p));
+		}
 	}
 }

@@ -512,6 +541,16 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }

+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);
@@ -698,6 +737,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
 			std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
 			int oi=0;
+			int closePoints = 0;
+			float closeROI[4];
+			closeROI[0] = depth.cols/4;
+			closeROI[1] = 3*(depth.cols/4);
+			closeROI[2] = depth.rows/4;
+			closeROI[3] = 3*(depth.rows/4);
+			unsigned short minDepthValue=10000;
 			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
 				float * p = cloud.ptr<float>(0,i);
@@ -716,6 +762,17 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
 				unsigned short depth_value(pt.z * 1000.0f);

+				if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
+				   pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
+				   depth_value < 600)
+				{
+					++closePoints;
+					if(depth_value < minDepthValue)
+					{
+						minDepthValue = depth_value;
+					}
+				}
+
 				bool pixelSet = false;
 				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
 				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
@@ -745,6 +802,11 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				}
 			}

+			if(closePoints > 100)
+			{
+				this->post(new CameraTangoEvent(0, "TooClose", ""));
+			}
+
 			if(oi)
 			{
 				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
@@ -762,6 +824,12 @@ SensorData CameraTango::captureImage(CameraInfo * info)

 			Transform poseDevice = getPoseAtTimestamp(rgbStamp);

+			// adjust origin
+			if(!originOffset_.isNull())
+			{
+				poseDevice = originOffset_ * poseDevice;
+			}
+
 			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
 			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
 			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
@@ -823,19 +891,33 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			{
 				//UTimer t;
 				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
-				data.setDepthOrRightRaw(depth);
 				//LOGD("Bilateral filtering, time=%fs", t.ticks());
 			}

 			if(rawScanPublished_)
 			{
-				data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
 			else
 			{
 				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
 			data.setGroundTruth(odom);
+
+			if(lastKnownGPS_.stamp() > 0.0 && rgbStamp-lastKnownGPS_.stamp()<1.0)
+			{
+				data.setGPS(lastKnownGPS_);
+			}
+			else if(lastKnownGPS_.stamp()>0.0)
+			{
+				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
+			}
+
+			if(lastEnvSensors_.size())
+			{
+				data.setEnvSensors(lastEnvSensors_);
+				lastEnvSensors_.clear();
+			}
 		}
 		else
 		{
@@ -865,15 +947,33 @@ void CameraTango::mainLoop()
 		{
 			rtabmap::Transform pose = data.groundTruth();
 			data.setGroundTruth(Transform());
+
 			// convert stamp to epoch
-			if(firstFrame_)
+			bool firstFrame = previousPose_.isNull();
+			if(firstFrame)
 			{
 				stampEpochOffset_ = UTimer::now()-data.stamp();
 			}
 			data.setStamp(stampEpochOffset_ + data.stamp());
-			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.000001);
-			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.000001, firstFrame_?9999:0.000001));
-			firstFrame_ = false;
+			OdometryInfo info;
+			if(!firstFrame)
+			{
+				info.interval = data.stamp()-previousStamp_;
+				info.transform = previousPose_.inverse() * pose;
+			}
+			// linear cov = 0.0001
+			info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
+			if(!firstFrame)
+			{
+				// angular cov = 0.000001
+				info.reg.covariance.at<double>(3,3) *= 0.01;
+				info.reg.covariance.at<double>(4,4) *= 0.01;
+				info.reg.covariance.at<double>(5,5) *= 0.01;
+			}
+			LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
+			this->post(new OdometryEvent(data, pose, info));
+			previousPose_ = pose;
+			previousStamp_ = data.stamp();
 		}
 		else if(!this->isKilled())
 		{
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define CAMERATANGO_H_

 #include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
 #include <rtabmap/utilite/USemaphore.h>
 #include <rtabmap/utilite/UEventsSender.h>
@@ -80,6 +81,7 @@ public:

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	void close(); // close Tango connection
+	void resetOrigin();
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	const CameraModel & getCameraModel() const {return model_;}
@@ -89,6 +91,8 @@ public:
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	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);
@@ -106,7 +110,8 @@ private:

 private:
 	void * tango_config_;
-	bool firstFrame_;
+	Transform previousPose_;
+	double previousStamp_;
 	UTimer cameraStartedTime_;
 	double stampEpochOffset_;
 	bool colorCamera_;
@@ -125,6 +130,10 @@ private:
 	TangoSupportRotation colorCameraToDisplayRotation_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
+	GPS lastKnownGPS_;
+	EnvSensors lastEnvSensors_;
+	Transform originOffset_;
+	bool originUpdate_;
 };

 } /* namespace rtabmap */
@@ -70,7 +70,7 @@ public:
 			increment();
 		}

-		return !isCanceled();
+		return ProgressState::callback(msg);
 	}
 	virtual ~ProgressionStatus(){}

@@ -56,7 +56,7 @@ class RTABMapApp : public UEventsHandler {

  void setScreenRotation(int displayRotation, int cameraRotation);

-  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());

  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);

@@ -119,8 +119,12 @@ class RTABMapApp : public UEventsHandler {
  void setOdomCloudShown(bool shown);
  void setMeshRendering(bool enabled, bool withTexture);
  void setPointSize(float value);
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float value);
  void setLighting(bool enabled);
  void setBackfaceCulling(bool enabled);
+  void setWireframe(bool enabled);
  void setLocalizationMode(bool enabled);
  void setTrajectoryMode(bool enabled);
  void setGraphOptimization(bool enabled);
@@ -143,19 +147,18 @@ class RTABMapApp : public UEventsHandler {
  void setRenderingTextureDecimation(int value);
  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);
-  cv::Mat mergeTextures(pcl::TextureMesh & mesh,
-		  int textureSize,
-		  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels) const;
  void cancelProcessing();
  bool exportMesh(
-		  const std::string & filePath,
 		  float cloudVoxelSize,
 		  bool regenerateCloud,
 		  bool meshing,
 		  int textureSize,
+		  int textureCount,
 		  int normalK,
 		  bool optimized,
 		  float optimizedVoxelSize,
@@ -163,11 +166,12 @@ class RTABMapApp : public UEventsHandler {
 		  int optimizedMaxPolygons,
 		  float optimizedColorRadius,
 		  bool optimizedCleanWhitePolygons,
-		  bool optimizedColorWhitePolygons,
+		  int optimizedMinClusterSize,
 		  float optimizedMaxTextureDistance,
 		  int optimizedMinTextureClusterSize,
 		  bool blockRendering);
  bool postExportation(bool visualize);
+  bool writeExportedMesh(const std::string & directory, const std::string & name);
  int postProcessing(int approach);

 protected:
@@ -217,6 +221,7 @@ class RTABMapApp : public UEventsHandler {
  bool filterPolygonsOnNextRender_;
  int gainCompensationOnNextRender_;
  bool bilateralFilteringOnNextRender_;
+  bool takeScreenshotOnNextRender_;
  bool cameraJustInitialized_;
  int meshDecimation_;
  int totalPoints_;
@@ -224,31 +229,36 @@ class RTABMapApp : public UEventsHandler {
  int lastDrawnCloudsCount_;
  float renderingTime_;
  double lastPostRenderEventTime_;
-  long processMemoryUsedBytes;
-  long processGPUMemoryUsedBytes;
+  double lastPoseEventTime_;
  std::map<std::string, float> bufferedStatsData_;

  bool visualizingMesh_;
  bool exportedMeshUpdated_;
-  pcl::TextureMesh::Ptr exportedMesh_;
-  cv::Mat exportedTexture_;
+  pcl::TextureMesh::Ptr optMesh_;
+  cv::Mat optTexture_;
+  int optRefId_;
+  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic

  // main_scene_ includes all drawable object for visualizing Tango device's
  // movement and point cloud.
  Scene main_scene_;

 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> visLocalizationEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;

 	rtabmap::Transform mapToOdom_;

 	boost::mutex rtabmapMutex_;
+	boost::mutex visLocalizationMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;

+	USemaphore screenshotReady_;
+
 	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;

@@ -71,6 +71,17 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	return app.openDatabase(databasePathC, databaseInMemory, optimize);
 }

+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
+    JNIEnv* env, jobject, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+{
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	std::string databaseSourceC;
+	GetJStringContent(env,databaseSource,databaseSourceC);
+	return app.openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+}
+
 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
 		JNIEnv* env, jobject, jobject iBinder) {
@@ -157,6 +168,24 @@ Java_com_introlab_rtabmap_RTABMapLib_setPointSize(
 	return app.setPointSize(value);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setFOV(
+		JNIEnv*, jobject, float fov)
+{
+	return app.setFOV(fov);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOrthoCropFactor(
+		JNIEnv*, jobject, float value)
+{
+	return app.setOrthoCropFactor(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridRotation(
+		JNIEnv*, jobject, float value)
+{
+	return app.setGridRotation(value);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLighting(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -169,6 +198,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setBackfaceCulling(
 	return app.setBackfaceCulling(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setWireframe(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setWireframe(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -304,6 +339,33 @@ Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
 	return app.setMappingParameter(keyC, valueC);
 }

+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGPS(
+		JNIEnv*, jobject,
+		double stamp,
+		double longitude,
+		double latitude,
+		double altitude,
+		double accuracy,
+		double bearing)
+{
+	return app.setGPS(rtabmap::GPS(stamp,
+			longitude,
+			latitude,
+			altitude,
+			accuracy,
+			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)
@@ -330,11 +392,11 @@ Java_com_introlab_rtabmap_RTABMapLib_cancelProcessing(
 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		JNIEnv* env, jobject,
-		jstring filePath,
 		float cloudVoxelSize,
 		bool regenerateCloud,
 		bool meshing,
 		int textureSize,
+		int textureCount,
 		int normalK,
 		bool optimized,
 		float optimizedVoxelSize,
@@ -342,19 +404,17 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		int optimizedMaxPolygons,
 		float optimizedColorRadius,
 		bool optimizedCleanWhitePolygons,
-		bool optimizedColorWhitePolygons,
+		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
 		bool blockRendering)
 {
-	std::string filePathC;
-	GetJStringContent(env,filePath,filePathC);
 	return app.exportMesh(
-			filePathC,
 			cloudVoxelSize,
 			regenerateCloud,
 			meshing,
 			textureSize,
+			textureCount,
 			normalK,
 			optimized,
 			optimizedVoxelSize,
@@ -362,7 +422,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 			optimizedMaxPolygons,
 			optimizedColorRadius,
 			optimizedCleanWhitePolygons,
-			optimizedColorWhitePolygons,
+			optimizedMinClusterSize,
 			optimizedMaxTextureDistance,
 			optimizedMinTextureClusterSize,
 			blockRendering);
@@ -375,6 +435,18 @@ Java_com_introlab_rtabmap_RTABMapLib_postExportation(
 	return app.postExportation(visualize);
 }

+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_writeExportedMesh(
+		JNIEnv* env, jobject, jstring directory, jstring name)
+{
+	std::string directoryC;
+	GetJStringContent(env,directory,directoryC);
+	std::string nameC;
+	GetJStringContent(env,name,nameC);
+	return app.writeExportedMesh(directoryC, nameC);
+}
+
+
 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
 		JNIEnv* env, jobject, int approach)
@@ -318,7 +318,8 @@ PointCloudDrawable::PointCloudDrawable(
 }

 PointCloudDrawable::PointCloudDrawable(
-		const Mesh & mesh) :
+		const Mesh & mesh,
+		bool createWireframe) :
 				vertex_buffers_(0),
 				textures_(0),
 				nPoints_(0),
@@ -330,7 +331,7 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(1.0f),
 				gainB_(1.0f)
 {
-	updateMesh(mesh);
+	updateMesh(mesh, createWireframe);
 }

 PointCloudDrawable::~PointCloudDrawable()
@@ -351,22 +352,33 @@ PointCloudDrawable::~PointCloudDrawable()
 	}
 }

-void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes)
+void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
 {
 	LOGD("Update polygons");
 	polygons_.clear();
+	polygonLines_.clear();
+	polygonsLowRes_.clear();
+	polygonLinesLowRes_.clear();
 	if(polygons.size() && organizedToDenseIndices_.size())
 	{
 		unsigned int polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
 		polygons_.resize(polygons.size() * polygonSize);
+		if(createWireframe)
+			polygonLines_.resize(polygons_.size()*2);
 		int oi = 0;
+		int li = 0;
 		for(unsigned int i=0; i<polygons.size(); ++i)
 		{
 			UASSERT(polygons[i].vertices.size() == polygonSize);
 			for(unsigned int j=0; j<polygonSize; ++j)
 			{
 				polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+				if(createWireframe)
+				{
+					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
+				}
 			}
 		}

@@ -375,13 +387,21 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 			unsigned int polygonSize = polygonsLowRes[0].vertices.size();
 			UASSERT(polygonSize == 3);
 			polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
+			if(createWireframe)
+				polygonLinesLowRes_.resize(polygonsLowRes_.size()*2);
 			int oi = 0;
+			int li = 0;
 			for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
 			{
 				UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
 				for(unsigned int j=0; j<polygonSize; ++j)
 				{
 					polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+					if(createWireframe)
+					{
+						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
+					}
 				}
 			}
 		}
@@ -505,7 +525,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	nPoints_ = totalPoints;
 }

-void PointCloudDrawable::updateMesh(const Mesh & mesh)
+void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;
@@ -797,7 +817,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)

 	if(polygons_.size() != polygons.size())
 	{
-		updatePolygons(polygons, polygonsLowRes);
+		updatePolygons(polygons, polygonsLowRes, createWireframe);
 	}

 	if(!pose_.isNull())
@@ -857,7 +877,8 @@ void PointCloudDrawable::Render(
 		int screenHeight,
 		float nearClipPlane,
 	    float farClipPlane,
-		bool packDepthToColorChannel) const
+		bool packDepthToColorChannel,
+		bool wireFrame) const
 {
 	if(vertex_buffers_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
@@ -1021,10 +1042,18 @@ void PointCloudDrawable::Render(
 		{
 			if(distanceToCameraSqr<16.0f || polygonsLowRes_.empty())
 			{
+				wireFrame = wireFrame && polygonLines_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+				else
 					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
 			}
 			else
 			{
+				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+				else
 					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
 			}
 		}
@@ -1032,10 +1061,18 @@ void PointCloudDrawable::Render(
 		{
 			if(distanceToCameraSqr<50.0f || polygonsLowRes_.empty())
 			{
+				wireFrame = wireFrame && polygonLines_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+				else
 					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
 			}
 			else
 			{
+				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+				else
 					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
 			}
 		}
@@ -55,12 +55,13 @@ private:
 		  float gainG = 1.0f,
 		  float gainB = 1.0f);
  PointCloudDrawable(
-    	  const Mesh & mesh);
+    	  const Mesh & mesh,
+		  bool createWireframe = false);
  virtual ~PointCloudDrawable();

-  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>());
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
-  void updateMesh(const Mesh & mesh);
+  void updateMesh(const Mesh & mesh, bool createWireframe = false);
  void setPose(const rtabmap::Transform & pose);
  void setVisible(bool visible) {visible_=visible;}
  void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
@@ -69,6 +70,7 @@ private:
  bool isVisible() const {return visible_;}
  bool hasMesh() const {return polygons_.size()!=0;}
  bool hasTexture() const {return textures_ != 0;}
+  float getMinHeight() const {return minHeight_;}
  const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
  const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
  const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
@@ -93,7 +95,8 @@ private:
 		  int screenHeight = 0,    // nonnull if depthTexture>0
 		  float nearClipPlane = 0, // nonnull if depthTexture>0
 		  float farClipPlane = 0,  // nonnull if depthTexture>0
-		  bool packDepthToColorChannel = false) const;
+		  bool packDepthToColorChannel = false,
+		  bool wireFrame = false) const;

 private:
  template<class PointT>
@@ -114,6 +117,8 @@ private:
  GLuint textures_;
  std::vector<GLuint> polygons_;
  std::vector<GLuint> polygonsLowRes_;
+  std::vector<GLuint> polygonLines_;
+  std::vector<GLuint> polygonLinesLowRes_;
  std::vector<GLuint> verticesLowRes_;
  std::vector<GLuint> verticesLowLowRes_;
  int nPoints_;
@@ -122,6 +127,7 @@ private:
  bool visible_;
  bool hasNormals_;
  std::vector<unsigned int> organizedToDenseIndices_;
+  float minHeight_; // odom frame

  float gainR_;
  float gainG_;
@@ -16,12 +16,16 @@

 #include <tango-gl/conversions.h>
 #include <tango-gl/gesture_camera.h>
+#include <tango-gl/util.h>

 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/common.h>

 #include <glm/gtx/transform.hpp>

@@ -32,7 +36,7 @@
 // add an offset in z to our origin. We'll set this offset to 1.3 meters based
 // on the average height of a human standing with a Tango device. This allows us
 // to place a grid roughly on the ground for most users.
-const glm::vec3 kHeightOffset = glm::vec3(0.0f, 1.3f, 0.0f);
+const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);

 // Color of the motion tracking trajectory.
 const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
@@ -86,6 +90,7 @@ Scene::Scene() :
 		boundingBoxRendering_(false),
 		lighting_(false),
 		backfaceCulling_(true),
+		wireFrame_(false),
 		r_(0.0f),
 		g_(0.0f),
 		b_(0.0f),
@@ -97,7 +102,7 @@ Scene::Scene() :
 {
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
-	      tango_gl::GestureCamera::kFirstPerson);
+	      tango_gl::GestureCamera::kThirdPersonFollow);
 }

 Scene::~Scene() {
@@ -129,7 +134,7 @@ void Scene::InitGLContent()
 	trace_->ClearVertexArray();
 	trace_->SetColor(kTraceColor);
 	grid_->SetColor(kGridColor);
-	grid_->SetPosition(-kHeightOffset);
+	grid_->SetPosition(kHeightOffset);
 	box_->SetShader();
 	box_->SetColor(1,0,0);

@@ -183,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();
@@ -193,6 +202,11 @@ void Scene::clear()
 		graph_ = 0;
 	}
 	pointClouds_.clear();
+	markers_.clear();
+	if(grid_)
+	{
+		grid_->SetPosition(kHeightOffset);
+	}
 }

 //Should only be called in OpenGL thread!
@@ -201,7 +215,7 @@ void Scene::SetupViewPort(int w, int h) {
 		LOGE("Setup graphic height not valid");
 	}
 	UASSERT(gesture_camera_ != 0);
-	gesture_camera_->SetAspectRatio(static_cast<float>(w) / static_cast<float>(h));
+	gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
 	glViewport(0, 0, w, h);
 	if(screenWidth_ != w || fboId_ == 0)
 	{
@@ -426,7 +440,7 @@ int Scene::Render() {

 	UTimer timer;

-	bool onlineBlending = blending_ && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
 	if(onlineBlending && fboId_)
 	{
 		// set the rendering destination to FBO
@@ -533,7 +547,7 @@ int Scene::Render() {
 				cloud->getPose().z() - openglCamera.z());
 		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];

-		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane());
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}

 	if(onlineBlending)
@@ -542,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();
 }

@@ -556,6 +576,24 @@ void Scene::SetCameraPose(const rtabmap::Transform & pose)
 	*currentPose_ = pose;
 }

+void Scene::setFOV(float angle)
+{
+	gesture_camera_->SetFieldOfView(angle);
+}
+void Scene::setOrthoCropFactor(float value)
+{
+	gesture_camera_->SetOrthoCropFactor(value);
+}
+void Scene::setGridRotation(float angleDeg)
+{
+	float angleRad = angleDeg * DEGREE_2_RADIANS;
+	if(grid_)
+	{
+		glm::quat rot = glm::rotate(glm::quat(1,0,0,0), angleRad, glm::vec3(0, 1, 0));
+		grid_->SetRotation(rot);
+	}
+}
+
 rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 {
 	if(fov)
@@ -563,7 +601,6 @@ rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 		*fov = gesture_camera_->getFOV();
 	}
 	return glmToTransform(gesture_camera_->GetTransformationMatrix());
-
 }

 void Scene::OnTouchEvent(int touch_count,
@@ -622,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,
@@ -645,7 +729,8 @@ void Scene::addCloud(
 void Scene::addMesh(
 		int id,
 		const Mesh & mesh,
-		const rtabmap::Transform & pose)
+		const rtabmap::Transform & pose,
+		bool createWireframe)
 {
 	LOGI("add mesh %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
@@ -656,9 +741,61 @@ void Scene::addMesh(
 	}

 	//create
-	PointCloudDrawable * drawable = new PointCloudDrawable(mesh);
+	PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
+
+	if(!mesh.pose.isNull() && mesh.cloud->size() && (!mesh.cloud->isOrganized() || mesh.indices->size()))
+	{
+		UTimer time;
+		float height = 0.0f;
+		Eigen::Affine3f affinePose = mesh.pose.toEigen3f();
+		if(mesh.polygons.size())
+		{
+			for(unsigned int i=0; i<mesh.polygons.size(); ++i)
+			{
+				for(unsigned int j=0; j<mesh.polygons[i].vertices.size(); ++j)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.polygons[i].vertices[j]), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+		else
+		{
+			if(mesh.cloud->isOrganized())
+			{
+				for(unsigned int i=0; i<mesh.indices->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.indices->at(i)), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+			else
+			{
+				for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(i), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+
+		if(grid_->GetPosition().y == kHeightOffset.y || grid_->GetPosition().y > height)
+		{
+			grid_->SetPosition(glm::vec3(0,height,0));
+		}
+		LOGD("compute min height %f s", time.ticks());
+	}
 }


@@ -57,6 +57,8 @@ class Scene {

  // Setup GL view port.
  void SetupViewPort(int w, int h);
+  int getViewPortWidth() const {return screenWidth_;}
+  int getViewPortHeight() const {return screenHeight_;}

  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}

@@ -100,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,
@@ -108,7 +116,8 @@ class Scene {
  void addMesh(
  		int id,
  		const Mesh & mesh,
-  		const rtabmap::Transform & pose);
+  		const rtabmap::Transform & pose,
+		bool createWireframe = false);

  void setCloudPose(int id, const rtabmap::Transform & pose);
  void setCloudVisible(int id, bool visible);
@@ -124,8 +133,12 @@ class Scene {
  void setMapRendering(bool enabled) {mapRendering_ = enabled;}
  void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
  void setPointSize(float size) {pointSize_ = size;}
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float angleDeg);
  void setLighting(bool enabled) {lighting_ = enabled;}
  void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
+  void setWireframe(bool enabled) {wireFrame_ = enabled;}
  void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
  void setGridColor(float r, float g, float b);

@@ -160,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_;
@@ -177,6 +192,7 @@ class Scene {
  bool boundingBoxRendering_;
  bool lighting_;
  bool backfaceCulling_;
+  bool wireFrame_;
  float r_;
  float g_;
  float b_;
@@ -22,8 +22,13 @@ namespace tango_gl {
 Camera::Camera() {
  field_of_view_ = 45.0f * DEGREE_2_RADIANS;
  aspect_ratio_ = 4.0f / 3.0f;
+  width_ = 800.0f;
+  height_ = 600.0f;
  near_clip_plane_ = 0.2f;
  far_clip_plane_ = 1000.0f;
+  ortho_ = false;
+  orthoScale_ = 2.0f;
+  orthoCropFactor_ = -1.0f;
 }

 glm::mat4 Camera::GetViewMatrix() {
@@ -31,12 +36,17 @@ glm::mat4 Camera::GetViewMatrix() {
 }

 glm::mat4 Camera::GetProjectionMatrix() {
-  return glm::perspective(field_of_view_, aspect_ratio_,
-                          near_clip_plane_, far_clip_plane_);
+	if(ortho_)
+	{
+		return glm::ortho(-orthoScale_*aspect_ratio_, orthoScale_*aspect_ratio_, -orthoScale_, orthoScale_, orthoScale_ + orthoCropFactor_, far_clip_plane_);
+	}
+  return glm::perspective(field_of_view_, aspect_ratio_, near_clip_plane_, far_clip_plane_);
 }

-void Camera::SetAspectRatio(float aspect_ratio) {
-  aspect_ratio_ = aspect_ratio;
+void Camera::SetWindowSize(float width, float height) {
+	width_ = width;
+	height_ = height;
+  aspect_ratio_ = width/height;
 }

 void Camera::SetFieldOfView(float fov) {
@@ -63,11 +63,8 @@ GestureCamera::~GestureCamera() { delete cam_parent_transform_; }

 void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 		float y0, float x1, float y1) {
-  if (camera_type_ == kFirstPerson) {
-    return;
-  }

-  if (touch_count == 1) {
+	if (camera_type_!=kFirstPerson && touch_count == 1) {
 		switch (event) {
 		case kTouch0Down: {
 			cam_start_angle_ = cam_cur_angle_;
@@ -77,12 +74,17 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 			break;
 		}
 		case kTouchMove: {
+			glm::vec2 offset;
+
 			float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
 			float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;

+			if(camera_type_!=kTopOrtho)
 				cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
 			cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
+
 			StartCameraToCurrentTransform();
+
 			break;
 		}
 		default: { break; }
@@ -95,6 +97,7 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 			float abs_y = y0 - y1;
 			start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
 			cam_start_dist_ = GetPosition().z;
+			cam_start_fov_ = this->getFOV();

 			// center touch
 			touch0_start_position_.x = (x0+x1)/2.0f;
@@ -106,9 +109,21 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 			float abs_y = y0 - y1;
 			float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);

+			if(camera_type_ == kFirstPerson)
+			{
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
+			}
+			else
+			{
 				cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
 								kCamViewMinDist, kCamViewMaxDist);

+				this->SetOrthoMode(camera_type_ == kTopOrtho);
+				if(camera_type_ == kTopOrtho)
+				{
+					this->SetOrthoScale(cam_cur_dist_);
+				}
+
 				glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
 				glm::vec2 offset;
 				offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
@@ -118,7 +133,7 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 				StartCameraToCurrentTransform();

 				anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
-
+			}
 			break;
 		}
 		default: { break; }
@@ -164,6 +179,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
  camera_type_ = camera_index;
  switch (camera_index) {
    case kFirstPerson:
+    	SetOrthoMode(false);
      SetFieldOfView(kLowFov);
      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
@@ -177,6 +193,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
      break;
    case kThirdPerson:
    case kThirdPersonFollow:
+    	SetOrthoMode(false);
    	SetFieldOfView(kHighFov);
      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
@@ -188,9 +205,23 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
      StartCameraToCurrentTransform();
      break;
    case kTopDown:
-      SetFieldOfView(kHighFov);
      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      SetOrthoMode(false);
+      SetFieldOfView(kHighFov);
+      cam_cur_dist_ = kTopDownCameraDist;
+      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+      cam_cur_angle_.x = -M_PI / 2.0f;
+      cam_cur_angle_.y = 0.0f;
+      cam_cur_target_rot_ = glm::quat(1,0,0,0);
+      StartCameraToCurrentTransform();
+      break;
+    case kTopOrtho:
+	  SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+	  SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	  SetOrthoMode(true);
+	  SetOrthoScale(kTopDownCameraDist);
+	  SetOrthoCropFactor(-1.0f);
 	  cam_cur_dist_ = kTopDownCameraDist;
 	  anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
 	  cam_cur_angle_.x = -M_PI / 2.0f;
@@ -27,8 +27,11 @@ class Camera : public Transform {
  Camera& operator=(const Camera&) = delete;
  ~Camera();

-  void SetAspectRatio(const float aspect_ratio);
+  void SetWindowSize(const float width, const float height);
  void SetFieldOfView(const float fov);
+  void SetOrthoMode(bool enabled) {ortho_ = enabled;}
+  void SetOrthoScale(float scale) {orthoScale_ = scale;}
+  void SetOrthoCropFactor(float value) {orthoCropFactor_ = value;}
  void SetNearFarClipPlanes(const float near, const float far);

  glm::mat4 GetViewMatrix();
@@ -55,7 +58,12 @@ class Camera : public Transform {
 protected:
  float field_of_view_;
  float aspect_ratio_;
+  float width_;
+  float height_;
  float near_clip_plane_, far_clip_plane_;
+  bool ortho_;
+  float orthoScale_;
+  float orthoCropFactor_;
 };
 }  // namespace tango_gl
 #endif  // TANGO_GL_CAMERA_H_
@@ -29,7 +29,8 @@ class GestureCamera : public Camera {
    kFirstPerson = 0,
    kThirdPersonFollow = 1,
    kTopDown = 2,
-    kThirdPerson = 3
+	kTopOrtho = 3,
+    kThirdPerson = 4
  };

  enum TouchEvent {
@@ -80,6 +81,7 @@ class GestureCamera : public Camera {
  glm::quat cam_cur_target_rot_;

  float cam_start_dist_;
+  float cam_start_fov_;
  float cam_cur_dist_;
  glm::vec3 anchor_offset_;

@@ -44,8 +44,8 @@ class LogHandler : public UEventsHandler
 public:
 	LogHandler()
 	{
-		ULogger::setLevel(ULogger::kWarning);
-		ULogger::setEventLevel(ULogger::kWarning);
+		ULogger::setLevel(ULogger::kDebug);
+		ULogger::setEventLevel(ULogger::kDebug);
 		ULogger::setPrintThreadId(true);

 		registerToEventsManager();
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<layer-list xmlns:android="http://schemas.android.com/apk/res/android" >
+    <item>
+        <shape>
+            <gradient
+                android:endColor="#00ffffff"
+                android:startColor="#ff686868"
+                android:useLevel="false" />
+        </shape>
+    </item>
+
+</layer-list>
@@ -23,6 +23,24 @@
        android:layout_height="fill_parent"
        android:layout_gravity="top"/>
    
+    <RelativeLayout 
+        android:layout_width="wrap_content"
+        android:layout_height="match_parent"
+        android:fitsSystemWindows="true">
+    
+    <ToggleButton
+        android:id="@+id/wireframe_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/light_button"
+        android:layout_alignLeft="@+id/light_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="5dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/wireframe"
+        android:textOn="@string/wireframe" />
+    
    <ToggleButton
        android:id="@+id/light_button"
        android:layout_width="100dp"
@@ -30,7 +48,7 @@
        android:layout_above="@+id/backface_button"
        android:layout_alignLeft="@+id/backface_button"
        android:layout_alignParentRight="true"
-        android:layout_marginBottom="15dp"
+        android:layout_marginBottom="5dp"
        android:layout_marginRight="5dp"
        android:paddingRight="5dp"
        android:textOff="@string/light_off"
@@ -40,60 +58,36 @@
        android:id="@+id/backface_button"
        android:layout_width="100dp"
        android:layout_height="wrap_content"
-        android:layout_above="@+id/first_person_button"
-        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_above="@+id/camera_button"
+        android:layout_alignRight="@+id/camera_button"
        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
+        android:layout_marginBottom="15dp"
        android:layout_marginRight="5dp"
        android:paddingRight="5dp"
        android:textOff="@string/backface_off"
        android:textOn="@string/backface_on" />
 
-    <ToggleButton
-        android:id="@+id/first_person_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/third_person_button"
-        android:layout_alignLeft="@+id/third_person_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/first_person"
-        android:textOn="@string/first_person" />
-    
-    <ToggleButton
-        android:id="@+id/third_person_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/top_down_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/third_person"
-        android:textOn="@string/third_person" />
-
-    <ToggleButton
-        android:id="@+id/top_down_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
+    <com.introlab.rtabmap.NDSpinner
+        android:id="@+id/camera_button"
+        android:layout_width="140dp"
+        android:layout_height="40dp"
        android:layout_alignParentBottom="true"
        android:layout_alignParentRight="true"
        android:layout_marginRight="5dp"
+        android:layout_marginBottom="10dp"
        android:paddingRight="5dp"
-        android:textOff="@string/top_down"
-        android:textOn="@string/top_down" />
+        android:paddingBottom="10dp"
+        android:text="@string/camera_button"
+        android:spinnerMode="dropdown"/>

    <ToggleButton
        android:id="@+id/pause_button"
        android:layout_width="100dp"
        android:layout_height="wrap_content"
-        android:layout_alignLeft="@+id/first_person_button"
+		android:layout_marginRight="5dp"
        android:layout_alignParentTop="true"
-        android:layout_marginTop="61dp"
+        android:layout_marginTop="10dp"
        android:layout_alignParentRight="true"
-        android:paddingRight="5dp"
        android:textOff="@string/pause"
        android:textOn="@string/resume" />

@@ -101,26 +95,50 @@
        android:id="@+id/button_shareToSketchfab"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
-        android:layout_alignRight="@+id/pause_button"
+        android:layout_marginRight="5dp"
+        android:layout_marginTop="10dp"
+        android:layout_alignParentRight="true"
+        android:layout_below="@+id/pause_button"
        android:text="@string/share_to_sketchfab" />

    <Button
        android:id="@+id/button_saveOnDevice"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
-        android:layout_toLeftOf="@+id/button_shareToSketchfab"
+        android:layout_alignRight="@+id/button_shareToSketchfab"
+        android:layout_below="@+id/button_shareToSketchfab"
        android:text="@string/save_to_file" />

    <Button
        android:id="@+id/close_visualization_button"
        android:layout_width="200dp"
        android:layout_height="wrap_content"
-        android:layout_alignBaseline="@+id/top_down_button"
-        android:layout_alignBottom="@+id/top_down_button"
+        android:layout_above="@+id/camera_button"
        android:layout_centerHorizontal="true"
-        android:paddingLeft="5dp"
        android:text="@string/close_visualization" />

+    <SeekBar
+        android:id="@+id/seekBar_ortho_cut"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:rotation="270"
+        android:layout_above="@+id/light_button"
+        android:layout_alignParentLeft="true"
+        android:layout_gravity="center"
+        android:layout_marginLeft="-50dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+    
+    <SeekBar
+        android:id="@+id/seekBar_grid"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentLeft="true"
+        android:layout_alignBottom="@+id/camera_button"
+        android:layout_marginLeft="35dp"
+        android:paddingBottom="10dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+
+
+
+</RelativeLayout>        
 </RelativeLayout>
@@ -58,12 +58,12 @@
   			android:entries="@array/pref_background_color_keys"
   			android:entryValues="@array/pref_background_color_values"
   			android:defaultValue="@string/pref_default_background_color"/>
-        <SwitchPreference
+        <com.introlab.rtabmap.CustomSwitchPreference
            android:key="@string/pref_key_blending"
            android:title="@string/pref_title_blending"
            android:summary="@string/pref_summary_blending"
            android:defaultValue="@string/pref_default_blending"/>
-        <SwitchPreference
+        <com.introlab.rtabmap.CustomSwitchPreference
            android:key="@string/pref_key_nodes_filtering"
            android:title="@string/pref_title_nodes_filtering"
            android:summary="@string/pref_summary_nodes_filtering"
@@ -79,22 +79,22 @@
            android:summary="@string/pref_summary_mapping"
            android:persistent="false">
        
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_append"
 	            android:title="@string/pref_title_append"
 	            android:summary="@string/pref_summary_append"
 	            android:defaultValue="@string/pref_default_append"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_resolution"
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"
 		        android:summary="@string/pref_summary_smoothing"
 		        android:defaultValue="@string/pref_default_smoothing"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_fisheye"
 		        android:title="@string/pref_title_fisheye"
 		        android:summary="@string/pref_summary_fisheye"
@@ -109,6 +109,13 @@
 		            android:entries="@array/pref_update_rate_keys"
 		   			android:entryValues="@array/pref_update_rate_values"
 		   			android:defaultValue="@string/pref_default_update_rate"/>
+		        <ListPreference
+		            android:key="@string/pref_key_max_speed"
+		            android:title="@string/pref_title_max_speed"
+		            android:summary="@string/pref_summary_max_speed"
+		            android:entries="@array/pref_max_speed_keys"
+		   			android:entryValues="@array/pref_max_speed_values"
+		   			android:defaultValue="@string/pref_default_max_speed"/>
 		        <ListPreference 
 		            android:key="@string/pref_key_time_thr"
 		    		android:title="@string/pref_title_time_thr"
@@ -179,25 +186,49 @@
 		   			android:entries="@array/pref_optimizer_keys"
 		   			android:entryValues="@array/pref_optimizer_values"
 		   			android:defaultValue="@string/pref_default_optimizer"/>
-		        <SwitchPreference 
+		        <com.introlab.rtabmap.CustomSwitchPreference 
 		            android:key="@string/pref_key_optimize_end"
 		    		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">
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_keep_all_db"
 		            android:title="@string/pref_title_keep_all_db"
 		            android:summary="@string/pref_summary_keep_all_db"
 		            android:defaultValue="@string/pref_default_keep_all_db"/>
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_raw_scan_saved"
 		            android:title="@string/pref_title_raw_scan_saved"
 		            android:summary="@string/pref_summary_raw_scan_saved"
 		            android:defaultValue="@string/pref_default_raw_scan_saved"/>
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_gps_saved"
+		            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"
 		            android:summary="@string/pref_summary_db_in_memory"
@@ -227,6 +258,14 @@
 	   			android:entryValues="@array/pref_texture_size_values"
 	   			android:defaultValue="@string/pref_default_texture_size"/>
 	        
+	        <ListPreference 
+	            android:key="@string/pref_key_texture_count"
+	    		android:title="@string/pref_title_texture_count"
+	    		android:summary="@string/pref_summary_texture_count"
+	   			android:entries="@array/pref_texture_count_keys"
+	   			android:entryValues="@array/pref_texture_count_values"
+	   			android:defaultValue="@string/pref_default_texture_count"/>
+	        
 	        <ListPreference 
 	            android:key="@string/pref_key_normal_k"
 	    		android:title="@string/pref_title_normal_k"
@@ -251,7 +290,7 @@
 	   			android:entryValues="@array/pref_min_texture_cluster_size_values"
 	   			android:defaultValue="@string/pref_default_min_texture_cluster_size"/>
 	        
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_block_render"
 	            android:title="@string/pref_title_block_render"
 	            android:summary="@string/pref_summary_block_render"
@@ -278,15 +317,22 @@
 		   			android:entryValues="@array/pref_opt_color_radius_values"
 		   			android:defaultValue="@string/pref_default_opt_color_radius"/>
 		        
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_opt_clean_white"
 		            android:title="@string/pref_title_opt_clean_white"
 		            android:summary="@string/pref_summary_opt_clean_white"
 		            android:defaultValue="@string/pref_default_opt_clean_white"/>
 		        
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_min_cluster_size"
+		    		android:title="@string/pref_title_opt_min_cluster_size"
+		    		android:summary="@string/pref_summary_opt_min_cluster_size"
+		   			android:entries="@array/pref_opt_min_cluster_size_keys"
+		   			android:entryValues="@array/pref_opt_min_cluster_size_values"
+		   			android:defaultValue="@string/pref_default_opt_min_cluster_size"/>
+		        
 		       </PreferenceCategory>
 	 	</PreferenceScreen>
-        
 	 	<ListPreference 
            android:key="@string/pref_key_gain_max_radius"
    		android:title="@string/pref_title_gain_max_radius"
@@ -301,7 +347,20 @@
   			android:entries="@array/pref_cluster_ratio_keys"
   			android:entryValues="@array/pref_cluster_ratio_values"
   			android:defaultValue="@string/pref_default_cluster_ratio"/>
-        
+        <com.introlab.rtabmap.CustomSwitchPreference 
+            android:key="@string/pref_key_notification_sound"
+    		android:title="@string/pref_title_notification_sound"
+    		android:summary="@string/pref_summary_notification_sound"
+   			android:defaultValue="@string/pref_default_notification_sound"/>
+    </PreferenceCategory>
+	<PreferenceCategory
+        android:title="@string/pref_title_presets">
+        <Preference android:title="@string/pref_title_open_button"
+	                android:key="@string/pref_key_open_button"/>
+        <Preference android:title="@string/pref_title_save_button"
+	                android:key="@string/pref_key_save_button"/>
+        <Preference android:title="@string/pref_title_remove_button"
+	                android:key="@string/pref_key_remove_button"/>
 	    <Preference android:title="@string/pref_title_reset_button"
 	                android:key="@string/pref_key_reset_button"/>
    </PreferenceCategory>
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+android:layout_width="match_parent"
+android:layout_height="wrap_content"
+android:background="#fff">
+
+<ImageView
+android:id="@+id/imageView"
+android:layout_width="@dimen/image_width"
+android:layout_height="@dimen/image_width"
+android:layout_marginLeft="10dp"
+android:padding="5dp"
+android:src="@drawable/ic_launcher" />
+
+<TextView
+android:id="@+id/textView"
+android:layout_width="wrap_content"
+android:layout_height="wrap_content"
+android:layout_gravity="center"
+android:text="Demo"
+android:textColor="#000" />
+</LinearLayout>
@@ -0,0 +1,9 @@
+
+    <resources>
+        <string-array name="camera_view_array">
+        <item>First View</item>
+        <item>Third-P. View</item>
+        <item>Top View</item>
+        <item>Ortho View</item>
+    </string-array>
+    </resources>
@@ -0,0 +1,4 @@
+
+    <resources>
+        <dimen name="image_width">150dp</dimen>
+    </resources>
@@ -8,15 +8,14 @@
    <string name="sketchfab">Upload to Sketchfab&#8230;</string>
    <string name="status">"Status: "</string>
    <string name="words">"Words: "</string>
-    <string name="first_person">First</string>
-    <string name="third_person">Third</string>
-    <string name="top_down">Top</string>
+    <string name="camera_button">First View</string>
    <string name="pause">Pause</string>
    <string name="resume">Resume</string>
    <string name="backface_on">Backface</string>
    <string name="backface_off">Backface</string>
    <string name="light_on">Lighting</string>
    <string name="light_off">Lighting</string>
+    <string name="wireframe">Wireframe</string>
    <string name="close_visualization">Close Visualization</string>
    <string name="save_to_file">Export to File&#8230;</string>
    <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
@@ -35,12 +34,19 @@
    <string name="memory">"Used Memory (MB): "</string>
    <string name="hypothesis">"Hypothesis (%): "</string>
    <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 -->
    <string name="pref_key_tags">pref_key_tags</string>
    <string name="pref_default_tags">rtabmap 3dscan tango</string>
    <string name="pref_key_rendering">pref_key_rendering</string>
    <string name="pref_default_rendering">2</string>
+    <string name="pref_key_open_button">pref_key_open_button</string>
+    <string name="pref_key_save_button">pref_key_save_button</string>
+    <string name="pref_key_remove_button">pref_key_remove_button</string>
    <string name="pref_key_reset_button">pref_key_reset_button</string>
    <string name="pref_key_density">pref_key_density</string>
    <string name="pref_default_density">1</string>
@@ -49,7 +55,7 @@
    <string name="pref_key_depth">pref_key_depth</string>
    <string name="pref_default_depth">2.5</string>
    <string name="pref_key_point_size">pref_key_point_size</string>
-    <string name="pref_default_point_size">5</string>
+    <string name="pref_default_point_size">10</string>
    <string name="pref_key_angle">pref_key_angle</string>
    <string name="pref_default_angle">20</string>
    <string name="pref_key_triangle">pref_key_triangle</string>
@@ -73,6 +79,8 @@
    
    <string name="pref_key_update_rate">pref_key_update_rate</string>
    <string name="pref_default_update_rate">1</string>
+    <string name="pref_key_max_speed">pref_key_max_speed</string>
+    <string name="pref_default_max_speed">0</string>
    <string name="pref_key_time_thr">pref_key_time_thr</string>
    <string name="pref_default_time_thr">1000</string>
    <string name="pref_key_mem_thr">pref_key_mem_thr</string>
@@ -84,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">0.1</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>
@@ -95,17 +103,27 @@
    <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">true</string>
+    <string name="pref_default_db_in_memory">false</string>
    
    <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
    <string name="pref_default_cloud_voxel">0.01</string>
    <string name="pref_key_texture_size">pref_key_texture_size</string>
    <string name="pref_default_texture_size">4096</string>
+    <string name="pref_key_texture_count">pref_key_texture_count</string>
+    <string name="pref_default_texture_count">1</string>
    <string name="pref_key_normal_k">pref_key_normal_k</string>
    <string name="pref_default_normal_k">18</string>
    <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
@@ -120,10 +138,14 @@
    <string name="pref_default_opt_color_radius">0.05</string>
    <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>
    <string name="pref_default_opt_clean_white">true</string>
+    <string name="pref_key_opt_min_cluster_size">pref_key_opt_min_cluster_size</string>
+    <string name="pref_default_opt_min_cluster_size">0</string>
    <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
    <string name="pref_default_gain_max_radius">0.02</string>
    <string name="pref_key_cluster_ratio">pref_key_cluster_ratio</string>
    <string name="pref_default_cluster_ratio">0.05</string>
+    <string name="pref_key_notification_sound">pref_key_notification_sound</string>
+    <string name="pref_default_notification_sound">true</string>
    <!-- Preference keys: END -->

    <string name="pref_title_rendering">Rendering</string>
@@ -302,9 +324,11 @@
    <string name="pref_title_smoothing">Smoothing</string>
    <string name="pref_summary_smoothing">Smooth the point clouds.</string>
    <string name="pref_title_fisheye">Fish Eye Camera</string>
-    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. Cannot be used on Yellowstone tablet.</string>
+    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
    <string name="pref_title_update_rate">Update Rate</string>
    <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
+    <string name="pref_title_max_speed">Maximum Motion Speed</string>
+    <string name="pref_summary_max_speed">Images taken when the camera is moving too fast are ignored to avoid blurry textures.</string>
    <string name="pref_title_time_thr">Time Limit</string>
    <string name="pref_summary_time_thr">Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates.</string>
    <string name="pref_title_mem_thr">Memory Limit</string>
@@ -316,7 +340,7 @@
    <string name="pref_title_min_inliers">Min Inliers</string>
    <string name="pref_summary_min_inliers">Minimum visual inliers to accept a loop closure.</string>
    <string name="pref_title_opt_error">Max Optimization Error</string>
-    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this threshold.</string>
+    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this factor of the link\'s variance.</string>
    <string name="pref_title_features_voc">Max Features Extracted (Vocabulary)</string>
    <string name="pref_summary_features_voc">Extracting more features per image would result in better loop closure hypotheses but more processing time is required.</string>
    <string name="pref_title_features">Max Features Extracted (Loop Closure)</string>
@@ -327,10 +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 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>

@@ -352,6 +384,20 @@
        <item>"1"</item>
        <item>"0.5"</item>
    </string-array>
+    <string-array name="pref_max_speed_keys">
+        <item>"No Limit"</item>
+        <item>"High"</item>
+        <item>"Medium"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_max_speed_values">
+        <item>"0"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+    </string-array>
    <string-array name="pref_time_thr_keys">
        <item>"No Limit"</item>
        <item>"1500 ms"</item>
@@ -461,25 +507,29 @@
        <item>"10"</item>
    </string-array>
    <string-array name="pref_opt_error_keys">
-        <item>"1.0 m"</item>
-        <item>"0.5 m"</item>
-        <item>"0.35 m"</item>
-        <item>"0.2 m"</item>
-        <item>"0.1 m"</item>
-        <item>"0.05 m"</item>
-        <item>"0.025 m"</item>
-        <item>"0.01 m"</item>
+        <item>"10x"</item>
+        <item>"9x"</item>
+        <item>"8x"</item>
+        <item>"7x"</item>
+        <item>"6x"</item>
+        <item>"5x"</item>
+        <item>"4x"</item>
+        <item>"3x"</item>
+        <item>"2x"</item>
+        <item>"1x"</item>
        <item>"Disabled"</item>
    </string-array>
    <string-array name="pref_opt_error_values">
-        <item>"1.0"</item>
-        <item>"0.5"</item>
-        <item>"0.35"</item>
-        <item>"0.2"</item>
-        <item>"0.1"</item>
-        <item>"0.05"</item>
-        <item>"0.025"</item>
-        <item>"0.01"</item>
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
        <item>"0"</item>
    </string-array>
    <string-array name="pref_features_voc_keys">
@@ -554,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>
@@ -562,6 +697,8 @@
    <string name="pref_summary_cloud_voxel">If you don\'t need a very precise point cloud, you can set this to reduce the output point cloud size. This is also used for optimized mesh.</string>
    <string name="pref_title_texture_size">Texture Size</string>
    <string name="pref_summary_texture_size">If the map is large, you may want to increase this to maximize the texture resolution.</string>
+    <string name="pref_title_texture_count">Maximum Output Textures</string>
+    <string name="pref_summary_texture_count">Note that all textures are still merged into one for visualization, but exported in multiple files.</string>
    <string name="pref_title_normal_k">Normal K</string>
    <string name="pref_summary_normal_k">K-nearest neighbors used for normal computation when a mesh is created.</string>
    <string name="pref_title_max_texture_distance">Max Texture Distance</string>
@@ -603,6 +740,26 @@
        <item>"2048"</item>
        <item>"1024"</item>
    </string-array>
+    <string-array name="pref_texture_count_keys">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_texture_count_values">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
    <string-array name="pref_normal_k_values">
        <item>"30"</item>
        <item>"24"</item>
@@ -631,22 +788,22 @@
        <item>"2"</item>
    </string-array>
    <string-array name="pref_min_texture_cluster_size_keys">
-        <item>"Disabled"</item>
        <item>"1000"</item>
        <item>"500"</item>
        <item>"200"</item>
        <item>"100"</item>
        <item>"50"</item>
        <item>"10"</item>
+        <item>"Disabled"</item>
    </string-array>
    <string-array name="pref_min_texture_cluster_size_values">
-        <item>"0"</item>
        <item>"1000"</item>
        <item>"500"</item>
        <item>"200"</item>
        <item>"100"</item>
        <item>"50"</item>
        <item>"10"</item>
+        <item>"0"</item>
    </string-array>
    
    <string name="pref_title_optimized">Optimized</string>
@@ -658,6 +815,8 @@
    <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
    <string name="pref_title_opt_clean_white">Clean Mesh</string>
    <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
+	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>
+    <string name="pref_title_opt_min_cluster_size">Polygon Filtering</string>
    
    <string-array name="pref_opt_depth_keys">
        <item>"Auto"</item>
@@ -707,12 +866,22 @@
        <item>"0.01"</item>
        <item>"-1"</item>
    </string-array>
+    <string-array name="pref_opt_min_cluster_size_keys">
+        <item>"Only biggest cluster kept"</item>
+        <item>"Keep all polygons"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+    </string-array>

    <string name="pref_title_general">General</string>
    <string name="pref_title_gain_max_radius">Color Correction Radius</string>
    <string name="pref_summary_gain_max_radius">Radius used to find pixel correspondences for Adjust Colors optimization.</string>
    <string name="pref_title_cluster_ratio">Noise Filtering Ratio</string>
    <string name="pref_summary_cluster_ratio">Polygon clusters with size smaller than this ratio of the largest cluster are removed by the Noise Filtering optimization.</string>
+    <string name="pref_title_notification_sound">Notification Sound</string>
+    <string name="pref_summary_notification_sound">After saving database or preparing data to export, a notification sound is played.</string>

    <string-array name="pref_gain_max_radius_keys">
        <item>"0.3 m"</item>
@@ -763,6 +932,10 @@
        <item>"0.01"</item>
    </string-array>

+    <string name="pref_title_presets">Presets</string>
+    <string name="pref_title_open_button">Open</string>
+    <string name="pref_title_save_button">Save</string>
+    <string name="pref_title_remove_button">Remove</string>
    <string name="pref_title_reset_button">Restore All Default Settings</string>
    <string name="title_activity_sketchfab">SketchfabActivity</string>
    <string name="hello_world">Hello world!</string>
@@ -0,0 +1,10 @@
+<resources>
+    <style name="ThemeActionBar" parent="@android:style/Widget.DeviceDefault.ActionBar.Solid">
+        <item name="android:background">#20000000</item>
+    </style>
+
+    <style name="ThemeApp" parent="@android:style/Theme.DeviceDefault">
+        <item name="android:actionBarStyle">@style/ThemeActionBar</item>
+        <item name="android:windowActionBarOverlay">true</item>
+    </style>
+</resources>
@@ -0,0 +1,82 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.preference.SwitchPreference;
+import android.util.AttributeSet;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.Switch;
+
+/**
+ * 
+ * @author mathieu
+ * Bug fix of switch preferences changing states 
+ * when scrolling on Jelly Bean: 
+ * https://issuetracker.google.com/issues/36941388#comment4
+ */
+
+public class CustomSwitchPreference extends SwitchPreference { 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     * @param defStyle Theme attribute defining the default style options 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs, int defStyle) { 
+        super(context, attrs, defStyle); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs) { 
+        super(context, attrs); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with default style options. 
+     * 
+     * @param context The Context that will style this preference 
+     */ 
+    public CustomSwitchPreference(Context context) { 
+        super(context, null); 
+    } 
+
+    @Override 
+    protected void onBindView(View view) { 
+        // Clean listener before invoke SwitchPreference.onBindView 
+        ViewGroup viewGroup= (ViewGroup)view; 
+        clearListenerInViewGroup(viewGroup); 
+        super.onBindView(view); 
+    } 
+
+    /** 
+     * Clear listener in Switch for specify ViewGroup. 
+     * 
+     * @param viewGroup The ViewGroup that will need to clear the listener. 
+     */ 
+    private void clearListenerInViewGroup(ViewGroup viewGroup) { 
+        if (null == viewGroup) { 
+            return; 
+        } 
+
+        int count = viewGroup.getChildCount(); 
+        for(int n = 0; n < count; ++n) { 
+            View childView = viewGroup.getChildAt(n); 
+            if(childView instanceof Switch) { 
+                final Switch switchView = (Switch) childView; 
+                switchView.setOnCheckedChangeListener(null); 
+                return; 
+            } else if (childView instanceof ViewGroup){ 
+                ViewGroup childGroup = (ViewGroup)childView; 
+                clearListenerInViewGroup(childGroup); 
+            } 
+        } 
+    } 
+
+}
@@ -0,0 +1,102 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.database.Cursor;
+import android.database.sqlite.SQLiteDatabase;
+import android.graphics.Bitmap;
+import android.graphics.BitmapFactory;
+import android.graphics.drawable.Drawable;
+import android.util.Log;
+import android.view.LayoutInflater;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.BaseAdapter;
+import android.widget.ImageView;
+import android.widget.LinearLayout;
+import android.widget.SimpleAdapter;
+
+import java.io.ByteArrayInputStream;
+import java.io.InputStream;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+public class DatabaseListArrayAdapter extends SimpleAdapter {
+	LayoutInflater inflater;
+	Context context;
+	ArrayList<HashMap<String, String>> arrayList;
+	int imageWidth;
+
+	public DatabaseListArrayAdapter(Context context, ArrayList<HashMap<String, String>> data, int resource, String[] from, int[] to) {
+		super(context, data, resource, from, to);
+		this.context = context;
+		this.arrayList = data;
+		this.imageWidth = (int)context.getResources().getDimension(R.dimen.image_width);
+		inflater.from(context);
+	}
+
+	@Override
+	public View getView(final int position, View convertView, ViewGroup parent) {
+		View view = super.getView(position, convertView, parent);
+		ImageView imageView = (ImageView) view.findViewById(R.id.imageView);
+		
+		boolean imageSet = false;
+		String path = this.arrayList.get(position).get("path");
+		if(!path.isEmpty())
+		{
+			SQLiteDatabase db = null;
+			try {
+				db = SQLiteDatabase.openDatabase(path, null, SQLiteDatabase.OPEN_READONLY);
+				
+				// get version
+				Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);    
+		        if(c1.moveToFirst()) {
+		        	String version = c1.getString(c1.getColumnIndex("version"));
+		        	Log.i(RTABMapActivity.TAG, "Version="+version);
+		        	if(Util.versionCompare(version, "0.12.0") >= 0) {
+		        		Cursor c2 = db.rawQuery("SELECT preview_image FROM Admin WHERE preview_image is not null", null); 
+		        		if(c2.moveToFirst()) {
+		        			Log.i(RTABMapActivity.TAG, "Found image preview for db " + path);
+
+		        			byte[] bytes = c2.getBlob(c2.getColumnIndex("preview_image"));
+		        			ByteArrayInputStream inputStream = new ByteArrayInputStream(bytes);
+		        			Bitmap bitmap = BitmapFactory.decodeStream(inputStream);
+		        			imageView.setImageBitmap(bitmap);
+		        			imageSet = true;
+		        		}
+		        		else {
+		        			Log.i(RTABMapActivity.TAG, "Not found image preview for db " + path);
+		        		}
+		        	}
+		        	else {
+		        		Log.i(RTABMapActivity.TAG, "Too old database for preview image, path = " + path);
+		        	}
+		        }
+		        else {
+		        	Log.e(RTABMapActivity.TAG, "Failed getting version from database");
+		        }
+	
+	        } catch (Exception e) {
+	            Log.e(RTABMapActivity.TAG, e.getMessage());
+	        }
+			finally {
+				if(db != null && db.isOpen()) {
+	            	db.close();
+	            }
+			}
+		}
+		else
+		{
+			Log.e(RTABMapActivity.TAG, "Database path empty for item " + position);
+		}
+		
+		if(!imageSet)
+		{
+			Drawable myDrawable = context.getResources().getDrawable(R.drawable.ic_launcher);
+			imageView.setImageDrawable(myDrawable);
+		}
+		LinearLayout.LayoutParams layoutParams = new LinearLayout.LayoutParams(imageWidth,imageWidth/(!imageSet?2:1));
+		imageView.setLayoutParams(layoutParams);
+		return view;
+	}
+
+}
@@ -0,0 +1,43 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.util.AttributeSet;
+import android.widget.Spinner;
+
+
+/** Spinner extension that calls onItemSelected even when the selection is the same as its previous value 
+Author: Mattia Ruggiero 
+https://stackoverflow.com/questions/5335306/how-can-i-get-an-event-in-android-spinner-when-the-current-selected-item-is-sele
+*/
+public class NDSpinner extends Spinner {
+
+    public NDSpinner(Context context)
+    { super(context); }
+
+    public NDSpinner(Context context, AttributeSet attrs)
+    { super(context, attrs); }
+
+    public NDSpinner(Context context, AttributeSet attrs, int defStyle)
+    { super(context, attrs, defStyle); }
+
+    @Override 
+    public void setSelection(int position, boolean animate) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position, animate);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            //getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    } 
+
+    @Override
+    public void setSelection(int position) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    }
+
+}
@@ -28,6 +28,7 @@ public class RTABMapLib
    public static native void setScreenRotation(int displayRotation, int cameraRotation);
    
    public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase2(String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
    
    /*
     * Called when the Tango service is connected.
@@ -77,8 +78,12 @@ public class RTABMapLib
    public static native void setMaxCloudDepth(float value);
    public static native void setMinCloudDepth(float value);
    public static native void setPointSize(float value);
+    public static native void setFOV(float value);
+    public static native void setOrthoCropFactor(float value);
+    public static native void setGridRotation(float value);
    public static native void setLighting(boolean enabled);
    public static native void setBackfaceCulling(boolean enabled);
+    public static native void setWireframe(boolean enabled);
    public static native void setCloudDensityLevel(int value);
    public static native void setMeshAngleTolerance(float value);
    public static native void setMeshTriangleSize(int value);
@@ -87,16 +92,24 @@ public class RTABMapLib
    public static native void setRenderingTextureDecimation(int value);
    public static native void setBackgroundColor(float gray);
    public static native int setMappingParameter(String key, String value);
+    public static native void setGPS(
+			double stamp,
+			double longitude, 
+			double latitude, 
+			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);
    public static native void cancelProcessing();
    public static native boolean exportMesh(
-    		String filePath,
    		float cloudVoxelSize,
    		boolean regenerateCloud,
    		boolean meshing,
    		int textureSize,
+    		int textureCount,
    		int normalK,
    		boolean optimized,
    		float optimizedVoxelSize,
@@ -104,10 +117,11 @@ public class RTABMapLib
    		int optimizedMaxPolygons,
    		float optimizedColorRadius,
    		boolean optimizedCleanWhitePolygons,
-    		boolean optimizedColorWhitePolygons,
+    		int optimizedMinClusterSize,
    		float optimizedMaxTextureDistance,
    		int optimizedMinTextureClusterSize,
    		boolean blockRendering);
+    public static native boolean writeExportedMesh(String directory, String name);
    public static native boolean postExportation(boolean visualize);
    public static native int postProcessing(int approach);
    
@@ -42,6 +42,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private TextManager mTextManager = null;
 	private float mSurfaceHeight = 0.0f;
 	private float mTextColor = 1.0f;
+	private int mOffset = 0;
 	
 	private Vector<TextObject> mTexts;

@@ -66,6 +67,11 @@ public class Renderer implements GLSurfaceView.Renderer {
 		mToast = toast;
 	}
 	
+	public void setOffset(int offset)
+	{
+		mOffset = offset;
+	}
+
 	// Render loop of the Gl context.
 	public void onDrawFrame(GL10 useGLES20instead) {

@@ -94,7 +100,9 @@ public class Renderer implements GLSurfaceView.Renderer {
 					mTextManager.PrepareDraw(txtcollection);
 				}
 				
-				mTextManager.Draw(mtrxProjectionAndView);
+				float[] mvp = new float[16];
+				Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
+				mTextManager.Draw(mvp);
 			}

 			if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
@@ -103,6 +111,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 					public void run() {
 						if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
 						mProgressDialog.dismiss();
+						mActivity.resetNoTouchTimer();
 					}
 				});
 			}
@@ -1,23 +1,36 @@
 package com.introlab.rtabmap;

-import android.app.Activity;
+import java.io.File;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Iterator;
+import java.util.Map.Entry;
+
+import android.app.AlertDialog;
+import android.content.DialogInterface;
 import android.content.SharedPreferences;
 import android.content.SharedPreferences.OnSharedPreferenceChangeListener;
 import android.os.Bundle;
 import android.preference.ListPreference;
 import android.preference.Preference;
 import android.preference.PreferenceActivity;
-import android.preference.PreferenceManager;
+import android.text.InputType;
+import android.view.WindowManager;
+import android.view.inputmethod.EditorInfo;
+import android.widget.EditText;

 public class SettingsActivity extends PreferenceActivity implements OnSharedPreferenceChangeListener {
 	
+	private SettingsActivity getActivity() {return this;}
+	
    @Override
    public void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        addPreferencesFromResource(R.layout.activity_settings);
        
-        Preference button = findPreference(getString(R.string.pref_key_reset_button));
-        button.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        Preference buttonReset = findPreference(getString(R.string.pref_key_reset_button));
+        buttonReset.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
        	@Override
        	public boolean onPreferenceClick(Preference preference) {   
        		getPreferenceScreen().getSharedPreferences().edit().clear().commit();
@@ -28,6 +41,150 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
        	}
        });
        
+        Preference buttonOpen = findPreference(getString(R.string.pref_key_open_button));
+        buttonOpen.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Choose Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						//sp1 is the shared pref to copy to
+        						SharedPreferences.Editor ed = getPreferenceScreen().getSharedPreferences().edit(); 
+        						SharedPreferences sp = getActivity().getSharedPreferences(files[which], MODE_PRIVATE); //The shared preferences to copy from
+        						//Cycle through all the entries in the sp
+        						for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+        						 Object v = entry.getValue(); 
+        						 String key = entry.getKey();
+        						 //Now we just figure out what type it is, so we can copy it.
+        						 // Note that i am using Boolean and Integer instead of boolean and int.
+        						 // That's because the Entry class can only hold objects and int and boolean are primatives.
+        						 if(v instanceof Boolean) 
+        						 // Also note that i have to cast the object to a Boolean 
+        						 // and then use .booleanValue to get the boolean
+        						    ed.putBoolean(key, ((Boolean)v).booleanValue());
+        						 else if(v instanceof Float)
+        						    ed.putFloat(key, ((Float)v).floatValue());
+        						 else if(v instanceof Integer)
+        						    ed.putInt(key, ((Integer)v).intValue());
+        						 else if(v instanceof Long)
+        						    ed.putLong(key, ((Long)v).longValue());
+        						 else if(v instanceof String)
+        						    ed.putString(key, ((String)v));         
+        						}
+        						ed.commit(); //save it.
+        						recreate();
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonSave = findPreference(getString(R.string.pref_key_save_button));
+        buttonSave.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+    			builder.setTitle("Save Presets:");
+    			final EditText input = new EditText(getActivity());
+    			input.setInputType(InputType.TYPE_CLASS_TEXT); 
+    			input.setImeOptions(EditorInfo.IME_FLAG_NO_EXTRACT_UI);
+    			builder.setView(input);
+    			builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+    				@Override
+    				public void onClick(DialogInterface dialog, int which)
+    				{
+    					final String fileName = input.getText().toString();  
+    					dialog.dismiss();
+    					if(!fileName.isEmpty())
+    					{
+    						File newFile = new File(getApplicationInfo().dataDir + "/shared_prefs/" + fileName + ".xml");
+    						if(newFile.exists())
+    						{
+    							new AlertDialog.Builder(getActivity())
+    							.setTitle("Presets Already Exist")
+    							.setMessage("Do you want to overwrite the existing file?")
+    							.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									saveConfig(fileName);
+    								}
+    							})
+    							.setNegativeButton("No", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									dialog.dismiss();
+    								}
+    							})
+    							.show();
+    						}
+    						else
+    						{
+    							saveConfig(fileName);
+    						}
+    					}
+    				}
+    			});
+    			AlertDialog alertToShow = builder.create();
+    			alertToShow.getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_VISIBLE);
+    			alertToShow.show();
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonRemove = findPreference(getString(R.string.pref_key_remove_button));
+        buttonRemove.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Remove Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						File file = new File(getApplicationInfo().dataDir + "/shared_prefs/" + files[which] + ".xml");
+        						if(file.exists())
+        						{
+        							file.delete();
+        						}
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        
        ((Preference)findPreference(getString(R.string.pref_key_density))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_density))).getEntry() + ") "+getString(R.string.pref_summary_density));
        ((Preference)findPreference(getString(R.string.pref_key_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth))).getEntry() + ") "+getString(R.string.pref_summary_depth));
        ((Preference)findPreference(getString(R.string.pref_key_min_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getEntry() + ") "+getString(R.string.pref_summary_min_depth));
@@ -38,6 +195,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
        ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
          
        ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
        ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
        ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
        ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
@@ -48,15 +206,19 @@ 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));
+        ((Preference)findPreference(getString(R.string.pref_key_texture_count))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_count))).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
        ((Preference)findPreference(getString(R.string.pref_key_normal_k))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_normal_k))).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
        ((Preference)findPreference(getString(R.string.pref_key_max_texture_distance))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_texture_distance))).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
        ((Preference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));

        ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
        ((Preference)findPreference(getString(R.string.pref_key_opt_color_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_color_radius))).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        ((Preference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));

        ((Preference)findPreference(getString(R.string.pref_key_cluster_ratio))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cluster_ratio))).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
        ((Preference)findPreference(getString(R.string.pref_key_gain_max_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_gain_max_radius))).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
@@ -94,6 +256,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
        	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
        	
        	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
        	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
        	if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
        	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
@@ -104,15 +267,19 @@ 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));
+        	if(key.compareTo(getString(R.string.pref_key_texture_count))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
        	if(key.compareTo(getString(R.string.pref_key_normal_k))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
        	if(key.compareTo(getString(R.string.pref_key_max_texture_distance))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
        	if(key.compareTo(getString(R.string.pref_key_min_texture_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));

        	if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
        	if(key.compareTo(getString(R.string.pref_key_opt_color_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        	if(key.compareTo(getString(R.string.pref_key_opt_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
        	
        	if(key.compareTo(getString(R.string.pref_key_cluster_ratio))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
        	if(key.compareTo(getString(R.string.pref_key_gain_max_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
@@ -135,4 +302,33 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
        getPreferenceScreen().getSharedPreferences()
                .unregisterOnSharedPreferenceChangeListener(this);
    }
+    
+    private void saveConfig(String fileName)
+    {
+    	//sp1 is the shared pref to copy to
+		SharedPreferences.Editor ed = getActivity().getSharedPreferences(fileName, MODE_PRIVATE).edit(); 
+		SharedPreferences sp = getPreferenceScreen().getSharedPreferences(); //The shared preferences to copy from
+		ed.clear(); // This clears the one we are copying to, but you don't necessarily need to do that.
+		//Cycle through all the entries in the sp
+		for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+		 Object v = entry.getValue(); 
+		 String key = entry.getKey();
+		 //Now we just figure out what type it is, so we can copy it.
+		 // Note that i am using Boolean and Integer instead of boolean and int.
+		 // That's because the Entry class can only hold objects and int and boolean are primatives.
+		 if(v instanceof Boolean) 
+		 // Also note that i have to cast the object to a Boolean 
+		 // and then use .booleanValue to get the boolean
+		    ed.putBoolean(key, ((Boolean)v).booleanValue());
+		 else if(v instanceof Float)
+		    ed.putFloat(key, ((Float)v).floatValue());
+		 else if(v instanceof Integer)
+		    ed.putInt(key, ((Integer)v).intValue());
+		 else if(v instanceof Long)
+		    ed.putLong(key, ((Long)v).longValue());
+		 else if(v instanceof String)
+		    ed.putString(key, ((String)v));         
+		}
+		ed.commit(); //save it.
+    }
 }
@@ -44,7 +44,6 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	private Dialog mAuthDialog;
 	
 	private String mAuthToken;
-	private boolean mExportedOBJ;
 	private String mWorkingDirectory;
 	
 	EditText mFilename;
@@ -67,10 +66,10 @@ 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);
-		mExportedOBJ = getIntent().getExtras().getBoolean(RTABMapActivity.RTABMAP_EXPORTED_OBJ_KEY);
 		mFilename.setText(getIntent().getExtras().getString(RTABMapActivity.RTABMAP_FILENAME_KEY));
 		mWorkingDirectory = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_WORKING_DIR_KEY);
 		
@@ -129,51 +128,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 			editor.commit();
 		}
 		
-		final String extension  = mExportedOBJ?".obj":".ply";
-		
-		String[] files = new String[0];
-		// verify if we have all files
-		if(extension.compareTo(".obj") == 0)
-		{
-			File objFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".obj");
-			File mltFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".mtl");
-			File jpgFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".jpg");
-			if(objFile.exists() && mltFile.exists() && jpgFile.exists())
-			{
-				files = new String[3];
-				files[0] = objFile.getAbsolutePath();
-				files[1] = mltFile.getAbsolutePath();
-				files[2] = jpgFile.getAbsolutePath();
-			}
-			else
-			{
-				Toast.makeText(getActivity(), String.format("Missing OBJ files!"), Toast.LENGTH_LONG).show();
-			}
-		}
-		else if(extension.compareTo(".ply") == 0)
-		{
-			File plyFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + extension);
-			if(plyFile.exists())
-			{
-				files = new String[1];
-				files[0] = plyFile.getAbsolutePath();
-			}
-			else
-			{
-				Toast.makeText(getActivity(), String.format("Missing PLY file!"), Toast.LENGTH_LONG).show();
-			}
-		}
-		else
-		{
-			Toast.makeText(getActivity(), String.format("Unknown file extension \"%s\"!", extension), Toast.LENGTH_LONG).show();
-		}
-
-		if(files.length > 0)
-		{
-			final String[] filesToZip = files;
-			authorizeAndPublish(filesToZip, mFilename.getText().toString());
-		}
-	
+		authorizeAndPublish(mFilename.getText().toString());	
 	}
 	
 	private boolean isNetworkAvailable() {
@@ -183,24 +138,27 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	    return activeNetworkInfo != null && activeNetworkInfo.isConnected();
 	}
 	
-	private void authorizeAndPublish(final String[] filesToZip, final String fileName)
+	private void authorizeAndPublish(final String fileName)
 	{
 		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() {
 				public void onClick(DialogInterface dialog, int which) {
-					authorizeAndPublish(filesToZip, fileName);
+					authorizeAndPublish(fileName);
 				}
 			})
 			.setNeutralButton("Abort", new DialogInterface.OnClickListener() {
 				public void onClick(DialogInterface dialog, int which) {
 				}
 			})
-			.show();
+			.create();
+			ad.setCanceledOnTouchOutside(false);
+			ad.show();
 			return;
 		}	

@@ -211,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);
@@ -240,32 +200,74 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 						
 						mAuthDialog.dismiss();

-						zipAndPublish(filesToZip, fileName);
+						zipAndPublish(fileName);
 					}
 				}
 			});
 			mAuthDialog.show();
 			mAuthDialog.setTitle("Authorize RTAB-Map");
-			mAuthDialog.setCancelable(true);
 			web.loadUrl(auth_url);
 		}
 		else
 		{
-			zipAndPublish(filesToZip, fileName);
+			zipAndPublish(fileName);
 		}
 	}

-	private void zipAndPublish(final String[] filesToZip, final String fileName)
+	private void zipAndPublish(final String fileName)
 	{		
-		final String zipOutput = mWorkingDirectory+fileName+".zip";
-
 		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() {
 			public void run() {
 				try{
+					
+					File tmpDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR);
+					tmpDir.mkdirs();
+					String[] fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+					if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR));
+					for(int i=0; i<fileNames.length; ++i)
+					{
+						File f = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i]);
+						if(f.delete())
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleted \"%s\"", f.getPath()));
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Failed deleting \"%s\"", f.getPath()));
+						}
+					}
+					File exportDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_EXPORT_DIR);
+					exportDir.mkdirs();
+					
+					if(RTABMapLib.writeExportedMesh(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
+					{
+						String[] files = new String[0];
+						// verify if we have all files
+				
+						fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+						if(fileNames.length > 0)
+						{
+							files = new String[fileNames.length];
+							for(int i=0; i<fileNames.length; ++i)
+							{
+								files[i] = mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i];
+							}
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, "Missing files!");
+						}
+				
+						if(files.length > 0)
+						{
+							final String[] filesToZip = files;
+							final String zipOutput = mWorkingDirectory+fileName+".zip";
 							Util.zip(filesToZip, zipOutput);
 							runOnUiThread(new Runnable() {
 								public void run() {
@@ -313,10 +315,24 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 											// do nothing...
 										}
 									});
-							builder.show();
+									AlertDialog ad = builder.create();
+									ad.setCancelable(false);
+									ad.setCanceledOnTouchOutside(false);
+									ad.show();
 								}
 							});
 						}
+					}
+					else
+					{
+						runOnUiThread(new Runnable() {
+							public void run() {
+								mProgressDialog.dismiss();
+								Toast.makeText(getActivity(), String.format("Failed writing files!"), Toast.LENGTH_LONG).show();
+							}
+						});
+					}
+				}
 				catch(IOException ex) {
 					Log.e(RTABMapActivity.TAG, "Failed to zip", ex);
 				}
@@ -365,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());
 			}
@@ -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;}
 	
@@ -2,9 +2,12 @@ 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.util.Arrays;
 import java.util.zip.ZipEntry;
 import java.util.zip.ZipOutputStream;

@@ -52,5 +55,72 @@ public class Util {
 		}
 	}

+	public static String[] loadFileList(String directory, final boolean databasesOnly) {
+		File path = new File(directory); 
+		String fileList[];
+		try {
+			path.mkdirs();
+		}
+		catch(SecurityException e) {
+			Log.e(RTABMapActivity.TAG, "unable to write on the sd card " + e.toString());
+		}
+		if(path.exists()) {
+			FilenameFilter filter = new FilenameFilter() {
+
+				@Override
+				public boolean accept(File dir, String filename) {
+					File sel = new File(dir, filename);
+					if(databasesOnly)
+					{
+						return filename.compareTo(RTABMapActivity.RTABMAP_TMP_DB) != 0 && filename.endsWith(".db");
+					}
+					else
+					{
+						return sel.isFile();
+					}
+				}
+
+			};
+			fileList = path.list(filter);
+			Arrays.sort(fileList);
+		}
+		else {
+			fileList = new String[0];
+		}
+		return fileList;
+	}
+	
+	/**
+	 * https://stackoverflow.com/questions/6701948/efficient-way-to-compare-version-strings-in-java
+	 * Compares two version strings. 
+	 * 
+	 * Use this instead of String.compareTo() for a non-lexicographical 
+	 * comparison that works for version strings. e.g. "1.10".compareTo("1.6").
+	 * 
+	 * @note It does not work if "1.10" is supposed to be equal to "1.10.0".
+	 * 
+	 * @param str1 a string of ordinal numbers separated by decimal points. 
+	 * @param str2 a string of ordinal numbers separated by decimal points.
+	 * @return The result is a negative integer if str1 is _numerically_ less than str2. 
+	 *         The result is a positive integer if str1 is _numerically_ greater than str2. 
+	 *         The result is zero if the strings are _numerically_ equal.
+	 */
+	public static int versionCompare(String str1, String str2) {
+	    String[] vals1 = str1.split("\\.");
+	    String[] vals2 = str2.split("\\.");
+	    int i = 0;
+	    // set index to first non-equal ordinal or length of shortest version string
+	    while (i < vals1.length && i < vals2.length && vals1[i].equals(vals2[i])) {
+	      i++;
+	    }
+	    // compare first non-equal ordinal number
+	    if (i < vals1.length && i < vals2.length) {
+	        int diff = Integer.valueOf(vals1[i]).compareTo(Integer.valueOf(vals2[i]));
+	        return Integer.signum(diff);
+	    }
+	    // the strings are equal or one string is a substring of the other
+	    // e.g. "1.2.3" = "1.2.3" or "1.2.3" < "1.2.3.4"
+	    return Integer.signum(vals1.length - vals2.length);
+	}
 	
 }
@@ -74,7 +74,11 @@ ELSE()
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
 IF(Qt5_FOUND)
+    IF(Qt5Svg_FOUND)
        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    ELSE()
+        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
+    ENDIF()
 ENDIF(Qt5_FOUND)

 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -100,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
        WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
       
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
  SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
  SET(plugin_dest_dir bin)
  SET(qtconf_dest_dir bin)
@@ -150,11 +154,28 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
 		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
 		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
 		list(GET loc_list 1 plugin_type)
+		IF(NOT plugin_root)
+			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
+		ENDIF(NOT plugin_root)
 	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
          DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
          COMPONENT runtime)
 	endforeach()
+	IF(WIN32)
+		IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+			IF(EXISTS ${plugin_loc})
+				get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+				string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+				list(GET loc_list 1 plugin_type)
+				INSTALL(FILES ${plugin_loc}
+					DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+					COMPONENT runtime)
+				#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+			ENDIF(EXISTS ${plugin_loc})
+		ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+	ENDIF(WIN32)
  ENDIF()
  
  # install a qt.conf file
@@ -185,5 +206,5 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
    include(\"BundleUtilities\")
    fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
  " COMPONENT runtime)
-ENDIF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
  
@@ -46,7 +46,7 @@ public:
 	}
 	virtual ~ObjDeletionHandler() {}

-signals:
+Q_SIGNALS:
 	void objDeletionEventReceived(int);

 protected:
@@ -55,7 +55,7 @@ protected:
 		if(event->getClassName().compare("UObjDeletedEvent") == 0 &&
 		   event->getCode() == _watchedId)
 		{
-			emit objDeletionEventReceived(_watchedId);
+			Q_EMIT objDeletionEventReceived(_watchedId);
 		}
 		return false;
 	}
@@ -43,13 +43,13 @@ int main(int argc, char* argv[])
 {
 	/* Set logger type */
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setLevel(ULogger::kWarning);

 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box

-	ParametersMap parameters = Parameters::parseArguments(argc, argv, true);
+	ParametersMap parameters = Parameters::parseArguments(argc, argv, false);
 	MainWindow * mainWindow = new MainWindow();
    app->installEventFilter(mainWindow); // to catch FileOpen events.
    
@@ -61,11 +61,10 @@ int main(int argc, char* argv[])
           UFile::getExtension(value).compare("db") == 0)
        {
            database = value;
-            break;
        }
    }

-	UINFO("Program started...");
+	printf("Program started...\n");

 	UEventsManager::addHandler(mainWindow);

@@ -85,9 +84,9 @@ int main(int argc, char* argv[])
    
    if(!database.empty())
    {
-    	mainWindow->openDatabase(database.c_str());
+    	mainWindow->openDatabase(database.c_str(), parameters);
    }
-    if(parameters.size())
+    else if(parameters.size())
    {
    	mainWindow->updateParameters(parameters);
    }
@@ -101,14 +100,13 @@ int main(int argc, char* argv[])
 	UEventsManager::removeHandler(mainWindow);
 	UEventsManager::removeHandler(rtabmap);

-	UINFO("Killing threads...");
 	rtabmap->join(true);

-	UINFO("Closing RTAB-Map...");
+	printf("Closing RTAB-Map...\n");
 	delete rtabmap;
 	delete mainWindow;
 	delete app;
-	UINFO("All done!");
+	printf("All done!\n");

    return 0;
 }
@@ -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)
-find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 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)
-find_library(Triclops_LIBRARY NAMES triclops triclops_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 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
@@ -17,6 +17,14 @@ FIND_LIBRARY(CHOLMOD_LIB cholmod)
 FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 
  PATHS "C:\\Program Files\\g2o\\include")

+FIND_FILE(G2O_CONFIG_FILE g2o/config.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
+#ifdef G2O_NUMBER_FORMAT_STR
+#define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
+#endif
+
 # Macro to unify finding both the debug and release versions of the
 # libraries; this is adapted from the rtabmap config

@@ -75,7 +83,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER

 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
  SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
  SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -105,5 +113,15 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	  ${CHOLMOD_LIB})
  ENDIF(G2O_SOLVER_CHOLMOD)

+  FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
+  STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
+  IF(${matchres} EQUAL -1)
+    MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
+    SET(G2O_CPP11 0)
+  ELSE()
+    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
+    SET(G2O_CPP11 1)
+  ENDIF()
+
  SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
@@ -0,0 +1,183 @@
+#.rst:
+# FindKinectSDK2
+# --------------
+#
+# Find Kinect for Windows SDK v2 (Kinect SDK v2) include dirs, library dirs, libraries
+#
+# Use this module by invoking find_package with the form::
+#
+#    find_package( KinectSDK2 [REQUIRED] )
+#
+# Results for users are reported in following variables::
+#
+#    KinectSDK2_FOUND                - Return "TRUE" when Kinect SDK v2 found. Otherwise, Return "FALSE".
+#    KinectSDK2_INCLUDE_DIRS         - Kinect SDK v2 include directories. (${KinectSDK2_DIR}/inc)
+#    KinectSDK2_LIBRARY_DIRS         - Kinect SDK v2 library directories. (${KinectSDK2_DIR}/Lib/x86 or ${KinectSDK2_DIR}/Lib/x64)
+#    KinectSDK2_LIBRARIES            - Kinect SDK v2 library files. (${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib (If check the box of any application festures, corresponding library will be added.))
+#    KinectSDK2_COMMANDS             - Copy commands of redist files for application functions of Kinect SDK v2. (If uncheck the box of all application features, this variable has defined empty command.)
+#
+# This module reads hints about search locations from following environment variables::
+#
+#    KINECTSDK20_DIR                 - Kinect SDK v2 root directory. (This environment variable has been set by installer of Kinect SDK v2.)
+#
+# CMake entries::
+#
+#    KinectSDK2_DIR                  - Kinect SDK v2 root directory. (Default $ENV{KINECTSDK20_DIR})
+#    KinectSDK2_FACE                 - Check the box when using Face or HDFace features. (Default uncheck)
+#    KinectSDK2_FUSION               - Check the box when using Fusion features. (Default uncheck)
+#    KinectSDK2_VGB                  - Check the box when using Visual Gesture Builder features. (Default uncheck)
+#
+# Example to find Kinect SDK v2::
+#
+#    cmake_minimum_required( VERSION 2.8 )
+#
+#    project( project )
+#    add_executable( project main.cpp )
+#    set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "project" )
+#
+#    # Find package using this module.
+#    find_package( KinectSDK2 REQUIRED )
+#
+#    if(KinectSDK2_FOUND)
+#      # [C/C++]>[General]>[Additional Include Directories]
+#      include_directories( ${KinectSDK2_INCLUDE_DIRS} )
+#
+#      # [Linker]>[General]>[Additional Library Directories]
+#      link_directories( ${KinectSDK2_LIBRARY_DIRS} )
+#
+#      # [Linker]>[Input]>[Additional Dependencies]
+#      target_link_libraries( project ${KinectSDK2_LIBRARIES} )
+#
+#      # [Build Events]>[Post-Build Event]>[Command Line]
+#      add_custom_command( TARGET project POST_BUILD ${KinectSDK2_COMMANDS} )
+#    endif()
+#
+# =============================================================================
+#
+# Copyright (c) 2016 Tsukasa SUGIURA
+# Distributed under the MIT License.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+#
+# =============================================================================
+
+##### Utility #####
+
+# Check Directory Macro
+macro(CHECK_DIR _DIR)
+  if(NOT EXISTS "${${_DIR}}")
+    message(WARNING "Directory \"${${_DIR}}\" not found.")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_DIR)
+  endif()
+endmacro()
+
+# Check Files Macro
+macro(CHECK_FILES _FILES _DIR)
+  set(_MISSING_FILES)
+  foreach(_FILE ${${_FILES}})
+    if(NOT EXISTS "${_FILE}")
+      get_filename_component(_FILE ${_FILE} NAME)
+      set(_MISSING_FILES "${_MISSING_FILES}${_FILE}, ")
+    endif()
+  endforeach()
+  if(_MISSING_FILES)
+    message(WARNING "In directory \"${${_DIR}}\" not found files: ${_MISSING_FILES}")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_FILES)
+  endif()
+endmacro()
+
+# Target Platform
+set(TARGET_PLATFORM)
+if(NOT CMAKE_CL_64)
+  set(TARGET_PLATFORM x86)
+else()
+  set(TARGET_PLATFORM x64)
+endif()
+
+##### Find Kinect SDK v2 #####
+
+# Found
+set(KinectSDK2_FOUND TRUE)
+if(MSVC_VERSION LESS 1700)
+  message(WARNING "Kinect for Windows SDK v2 supported Visual Studio 2012 or later.")
+  set(KinectSDK2_FOUND FALSE)
+endif()
+
+# Options
+option(KinectSDK2_FACE "Face and HDFace features" FALSE)
+option(KinectSDK2_FUSION "Fusion features" FALSE)
+option(KinectSDK2_VGB "Visual Gesture Builder features" FALSE)
+
+# Root Directoty
+set(KinectSDK2_DIR)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_DIR $ENV{KINECTSDK20_DIR} CACHE PATH "Kinect for Windows SDK v2 Install Path." FORCE)
+  check_dir(KinectSDK2_DIR)
+endif()
+
+# Include Directories
+set(KinectSDK2_INCLUDE_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_INCLUDE_DIRS ${KinectSDK2_DIR}/inc)
+  check_dir(KinectSDK2_INCLUDE_DIRS)
+endif()
+
+# Library Directories
+set(KinectSDK2_LIBRARY_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARY_DIRS ${KinectSDK2_DIR}/Lib/${TARGET_PLATFORM})
+  check_dir(KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Dependencies
+set(KinectSDK2_LIBRARIES)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib)
+
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Face.lib)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Fusion.lib)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.VisualGestureBuilder.lib)
+  endif()
+
+  check_files(KinectSDK2_LIBRARIES KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Custom Commands
+set(KinectSDK2_COMMANDS)
+if(KinectSDK2_FOUND)
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Face/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Fusion/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/VGB/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  # Empty Commands
+  if(NOT KinectSDK2_COMMANDS)
+    set(KinectSDK2_COMMANDS COMMAND)
+  endif()
+endif()
+
+message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")
@@ -0,0 +1,33 @@
+# - Find ORB_SLAM2 
+#
+# It sets the following variables:
+#  ORB_SLAM2_FOUND         - Set to false, or undefined, if ORB_SLAM2 isn't found.
+#  ORB_SLAM2_INCLUDE_DIRS  - The ORB_SLAM2 include directory.
+#  ORB_SLAM2_LIBRARIES     - The ORB_SLAM2 library to link against.
+#
+#  Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder.
+
+find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
+find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
+find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
+find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
+
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+   SET(ORB_SLAM2_FOUND TRUE)
+   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
+   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY})
+ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+
+IF (ORB_SLAM2_FOUND)
+   # show which ORB_SLAM2 was found only if not quiet
+   IF (NOT ORB_SLAM2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}")
+   ENDIF (NOT ORB_SLAM2_FIND_QUIETLY)
+ELSE (ORB_SLAM2_FOUND)
+   # fatal error if ORB_SLAM2 is required but not found
+   IF (ORB_SLAM2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2")
+   ENDIF (ORB_SLAM2_FIND_REQUIRED)
+ENDIF (ORB_SLAM2_FOUND)
+
@@ -0,0 +1,41 @@
+# - Find librealsense (https://github.com/IntelRealSense/librealsense)
+#
+#  RealSense2_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  RealSense2_FOUND         - Set to false, or undefined, if RealSense2 isn't found.
+#  RealSense2_INCLUDE_DIRS  - The RealSense2 include directory.
+#  RealSense2_LIBRARIES     - The RealSense2 library to link against.
+
+#RealSense library
+
+find_path(RealSense2_INCLUDE_DIRS NAMES librealsense2/rs.hpp PATHS $ENV{RealSense2_ROOT_DIR}/include)
+if(CMAKE_CL_64)
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x64  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x64)
+else()
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x86  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x86)
+endif()
+
+IF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+   SET(RealSense2_FOUND TRUE)
+ENDIF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+
+IF (RealSense2_FOUND)
+   SET(RealSense2_LIBRARIES ${RealSense2_LIBRARY})
+   
+   # Compatibility with linux names
+   SET(realsense2_LIBRARIES ${RealSense2_LIBRARIES})
+   SET(realsense2_INCLUDE_DIRS ${RealSense2_INCLUDE_DIRS})
+   SET(realsense2_FOUND ${RealSense2_FOUND})
+  
+   # show which RealSense was found only if not quiet
+   IF (NOT RealSense2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RealSense: ${RealSense2_LIBRARIES}")
+   ENDIF (NOT RealSense2_FIND_QUIETLY)
+ELSE (RealSense2_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (RealSense2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RealSense2 (librealsense2)")
+   ENDIF (RealSense2_FIND_REQUIRED)
+ENDIF (RealSense2_FOUND)
+
@@ -2,46 +2,29 @@
 # This module finds an installed Sqlite3 package.
 #
 # It sets the following variables:
-#  SQLITE3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  SQLITE3_INCLUDE_DIR - The Sqlite3 include directory.
-#  SQLITE3_LIBRARY     - The Sqlite3 library to link against.
+#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
+#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
+#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.

-SET(SQLITE3_VERSION_REQUIRED "3.6.0")
+FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})

-IF(UNIX)
-	FIND_PROGRAM(SQLITE3_EXEC NAME sqlite3 PATHS)  
-	IF(SQLITE3_EXEC)  
-        MESSAGE(STATUS "Found Sqlite3 executable : ${SQLITE3_EXEC}")
-		EXECUTE_PROCESS(COMMAND ${SQLITE3_EXEC} --version 
-				   	    OUTPUT_VARIABLE SQLITE3_VERSION
-						OUTPUT_STRIP_TRAILING_WHITESPACE
-						WORKING_DIRECTORY "./"
-   		)
-		IF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-		    MESSAGE(FATAL_ERROR "Sqlite ${SQLITE3_VERSION} found, but version ${SQLITE3_VERSION_REQUIRED} minimum is required")
-		ENDIF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-	ELSE(SQLITE3_EXEC)
-		MESSAGE(FATAL_ERROR "Could not find Sqlite3 executable")
-	ENDIF(SQLITE3_EXEC)
-ENDIF(UNIX)
+FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})

-FIND_PATH(SQLITE3_INCLUDE_DIR sqlite3.h)
+IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
+   SET(Sqlite3_FOUND TRUE)
+   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
+   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
+ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)

-FIND_LIBRARY(SQLITE3_LIBRARY NAMES sqlite3.dll sqlite3)
-
-IF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-   SET(SQLITE3_FOUND TRUE)
-ENDIF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-
-IF (SQLITE3_FOUND)
+IF (Sqlite3_FOUND)
   # show which Sqlite3 was found only if not quiet
   IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3")
+      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
   ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (SQLITE3_FOUND)
+ELSE (Sqlite3_FOUND)
   # fatal error if Sqlite3 is required but not found
   IF (Sqlite3_FIND_REQUIRED)
      MESSAGE(FATAL_ERROR "Could not find Sqlite3")
   ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (SQLITE3_FOUND)
+ENDIF (Sqlite3_FOUND)

@@ -59,18 +59,14 @@ public:
 	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
 	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}

-	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
+	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);

 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,
 			const std::vector<int> & oldIds,
-			const std::vector<int> & newIds) const;
+			const std::vector<int> & newIds);
 	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
-	float addNeighborProb(cv::Mat & prediction,
-			unsigned int col,
-			const std::map<int, int> & neighbors,
-			const std::map<int, int> & idToIndexMap) const;
 	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;

 private:
@@ -80,6 +76,8 @@ 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;
 };

 } // namespace rtabmap
@@ -66,6 +66,7 @@ public:
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}

+	void resetTimer();
 protected:
 	/**
 	 * Constructor
@@ -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),
@@ -59,6 +60,7 @@ public:
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
+	float timeStereoExposureCompensation;
 	float timeImageDecimation;
 	float timeScanFromDepth;
 	float timeUndistortDepth;
@@ -66,6 +68,7 @@ public:
 	float timeTotal;
 	Transform odomPose;
 	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
 };

 } // namespace rtabmap
@@ -75,6 +75,7 @@ public:
 	virtual ~CameraModel() {}

 	void initRectificationMap();
+	bool isRectificationMapInitialized() {return !mapX_.empty() && !mapY_.empty();}

 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
@@ -114,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;
@@ -27,207 +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;}
-	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
-			const Transform & localTransform=Transform::getIdentity())
-	{
-		_scanPath = dir;
-		_scanLocalTransform = localTransform;
-		_scanMaxPts = maxScanPts;
-		_scanDownsampleStep = downsampleStep;
-		_scanNormalsK = normalsK;
-		_scanVoxelSize = voxelSize;
-		if(_scanDownsampleStep>1)
-		{
-			_scanMaxPts /= _scanDownsampleStep;
-		}
-	}
-
-	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 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) 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;
-
-	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;
-
-	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>
@@ -27,347 +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
-#include <pcl/io/openni_camera/openni_depth_image.h>
-#include <pcl/io/openni_camera/openni_image.h>
-#endif
-
-#include <boost/signals2/connection.hpp>
-
-namespace openni
-{
-class Device;
-class VideoStream;
-}
-
-namespace pcl
-{
-class Grabber;
-}
-
-namespace libfreenect2
-{
-class Freenect2;
-class Freenect2Device;
-class SyncMultiFrameListener;
-class Registration;
-class PacketPipeline;
-}
-
-namespace rs
-{
-	class context;
-	class device;
-	namespace slam {
-	class slam;
-	}
-}
-
-typedef struct _freenect_context freenect_context;
-typedef struct _freenect_device freenect_device;
-
-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 HAVE_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);
-
-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;
-#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 = kTypeColor2DepthSD,
-					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);
-	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_;
-#endif
-};
-
-/////////////////////////
-// CameraRealSense
-/////////////////////////
-class slam_event_handler;
-class RTABMAP_EXP CameraRealSense :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraRealSense(
-		int deviceId = 0,
-		int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		bool computeOdometry = false,
-		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRealSense();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_REALSENSE
-	rs::context * ctx_;
-	rs::device * dev_;
-	int deviceId_;
-	int presetRGB_;
-	int presetDepth_;
-	bool computeOdometry_;
-
-	int motionSeq_[2];
-	rs::slam::slam * slam_;
-	UMutex slamLock_;
-
-	std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
-	std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
-	UMutex dataMutex_;
-	USemaphore dataReady_;
-#endif
-};
-
-
-/////////////////////////
-// CameraRGBDImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraRGBDImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraRGBDImages(
-			const std::string & pathRGBImages,
-			const std::string & pathDepthImages,
-			float depthScaleFactor = 1.0f,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRGBDImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages cameraDepth_;
-};
-
-} // namespace rtabmap
@@ -27,222 +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 = false);
-	CameraStereoZed(
-			const std::string & svoFilePath,
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 0,// 0=STANDARD, 1=FILL
-			int confidenceThr = 100,
-			bool computeOdometry = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
-	virtual ~CameraStereoZed();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_ZED
-	sl::Camera * zed_;
-	StereoCameraModel stereoModel_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-	std::string svoFilePath_;
-	int resolution_;
-	int quality_;
-	bool selfCalibration_;
-	int sensingMode_;
-	int confidenceThr_;
-	bool computeOdometry_;
-	bool lost_;
-#endif
-};
-
-/////////////////////////
-// CameraStereoImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoImages(
-			const std::string & pathLeftImages,
-			const std::string & pathRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoImages(
-			const std::string & pathLeftRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages * camera2_;
-	StereoCameraModel stereoModel_;
-};
-
-
-/////////////////////////
-// CameraStereoVideo
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoVideo :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoVideo(
-			const std::string & pathSideBySide,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			const std::string & pathLeft,
-			const std::string & pathRight,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			int device,
-			bool rectifyImages = false,
-			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoVideo();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	cv::VideoCapture capture_;
-	cv::VideoCapture capture2_;
-	std::string path_;
-	std::string path2_;
-	bool rectifyImages_;
-	StereoCameraModel stereoModel_;
-	std::string cameraName_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-};
-
-} // namespace rtabmap
+#include <rtabmap/core/camera/CameraStereoDC1394.h>
+#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
+#include <rtabmap/core/camera/CameraStereoImages.h>
+#include <rtabmap/core/camera/CameraStereoVideo.h>
+#include <rtabmap/core/camera/CameraStereoZed.h>
+#include <rtabmap/core/camera/CameraStereoTara.h>
@@ -45,6 +45,7 @@ class Camera;
 class CameraInfo;
 class SensorData;
 class StereoDense;
+class IMUFilter;

 /**
 * Class CameraThread
@@ -60,6 +61,7 @@ public:
 	virtual ~CameraThread();

 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
@@ -67,19 +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 normalsK = 0,
+			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;
@@ -98,20 +108,24 @@ private:
 private:
 	Camera * _camera;
 	bool _mirroring;
+	bool _stereoExposureCompensation;
 	bool _colorOnly;
 	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
@@ -83,5 +83,8 @@ cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);

+cv::Mat RTABMAP_EXP compressString(const std::string & str);
+std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */
@@ -68,6 +68,7 @@ public:
 	virtual ~DBDriver();

 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return _url.empty();}
 	const std::string & getUrl() const {return _url;}

 	void beginTransaction() const;
@@ -91,6 +92,7 @@ public:
 				int nodeId,
 				const cv::Mat & ground,
 				const cv::Mat & obstacles,
+				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
@@ -98,6 +100,29 @@ public:
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
 	void addStatistics(const Statistics & statistics) const;
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose = 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(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
+	cv::Mat loadOptimizedMesh(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;

 public:
 	// Mutex-protected methods of abstract versions below
@@ -107,22 +132,30 @@ public:
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
+	long getNodesMemoryUsed() const;
+	long getLinksMemoryUsed() const;
 	long getImagesMemoryUsed() const;
 	long getDepthImagesMemoryUsed() const;
+	long getCalibrationsMemoryUsed() const;
+	long getGridsMemoryUsed() const;
 	long getLaserScansMemoryUsed() const;
 	long getUserDataMemoryUsed() const;
 	long getWordsMemoryUsed() const;
+	long getFeaturesMemoryUsed() const;
+	long getStatisticsMemoryUsed() const;
 	int getLastNodesSize() const; // working memory
 	int getLastDictionarySize() const; // working memory
 	int getTotalNodesSize() const;
 	int getTotalDictionarySize() const;
 	ParametersMap getLastParameters() const;
-	std::map<std::string, float> getStatistics(int nodeId, double & stamp) const;
+	std::map<std::string, float> getStatistics(int nodeId, double & stamp, std::vector<int> * wmState=0) const;
+	std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatistics() const;
+	std::map<int, std::vector<int> > getAllStatisticsWmStates() const;

 	void executeNoResult(const std::string & sql) const;

 	// Load objects
-	void load(VWDictionary * dictionary) const;
+	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	void loadLastNodes(std::list<Signature *> & signatures) const;
 	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
@@ -131,43 +164,53 @@ public:
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
-	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) 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, 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;

 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());

-private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
+	virtual long getNodesMemoryUsedQuery() const = 0;
+	virtual long getLinksMemoryUsedQuery() const = 0;
 	virtual long getImagesMemoryUsedQuery() const = 0;
 	virtual long getDepthImagesMemoryUsedQuery() const = 0;
+	virtual long getCalibrationsMemoryUsedQuery() const = 0;
+	virtual long getGridsMemoryUsedQuery() const = 0;
 	virtual long getLaserScansMemoryUsedQuery() const = 0;
 	virtual long getUserDataMemoryUsedQuery() const = 0;
 	virtual long getWordsMemoryUsedQuery() const = 0;
+	virtual long getFeaturesMemoryUsedQuery() const = 0;
+	virtual long getStatisticsMemoryUsedQuery() const = 0;
 	virtual int getLastNodesSizeQuery() const = 0;
 	virtual int getLastDictionarySizeQuery() const = 0;
 	virtual int getTotalNodesSizeQuery() const = 0;
 	virtual int getTotalDictionarySizeQuery() const = 0;
 	virtual ParametersMap getLastParametersQuery() const = 0;
-	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const = 0;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const = 0;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const = 0;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const = 0;

 	virtual void executeNoResultQuery(const std::string & sql) const = 0;

 	virtual void getWeightQuery(int signatureId, int & weight) const = 0;

-	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void saveQuery(const std::list<Signature *> & signatures) = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
@@ -179,6 +222,7 @@ private:
 				int nodeId,
 				const cv::Mat & ground,
 				const cv::Mat & obstacles,
+				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint) const = 0;

@@ -187,27 +231,53 @@ private:
 					const cv::Mat & image) const = 0;

 	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
+	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 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(
+				const cv::Mat & cloud,
+				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+				const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+				const cv::Mat & textures) const = 0;
+	virtual cv::Mat loadOptimizedMeshQuery(
+				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+				cv::Mat * textures) const = 0;

 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const = 0;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
 	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 getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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, 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;

 private:
 	//non-abstract methods
-	void saveOrUpdate(const std::vector<Signature *> & signatures) const;
+	void saveOrUpdate(const std::vector<Signature *> & signatures);
 	void saveOrUpdate(const std::vector<VisualWord *> & words) const;

 	//thread stuff
@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
-#include "sqlite3/sqlite3.h"
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;

 namespace rtabmap {

@@ -41,35 +43,44 @@ public:
 	virtual ~DBDriverSqlite3();

 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return getUrl().empty() || _dbInMemory;}
 	void setDbInMemory(bool dbInMemory);
 	void setJournalMode(int journalMode);
 	void setCacheSize(unsigned int cacheSize);
 	void setSynchronous(int synchronous);
 	void setTempStore(int tempStore);

-private:
+protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
+	virtual long getNodesMemoryUsedQuery() const;
+	virtual long getLinksMemoryUsedQuery() const;
 	virtual long getImagesMemoryUsedQuery() const;
 	virtual long getDepthImagesMemoryUsedQuery() const;
+	virtual long getCalibrationsMemoryUsedQuery() const;
+	virtual long getGridsMemoryUsedQuery() const;
 	virtual long getLaserScansMemoryUsedQuery() const;
 	virtual long getUserDataMemoryUsedQuery() const;
 	virtual long getWordsMemoryUsedQuery() const;
+	virtual long getFeaturesMemoryUsedQuery() const;
+	virtual long getStatisticsMemoryUsedQuery() const;
 	virtual int getLastNodesSizeQuery() const;
 	virtual int getLastDictionarySizeQuery() const;
 	virtual int getTotalNodesSizeQuery() const;
 	virtual int getTotalDictionarySizeQuery() const;
 	virtual ParametersMap getLastParametersQuery() const;
-	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const;

 	virtual void executeNoResultQuery(const std::string & sql) const;

 	virtual void getWeightQuery(int signatureId, int & weight) const;

-	virtual void saveQuery(const std::list<Signature *> & signatures) const;
+	virtual void saveQuery(const std::list<Signature *> & signatures);
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
@@ -81,6 +92,7 @@ private:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;

@@ -89,21 +101,47 @@ private:
 			const cv::Mat & image) const;

 	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const;
+	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 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(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+			const cv::Mat & textures) const;
+	virtual cv::Mat loadOptimizedMeshQuery(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+			cv::Mat * textures) const;

 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	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 getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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, 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;

@@ -119,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;
@@ -133,6 +168,7 @@ private:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;

@@ -140,9 +176,12 @@ private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;

-private:
+protected:
 	sqlite3 * _ppDb;
 	std::string _version;
+
+private:
+	long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;
@@ -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 */
@@ -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_ */
@@ -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
-		kFeatureFreak=9};   //new 0.11.14
+		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:
@@ -177,6 +179,8 @@ private:
 	int _subPixWinSize;
 	int _subPixIterations;
 	double _subPixEps;
+	int gridRows_;
+	int gridCols_;
 	// Stereo stuff
 	Stereo * _stereo;
 };
@@ -192,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:
@@ -219,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:
@@ -242,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:
@@ -273,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:
@@ -341,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;
@@ -422,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:
@@ -433,27 +437,55 @@ private:
 	cv::Ptr<CV_BRISK> brisk_;
 };

-//FREAK
-class RTABMAP_EXP FREAK : public Feature2D
+//KAZE
+class RTABMAP_EXP KAZE : public Feature2D
 {
 public:
-	FREAK(const ParametersMap & parameters = ParametersMap());
-	virtual ~FREAK();
+	KAZE(const ParametersMap & parameters = ParametersMap());
+	virtual ~KAZE();

 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const { return kFeatureFreak; }
+	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:
-	bool orientationNormalized_;
-	bool scaleNormalized_;
-	float patternScale_;
+	bool extended_;
+	bool upright_;
+	float threshold_;
 	int nOctaves_;
+	int nOctaveLayers_;
+	int diffusivity_;

-	cv::Ptr<CV_FREAK> _freak;
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<cv::KAZE> kaze_;
+#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_;
 };


@@ -49,21 +49,25 @@ public:
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
 			const cv::Mat & features,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildKDTreeIndex(
 			const cv::Mat & features,
 			int trees = 4,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildKDTreeSingleIndex(
 			const cv::Mat & features,
 			int leafMaxSize = 10,
 			bool reorder = true,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildLSHIndex(
 			const cv::Mat & features,
 			unsigned int table_number = 12,
 			unsigned int key_size = 20,
-			unsigned int multi_probe_level = 2);
+			unsigned int multi_probe_level = 2,
+			float rebalancingFactor = 2.0f);

 	bool isBuilt();

@@ -74,7 +78,7 @@ public:

 	void removePoint(unsigned int index);

-	// return squared distances
+	// return squared distances (indices should be casted in size_t)
 	void knnSearch(
 			const cv::Mat & query,
 			cv::Mat & indices,
@@ -102,6 +106,7 @@ private:
 	int featuresDim_;
 	bool isLSH_;
 	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
+	float rebalancingFactor_;

 	// keep feature in memory until the tree is rebuilt
 	// (in case the word is deleted when removed from the VWDictionary)
@@ -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_ */
@@ -73,18 +73,22 @@ public:

 	void apply(
 			int id,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) const;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			bool rgb = true) const;
 	void apply(
 			int id,
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-			const pcl::IndicesPtr & indices) const;
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
 	void apply(
 			int id,
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-			const pcl::IndicesPtr & indices) const;
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
 	void apply(
 			int id,
-			cv::Mat & image) const;
+			cv::Mat & image,
+			bool rgb = true) const;

 	double getGain(int id, double * r=0, double * g=0, double * b=0) const;
 	int getIndex(int id) const;
@@ -25,6 +25,20 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+/*
+ * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
+ * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
+ */
+
+/* +---------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)               |
+   |                          http://www.mrpt.org/                             |
+   |                                                                           |
+   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
+   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
+   | Released under BSD License. See details in http://www.mrpt.org/License    |
+   +---------------------------------------------------------------------------+ */
+

 #ifndef GEODETICCOORDS_H_
 #define GEODETICCOORDS_H_
@@ -52,6 +66,15 @@ public:
 	cv::Point3d toGeocentric_WGS84() const;
 	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y

+	void fromGeocentric_WGS84(const cv::Point3d& geocentric);
+	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
 	double longitude_; // deg
@@ -32,7 +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/GPS.h>

 namespace rtabmap {
 class Memory;
@@ -45,19 +47,87 @@ namespace graph {

 	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

+bool RTABMAP_EXP exportGPS(
+		const std::string & filePath,
+		const std::map<int, GPS> & gpsValues,
+		unsigned int rgba = 0xFFFFFFFF);
+
+/**
+ * Compute translation and rotation errors for KITTI datasets.
+ * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
+ * @param poses_gt, Ground Truth poses
+ * @param poses_result, Estimated poses
+ * @param t_err, Output translation error (%)
+ * @param r_err, Output rotation error (deg/m)
+ */
+void RTABMAP_EXP calcKittiSequenceErrors(
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		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).
+ * See https://vision.in.tum.de/data/datasets/rgbd-dataset
+ * @param groundTruth, Ground Truth poses
+ * @param poses, Estimated poses
+ * @return Gt to Map transform
+ */
+Transform RTABMAP_EXP calcRMSE(
+		const std::map<int, Transform> &groundTruth,
+		const std::map<int, Transform> &poses,
+		float & translational_rmse,
+		float & translational_mean,
+		float & translational_median,
+		float & translational_std,
+		float & translational_min,
+		float & translational_max,
+		float & rotational_rmse,
+		float & rotational_mean,
+		float & rotational_median,
+		float & rotational_std,
+		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,
@@ -72,13 +142,19 @@ 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);
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType);
@@ -183,6 +259,11 @@ int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose);

+std::vector<int> RTABMAP_EXP findNearestNodes(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose,
+		int k);
+
 /**
 * Get nodes near the query
 * @param nodeId the query id
@@ -205,6 +286,10 @@ float RTABMAP_EXP computePathLength(
 		unsigned int fromIndex = 0,
 		unsigned int toIndex = 0);

+// assuming they are all linked in map order
+float RTABMAP_EXP computePathLength(
+		const std::map<int, Transform> & path);
+
 std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);
@@ -0,0 +1,104 @@
+/*
+ * IMU.h
+ *
+ *  Created on: 2018-03-05
+ *      Author: mathieu
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include <opencv2/core/core.hpp>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+
+// Correspondence class to sensor_msgs/IMU
+class IMU
+{
+public:
+	IMU() {}
+	IMU(const cv::Vec4d & orientation, // qx qy qz qw
+		const cv::Mat & orientationCovariance,
+		const cv::Vec3d & angularVelocity,
+		const cv::Mat & angularVelocityCovariance,
+		const cv::Vec3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			orientation_(orientation),
+			orientationCovariance_(orientationCovariance),
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			linearAccelerationCovariance_(linearAccelerationCovariance),
+			localTransform_(localTransform)
+	{
+	}
+	IMU(const cv::Vec3d & angularVelocity,
+		const cv::Mat & angularVelocityCovariance,
+		const cv::Vec3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			linearAccelerationCovariance_(linearAccelerationCovariance),
+			localTransform_(localTransform)
+	{
+	}
+
+	// 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
+
+	const cv::Vec3d & angularVelocity() const {return angularVelocity_;}
+	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;} // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	const cv::Vec3d linearAcceleration() const {return linearAcceleration_;}
+	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;} // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	const Transform & localTransform() const {return localTransform_;}
+
+	bool empty() const
+	{
+		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
+
+	cv::Vec3d angularVelocity_;
+	cv::Mat angularVelocityCovariance_; // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	cv::Vec3d linearAcceleration_;
+	cv::Mat linearAccelerationCovariance_; // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	Transform localTransform_;
+};
+
+class IMUEvent : public UEvent
+{
+public:
+	IMUEvent() :
+		stamp_(0.0)
+	{}
+	IMUEvent(const IMU & data, double stamp) :
+		data_(data),
+		stamp_(stamp)
+	{
+	}
+	virtual std::string getClassName() const {return "IMUEvent";}
+	const IMU & getData() const {return data_;}
+	double getStamp() const {return stamp_;}
+
+private:
+	IMU data_;
+	double stamp_;
+};
+
+}
+
+
+#endif /* IMU_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_ */
@@ -0,0 +1,70 @@
+/*
+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/Transform.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <fstream>
+
+namespace rtabmap
+{
+
+/**
+ * Class IMUThread
+ *
+ */
+class RTABMAP_EXP IMUThread :
+	public UThread,
+	public UEventsSender
+{
+public:
+	IMUThread(int rate, const Transform & localTransform);
+	virtual ~IMUThread();
+
+	bool init(const std::string & path);
+	void setRate(int rate);
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+private:
+	int rate_;
+	Transform localTransform_;
+	std::ifstream imuFile_;
+	UTimer frameRateTimer_;
+	double captureDelay_;
+	double previousStamp_;
+};
+
+} // namespace rtabmap
@@ -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_ */
@@ -0,0 +1,130 @@
+/*
+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 CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP LaserScan
+{
+public:
+	enum Format{kUnknown=0,
+		kXY=1,
+		kXYI=2,
+		kXYNormal=3,
+		kXYINormal=4,
+		kXYZ=5,
+		kXYZI=6,
+		kXYZRGB=7,
+		kXYZNormal=8,
+		kXYZINormal=9,
+		kXYZRGBNormal=10};
+
+	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,
+			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,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity());
+
+	const cv::Mat & data() const {return data_;}
+	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();}
+	int size() const {return data_.cols;}
+	int dataType() const {return data_.type();}
+	bool is2d() const {return isScan2d(format_);}
+	bool hasNormals() const {return isScanHasNormals(format_);}
+	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;
+
+	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
+	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
+	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
+
+	void clear() {data_ = cv::Mat();}
+
+private:
+	cv::Mat data_;
+	Format format_;
+	int maxPoints_;
+	float rangeMin_;
+	float rangeMax_;
+	float angleMin_;
+	float angleMax_;
+	float angleIncrement_;
+	Transform localTransform_;
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ */
@@ -46,23 +46,23 @@ public:
 		kUserClosure,
 		kVirtualClosure,
 		kNeighborMerged,
-		kUndef};
+		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,
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
-			const cv::Mat & userData = cv::Mat());
-	Link(int from,
-			int to,
-			Type type,
-			const Transform & transform,
-			double rotVariance,
-			double transVariance,
+			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_;}
@@ -87,7 +87,6 @@ public:

 private:
 	void setInfMatrix(const cv::Mat & infMatrix);
-	void setVariance(double rotVariance, double transVariance);

 private:
 	int from_;
@@ -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_ */
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <pcl/pcl_config.h>

 namespace rtabmap {

@@ -56,6 +57,7 @@ class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
 class OccupancyGrid;
+class MarkerDetector;

 class RTABMAP_EXP Memory
 {
@@ -75,6 +77,7 @@ public:
 	bool update(const SensorData & data,
 			const Transform & pose,
 			const cv::Mat & covariance,
+			const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
 			Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
@@ -91,6 +94,29 @@ public:

 	int cleanup();
 	void saveStatistics(const Statistics & statistics);
+	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;
+	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(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
+	cv::Mat loadOptimizedMesh(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
 	void emptyTrash();
 	void joinTrashThread();
 	bool addLink(const Link & link, bool addInDatabase = false);
@@ -104,6 +130,8 @@ public:
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			bool ignoreIntermediateNodes = false,
+			bool ignoreLocalSpaceLoopIds = false,
+			const std::set<int> & nodesSet = std::set<int>(),
 			double * dbAccessTime = 0) const;
 	std::map<int, float> getNeighborsIdRadius(
 			int signatureId,
@@ -111,6 +139,7 @@ public:
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
+	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);

@@ -118,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.
@@ -143,9 +178,13 @@ public:
 	bool setUserData(int id, const cv::Mat & data);
 	int getDatabaseMemoryUsed() const; // in bytes
 	std::string getDatabaseVersion() const;
+	std::string getDatabaseUrl() const;
 	double getDbSavingTime() const;
+	int getMapId(int id, bool lookInDatabase = false) const;
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
 	Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
+	const std::map<int, Transform> & getGroundTruths() const {return _groundTruths;} // only those in working+STM memory
+	void getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth = 0) const;
 	bool getNodeInfo(int signatureId,
 			Transform & odomPose,
 			int & mapId,
@@ -153,6 +192,9 @@ public:
 			std::string & label,
 			double & stamp,
 			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;
@@ -175,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;
@@ -191,11 +234,11 @@ 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);
-	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(
 			int newId,
 			int oldId,
@@ -244,6 +287,8 @@ private:
 	bool _rawDescriptorsKept;
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
+	bool _saveIntermediateNodeData;
+	std::string _rgbCompressionFormat;
 	bool _incrementalMemory;
 	bool _reduceGraph;
 	int _maxStMemSize;
@@ -253,18 +298,29 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
+	bool _depthAsMask;
 	int _imagePreDecimation;
 	int _imagePostDecimation;
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
+	float _laserScanVoxelSize;
 	int _laserScanNormalK;
+	float _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
+	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
+	bool _useOdometryGravity;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
+	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
+	bool _covOffDiagonalIgnored;
+	bool _detectMarkers;
+	float _markerLinVariance;
+	float _markerAngVariance;

 	int _idCount;
 	int _idMapCount;
@@ -273,10 +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;
@@ -286,9 +351,11 @@ private:
 	bool _parallelized;

 	Registration * _registrationPipeline;
-	RegistrationIcp * _registrationIcp;
+	RegistrationIcp * _registrationIcpMulti;

 	OccupancyGrid * _occupancy;
+
+	MarkerDetector * _markerDetector;
 };

 } // namespace rtabmap
@@ -39,16 +39,32 @@ namespace rtabmap {

 class RTABMAP_EXP OccupancyGrid
 {
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
+	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
+	void setCloudAssembling(bool enabled);
 	float getMinMapSize() const {return minMapSize_;}
-	bool isGridFromDepth() const {return occupancyFromCloud_;}
+	bool isGridFromDepth() const {return occupancyFromDepth_;}
 	bool isFullUpdate() const {return fullUpdate_;}
+	float getUpdateError() const {return updateError_;}
+	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
+	const std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > & getCache() const {return cache_;}

 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -62,17 +78,31 @@ public:

 	void createLocalMap(
 			const Signature & node,
-			cv::Mat & ground,
-			cv::Mat & obstacles,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
 			cv::Point3f & viewPoint) const;

+	void createLocalMap(
+			const LaserScan & cloud,
+			const Transform & pose,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPointInOut) const;
+
 	void clear();
 	void addToCache(
 			int nodeId,
 			const cv::Mat & ground,
-			const cv::Mat & obstacles);
-	void update(const std::map<int, Transform> & poses);
-	const cv::Mat getMap(float & xMin, float & yMin) const;
+			const cv::Mat & obstacles,
+			const cv::Mat & empty);
+	bool update(const std::map<int, Transform> & poses); // return true if map has changed
+	cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getProbMap(float & xMin, float & yMin) const;
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}

 private:
 	ParametersMap parameters_;
@@ -85,7 +115,8 @@ private:
 	float footprintHeight_;
 	int scanDecimation_;
 	float cellSize_;
-	bool occupancyFromCloud_;
+	bool preVoxelFiltering_;
+	bool occupancyFromDepth_;
 	bool projMapFrame_;
 	float maxObstacleHeight_;
 	int normalKSearch_;
@@ -101,20 +132,30 @@ private:
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;
-	double scan2dMaxUnknownSpaceFilledRange_;
-	bool projRayTracing_;
+	bool rayTracing_;
 	bool fullUpdate_;
 	float minMapSize_;
 	bool erode_;
 	float footprintRadius_;
+	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;

-	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
 	cv::Mat map_;
 	cv::Mat mapInfo_;
 	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
 	float xMin_;
 	float yMin_;
 	std::map<int, Transform> addedNodes_;
+
+	bool cloudAssembling_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledEmptyCells_;
 };

 }
@@ -37,45 +37,164 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>

 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>

 #include <map>
 #include <string>

 namespace rtabmap {

-class OcTreeNodeInfo
+// forward declaraton for "friend"
+class RtabmapColorOcTree;
+
+class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode
 {
 public:
-	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
-		nodeRefId_(nodeRefId),
-		key_(key),
-		isObstacle_(isObstacle) {}
+	enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100};
+
+public:
+	friend class RtabmapColorOcTree; // needs access to node children (inherited)
+
+	RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {}
+	RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
+
+	void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
+	void setOccupancyType(char type) {type_=type;}
+	void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
+	int getNodeRefId() const {return nodeRefId_;}
+	int getOccupancyType() const {return type_;}
+	const octomap::point3d & getPointRef() const {return pointRef_;}
+
+	// following methods defined for octomap < 1.8 compatibility
+	RtabmapColorOcTreeNode* getChild(unsigned int i);
+	const RtabmapColorOcTreeNode* getChild(unsigned int i) const;
+	bool pruneNode();
+	void expandNode();
+	bool createChild(unsigned int i);
+
+private:
 	int nodeRefId_;
-	octomap::OcTreeKey key_;
-	bool isObstacle_;
+	int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
+	octomap::point3d pointRef_;
+};
+
+// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
+class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
+
+  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)
+	RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
+
+    std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
+
+     /**
+     * Prunes a node when it is collapsible. This overloaded
+     * version only considers the node occupancy for pruning,
+     * different colors of child nodes are ignored.
+     * @return true if pruning was successful
+     */
+    virtual bool pruneNode(RtabmapColorOcTreeNode* node);
+
+    virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
+
+    // set node color at given key or coordinate. Replaces previous color.
+    RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                 uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* setNodeColor(float x, float y,
+                                 float z, uint8_t r,
+                                 uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return setNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* averageNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap:: OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return averageNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* integrateNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return integrateNodeColor(key,r,g,b);
+    }
+
+    // update inner nodes, sets color to average child color
+    void updateInnerOccupancy();
+
+  protected:
+    void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
+
+    /**
+     * Static member object which ensures that this OcTree's prototype
+     * ends up in the classIDMapping only once. You need this as a
+     * static member in any derived octree class in order to read .ot
+     * files through the AbstractOcTree factory. You should also call
+     * ensureLinking() once from the constructor.
+     */
+    class StaticMemberInitializer{
+       public:
+         StaticMemberInitializer();
+
+         /**
+         * Dummy function to ensure that MSVC does not drop the
+         * StaticMemberInitializer, causing this tree failing to register.
+         * Needs to be called from the constructor of this octree.
+         */
+         void ensureLinking() {};
+    };
+    /// static member to ensure static initialization (only once)
+    static StaticMemberInitializer RtabmapColorOcTreeMemberInit;
+
  };

 class RTABMAP_EXP OctoMap {
 public:
-	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
+	static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
+
+public:
+	OctoMap(const ParametersMap & parameters);
+	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);

 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	void addToCache(int nodeId,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
 			const pcl::PointXYZ & viewPoint);
 	void addToCache(int nodeId,
 			const cv::Mat & ground,
 			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 octomap::ColorOcTree * octree() const {return octree_;}
+	const RtabmapColorOcTree * octree() const {return octree_;}

 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
 			unsigned int treeDepth = 0,
 			std::vector<int> * obstacleIndices = 0,
-			std::vector<int> * emptyIndices = 0) const;
+			std::vector<int> * emptyIndices = 0,
+			std::vector<int> * groundIndices = 0,
+			bool originalRefPoints = true) const;

 	cv::Mat createProjectionMap(
 			float & xMin,
@@ -89,16 +208,29 @@ public:
 	virtual ~OctoMap();
 	void clear();

+	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
+	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
+
+	void setMaxRange(float value) {rangeMax_ = value;}
+	void setRayTracing(bool enabled) {rayTracing_ = enabled;}
+	bool hasColor() const {return hasColor_;}
+
 private:
-	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
-	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_;
+	void updateMinMax(const octomap::point3d & point);
+
+private:
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; // [id: < <ground, obstacles>, empty>]
+	std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
 	std::map<int, cv::Point3f> cacheViewPoints_;
-	octomap::ColorOcTree * octree_;
-	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	RtabmapColorOcTree * octree_;
 	std::map<int, Transform> addedNodes_;
-	octomap::KeyRay keyRay_;
 	bool hasColor_;
 	bool fullUpdate_;
+	float updateError_;
+	float rangeMax_;
+	bool rayTracing_;
+	double minValues_[3];
+	double maxValues_[3];
 };

 } /* namespace rtabmap */
@@ -45,11 +45,19 @@ public:
 	enum Type {
 		kTypeUndef = -1,
 		kTypeF2M = 0,
-		kTypeF2F = 1
+		kTypeF2F = 1,
+		kTypeFovis = 2,
+		kTypeViso2 = 3,
+		kTypeDVO = 4,
+		kTypeORBSLAM2 = 5,
+		kTypeOkvis = 6,
+		kTypeLOAM = 7,
+		kTypeMSCKF = 8,
+		kTypeVINS = 9
 	};

 public:
-	static Odometry * create(const ParametersMap & parameters);
+	static Odometry * create(const ParametersMap & parameters = ParametersMap());
 	static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());

 public:
@@ -58,12 +66,17 @@ public:
 	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
 	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;}

 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -77,6 +90,7 @@ private:
 	bool _force3DoF;
 	bool _holonomic;
 	bool guessFromMotion_;
+	bool guessSmoothingDelay_;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -88,12 +102,16 @@ private:
 	float _kalmanMeasurementNoise;
 	int _imageDecimation;
 	bool _alignWithGround;
+	bool _publishRAMUsage;
+	bool _imagesAlreadyRectified;
 	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_;

 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
@@ -39,53 +39,29 @@ namespace rtabmap {
 class OdometryEvent : public UEvent
 {
 public:
-	static cv::Mat generateCovarianceMatrix(float rotVariance, float transVariance)
-	{
-		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
-		UASSERT(uIsFinite(transVariance) && transVariance>0);
-		cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-		covariance.at<double>(0,0) = transVariance;
-		covariance.at<double>(1,1) = transVariance;
-		covariance.at<double>(2,2) = transVariance;
-		covariance.at<double>(3,3) = rotVariance;
-		covariance.at<double>(4,4) = rotVariance;
-		covariance.at<double>(5,5) = rotVariance;
-		return covariance;
-	}
-public:
-	OdometryEvent() :
-		_covariance(cv::Mat::eye(6,6,CV_64FC1))
+	OdometryEvent()
 	{
+		_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 	OdometryEvent(
 		const SensorData & data,
 		const Transform & pose,
-		const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1),
 		const OdometryInfo & info = OdometryInfo()) :
 			_data(data),
 			_pose(pose),
 			_info(info)
 	{
-		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, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(1,1)) && covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(covariance.at<double>(5,5)) && covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		_covariance = covariance;
-	}
-	OdometryEvent(
-		const SensorData & data,
-		const Transform & pose,
-		double rotVariance = 1.0,
-		double transVariance = 1.0,
-		const OdometryInfo & info = OdometryInfo()) :
-			_data(data),
-			_pose(pose),
-			_covariance(generateCovarianceMatrix(rotVariance, transVariance)),
-			_info(info)
+		if(_info.reg.covariance.empty())
 		{
+			_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+		}
+		UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
 	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
@@ -93,24 +69,40 @@ public:
 	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
 	const Transform & pose() const {return _pose;}
-	const cv::Mat & covariance() const {return _covariance;}
+	const cv::Mat & covariance() const {return _info.reg.covariance;}
+	std::vector<float> velocity() const {
+		if(_info.interval>0.0)
+		{
+			std::vector<float> velocity(6,0);
+			float x,y,z,roll,pitch,yaw;
+			_info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+			velocity[0] = x/_info.interval;
+			velocity[1] = y/_info.interval;
+			velocity[2] = z/_info.interval;
+			velocity[3] = roll/_info.interval;
+			velocity[4] = pitch/_info.interval;
+			velocity[5] = yaw/_info.interval;
+			return velocity;
+		}
+		return std::vector<float>();
+	}
 	const OdometryInfo & info() const {return _info;}
-	double rotVariance() const {return uMax3(_covariance.at<double>(3,3), _covariance.at<double>(4,4), _covariance.at<double>(5,5));}
-	double transVariance() const {return uMax3(_covariance.at<double>(0,0), _covariance.at<double>(1,1), _covariance.at<double>(2,2));}

 private:
 	SensorData _data;
 	Transform _pose;
-	cv::Mat _covariance;
 	OdometryInfo _info;
 };

 class OdometryResetEvent : public UEvent
 {
 public:
-	OdometryResetEvent(){}
+	OdometryResetEvent(const Transform & pose = Transform::getIdentity()){_pose = pose;}
 	virtual ~OdometryResetEvent() {}
 	virtual std::string getClassName() const {return "OdometryResetEvent";}
+	const Transform & getPose() const {return _pose;}
+private:
+	Transform _pose;
 };

 }
@@ -30,6 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <map>
 #include "rtabmap/core/Transform.h"
+#include "rtabmap/core/RegistrationInfo.h"
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/LaserScan.h"
 #include <opencv2/features2d/features2d.hpp>

 namespace rtabmap {
@@ -39,11 +42,6 @@ class OdometryInfo
 public:
 	OdometryInfo() :
 		lost(true),
-		matches(0),
-		inliers(0),
-		icpInliersRatio(0.0f),
-		varianceLin(0.0f),
-		varianceAng(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
@@ -57,6 +55,7 @@ public:
 		stamp(0),
 		interval(0),
 		distanceTravelled(0.0f),
+		memoryUsage(0),
 		type(0)
 	{}

@@ -64,11 +63,7 @@ public:
 	{
 		OdometryInfo output;
 		output.lost = lost;
-		output.matches = matches;
-		output.inliers = inliers;
-		output.icpInliersRatio = icpInliersRatio;
-		output.varianceLin = varianceLin;
-		output.varianceAng = varianceAng;
+		output.reg = reg.copyWithoutData();
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
@@ -76,24 +71,24 @@ public:
 		output.localBundleOutliers = localBundleOutliers;
 		output.localBundleConstraints = localBundleConstraints;
 		output.localBundleTime = localBundleTime;
+		output.localBundlePoses = localBundlePoses;
+		output.localBundleModels = localBundleModels;
 		output.keyFrameAdded = keyFrameAdded;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
+		output.interval = interval;
 		output.transform = transform;
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
 		output.distanceTravelled = distanceTravelled;
+		output.memoryUsage = memoryUsage;
 		output.type = type;
 		return output;
 	}

 	bool lost;
-	int matches;
-	int inliers;
-	float icpInliersRatio;
-	float varianceLin;
-	float varianceAng;
+	RegistrationInfo reg;
 	int features;
 	int localMapSize;
 	int localScanMapSize;
@@ -101,6 +96,8 @@ public:
 	int localBundleOutliers;
 	int localBundleConstraints;
 	float localBundleTime;
+	std::map<int, Transform> localBundlePoses;
+	std::map<int, CameraModel> localBundleModels;
 	bool keyFrameAdded;
 	float timeEstimation;
 	float timeParticleFiltering;
@@ -109,16 +106,16 @@ public:
 	Transform transform;
 	Transform transformFiltered;
 	Transform transformGroundTruth;
+	Transform guessVelocity;
 	float distanceTravelled;
+	int memoryUsage; //MB

-	int type; // 0=F2M, 1=F2F
+	int type;

 	// F2M
 	std::multimap<int, cv::KeyPoint> words;
-	std::vector<int> wordMatches;
-	std::vector<int> wordInliers;
 	std::map<int, cv::Point3f> localMap;
-	cv::Mat localScanMap;
+	LaserScan localScanMap;

 	// F2F
 	std::vector<cv::Point2f> refCorners;
@@ -62,9 +62,13 @@ private:
 	USemaphore _dataAdded;
 	UMutex _dataMutex;
 	std::list<SensorData> _dataBuffer;
+	std::list<SensorData> _imuBuffer;
 	Odometry * _odometry;
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
+	Transform _resetPose;
+	double _lastImuStamp;
+	double _imuEstimatedDelay;
 };

 } // namespace rtabmap
@@ -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() {}
@@ -75,6 +87,9 @@ public:
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
 	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;}
@@ -82,14 +97,34 @@ public:
 	void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
 	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);

+	std::map<int, Transform> optimizeIncremental(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+
+	std::map<int, Transform> optimize(
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 				int rootId,
 				const std::map<int, Transform> & poses,
 				const std::multimap<int, Link> & constraints,
+				cv::Mat & outputCovariance,
 				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 				double * finalError = 0,
 				int * iterationsDone = 0);
@@ -99,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(
@@ -120,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(
@@ -128,7 +174,10 @@ protected:
 			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
-			bool robust            = Parameters::defaultOptimizerRobust());
+			bool robust            = Parameters::defaultOptimizerRobust(),
+			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored(),
+			bool landmarksIgnored  = Parameters::defaultOptimizerLandmarksIgnored(),
+			float gravitySigma     = Parameters::defaultOptimizerGravitySigma());
 	Optimizer(const ParametersMap & parameters);

 private:
@@ -137,6 +186,9 @@ private:
 	bool covarianceIgnored_;
 	double epsilon_;
 	bool robust_;
+	bool priorsIgnored_;
+	bool landmarksIgnored_;
+	float gravitySigma_;
 };

 } /* namespace rtabmap */
@@ -168,13 +168,15 @@ typedef std::pair<std::string, std::string> ParametersPair;
 class RTABMAP_EXP Parameters
 {
    // Rtabmap parameters
-	RTABMAP_PARAM(Rtabmap, VhStrategy, 	                 int, 0,     "None 0, Similarity 1, Epipolar 2.");
    RTABMAP_PARAM(Rtabmap, PublishStats,                 bool, true,  "Publishing statistics.");
    RTABMAP_PARAM(Rtabmap, PublishLastSignature,         bool, true,  "Publishing last signature.");
    RTABMAP_PARAM(Rtabmap, PublishPdf,                   bool, true,  "Publishing pdf.");
    RTABMAP_PARAM(Rtabmap, PublishLikelihood,            bool, true,  "Publishing likelihood.");
-	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0,   "Maximum time allowed for the detector (ms) (0 means infinity).");
-	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
+    RTABMAP_PARAM(Rtabmap, PublishRAMUsage,              bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
+    RTABMAP_PARAM(Rtabmap, ComputeRMSE,                  bool, true,  "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
+    RTABMAP_PARAM(Rtabmap, SaveWMState,                  bool, false, "Save working memory state after each update in statistics.");
+    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for map update (ms) (0 means infinity). When map update time exceeds this fixed time threshold, some nodes in Working Memory (WM) are transferred to Long-Term Memory to limit the size of the WM and decrease the update time.");
+    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      uFormat("Maximum nodes in the Working Memory (0 means infinity). Similar to \"%s\", when the number of nodes in Working Memory (WM) exceeds this treshold, some nodes are transferred to Long-Term Memory to keep WM size fixed.", kRtabmapTimeThr().c_str()));
    RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
    RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
    RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
@@ -184,10 +186,14 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
    RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
    RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
+    RTABMAP_PARAM(Rtabmap, StartNewMapOnGoodSignature,   bool, false, uFormat("Start a new map only if the first signature is not bad (i.e., has enough features, see %s).", kKpBadSignRatio().c_str()));
+    RTABMAP_PARAM(Rtabmap, ImagesAlreadyRectified,       bool, true,  "Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.");
+    RTABMAP_PARAM(Rtabmap, RectifyOnlyFeatures,          bool, false,  uFormat("If \"%s\" is false and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.", kRtabmapImagesAlreadyRectified().c_str()));

    // Hypotheses selection
    RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
    RTABMAP_PARAM(Rtabmap, LoopRatio,         float, 0,         "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+    RTABMAP_PARAM(Rtabmap, LoopGPS,           bool,  true,      uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));

    // Memory
    RTABMAP_PARAM(Mem, RehearsalSimilarity,         float, 0.6,     "Rehearsal similarity.");
@@ -197,6 +203,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
    RTABMAP_PARAM(Mem, 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).");
@@ -207,26 +215,37 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Mem, GenerateIds,                 bool, true,     "True=Generate location IDs, False=use input image IDs.");
    RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
    RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+    RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
    RTABMAP_PARAM(Mem, ImagePreDecimation,          int, 1,         "Image decimation (>=1) before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
    RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
    RTABMAP_PARAM(Mem, 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, LaserScanNormalK,        int, 0,          "If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.");
-	RTABMAP_PARAM(Mem, UseOdomFeatures,         bool, false, "Use odometry features.");
+    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 instead of regenerating them.");
+    RTABMAP_PARAM(Mem, UseOdomGravity,              bool, false,    uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
+    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");

    // KeypointMemory (Keypoint-based)
    RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
    RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
-	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
+    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
+    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
    RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
    RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
-	RTABMAP_PARAM(Kp, MaxFeatures,           int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
+    RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
    RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
-#ifdef RTABMAP_NONFREE
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+#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 10=ORB-OCTREE.");
 #else
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+    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 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.");
@@ -236,6 +255,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
    RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
    RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Kp, GridRows,                 int, 1,       uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));
+    RTABMAP_PARAM(Kp, GridCols,                 int, 1,       uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));

    //Database
    RTABMAP_PARAM(DbSqlite3, InMemory,     bool, false,      "Using database in the memory instead of a file on the hard disk.");
@@ -261,17 +282,17 @@ class RTABMAP_EXP Parameters

    RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");

-	RTABMAP_PARAM(FAST, Threshold,          int, 10, 	    "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
+    RTABMAP_PARAM(FAST, Threshold,          int, 20,      "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
    RTABMAP_PARAM(FAST, NonmaxSuppression,  bool, true,   "If true, non-maximum suppression is applied to detected corners (keypoints).");
    RTABMAP_PARAM(FAST, Gpu,                bool, false,  "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
    RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, "Used with FAST GPU.");
-	RTABMAP_PARAM(FAST, MinThreshold,       int, 1, 	    "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
+    RTABMAP_PARAM(FAST, MinThreshold,       int, 7,       "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
    RTABMAP_PARAM(FAST, MaxThreshold,       int, 200,     "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
-	RTABMAP_PARAM(FAST, GridRows,           int, 4,         "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
-	RTABMAP_PARAM(FAST, GridCols,           int, 4,         "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridRows,           int, 0,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridCols,           int, 0,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");

-	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
-	RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
+    RTABMAP_PARAM(GFTT, QualityLevel,      double, 0.001, "");
+    RTABMAP_PARAM(GFTT, MinDistance,       double, 3,    "");
    RTABMAP_PARAM(GFTT, BlockSize,         int, 3,       "");
    RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false,  "");
    RTABMAP_PARAM(GFTT, K,                 double, 0.04, "");
@@ -294,23 +315,34 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(BRISK, Octaves,      int, 3,  "Detection octaves. Use 0 to do single scale.");
    RTABMAP_PARAM(BRISK, PatternScale, float, 1,"Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");

+    RTABMAP_PARAM(KAZE, Extended,       bool, false,   "Set to enable extraction of extended (128-byte) descriptor.");
+    RTABMAP_PARAM(KAZE, Upright,        bool, false,   "Set to enable use of upright descriptors (non rotation-invariant).");
+    RTABMAP_PARAM(KAZE, Threshold,      float, 0.001,  "Detector response threshold to accept point.");
+    RTABMAP_PARAM(KAZE, NOctaves,       int, 4,        "Maximum octave evolution of the image.");
+    RTABMAP_PARAM(KAZE, NOctaveLayers,  int, 4,        "Default number of sublevels per scale level.");
+    RTABMAP_PARAM(KAZE, Diffusivity,    int, 1,        "Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.");
+
    // BayesFilter
    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
    RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, false, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");

    // Verify hypotheses
+    RTABMAP_PARAM(VhEp, Enabled, bool, false,       uFormat("Verify visual loop closure hypothesis by computing a fundamental matrix. This is done prior to transformation computation when %s is enabled.", kRGBDEnabled().c_str()));
    RTABMAP_PARAM(VhEp, MatchCountMin, int, 8,      "Minimum of matching visual words pairs to accept the loop hypothesis.");
    RTABMAP_PARAM(VhEp, RansacParam1,  float, 3,    "Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
-	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
+    RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of RANSAC.");

    // RGB-D SLAM
    RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
-	RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
+    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 mode 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,    uFormat("Reject loop closures if optimization error is greater than this value (0=disabled). 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, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, 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.");
    RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
@@ -322,14 +354,18 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,    "Save scan matching IDs in link's user data.");
    RTABMAP_PARAM(RGBD, NeighborLinkRefining,         bool, false,  uFormat("When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using registration approach selected (%s).", kRegStrategy().c_str()));
    RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes.");
+    RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
    RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
+    RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
+    RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
+    RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  0,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false) and when %s!=0. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));

    // Local/Proximity loop closure detection
    RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
-	RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true, 	"Detection over locations (in Working Memory or STM) near in space.");
+    RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true,  "Detection over locations (in Working Memory) near in space.");
    RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,     "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
    RTABMAP_PARAM(RGBD, ProximityMaxPaths,            int, 3,      "Maximum paths compared (from the most recent) for proximity detection by space. 0 means no limit.");
-	RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,   "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 1,    "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
    RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path. Set to 0 to disable merging the laser scans.");
    RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for visual proximity detection.");
@@ -352,8 +388,15 @@ class RTABMAP_EXP Parameters
 #endif
    RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
    RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
+    RTABMAP_PARAM(Optimizer, LandmarksIgnored,   bool, false,  "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
+    RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.0,      uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with 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");
+#else
    RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
+#endif
    RTABMAP_PARAM(g2o, Optimizer,         int, 0,          "0=Levenberg 1=GaussNewton");
    RTABMAP_PARAM(g2o, PixelVariance,     double, 1.0,     "Pixel variance used for bundle adjustment.");
    RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
@@ -362,12 +405,12 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");

    // Odometry
-	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO");
    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
    RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
    RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
    RTABMAP_PARAM(Odom, ImageBufferSize,     unsigned int, 1, "Data buffer size (0 min inf).");
-	RTABMAP_PARAM(Odom, FilteringStrategy, 		int, 0,         "0=No filtering 1=Kalman filtering 2=Particle filtering");
+    RTABMAP_PARAM(Odom, FilteringStrategy,      int, 0,       "0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.");
    RTABMAP_PARAM(Odom, ParticleSize,      unsigned int, 400, "Number of particles of the filter.");
    RTABMAP_PARAM(Odom, ParticleNoiseT,         float, 0.002, "Noise (m) of translation components (x,y,z).");
    RTABMAP_PARAM(Odom, ParticleLambdaT,        float, 100,   "Lambda of translation components (x,y,z).");
@@ -375,20 +418,27 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Odom, ParticleLambdaR,        float, 100,   "Lambda of rotational components (roll,pitch,yaw).");
    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, false,      "Guess next transformation from the last motion computed.");
+    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, 100,         "[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.7,       "[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.");
+    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.");
    RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,       "Decimation of the images before registration. 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(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");

-	// Odometry Bag-of-words
+    // Odometry Frame-to-Map
    RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
    RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
    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
    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
-	RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
+#endif
+    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");

    // Odometry Mono
    RTABMAP_PARAM(OdomMono, InitMinFlow,        float, 100,  "Minimum optical flow required for the initialization step.");
@@ -396,73 +446,205 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(OdomMono, MinTranslation,     float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
    RTABMAP_PARAM(OdomMono, MaxVariance,        float, 0.01, "Maximum variance to add new points to local map.");

+    // Odometry Fovis
+    RTABMAP_PARAM(OdomFovis, FeatureWindowSize,               int, 9,        "The size of the n x n image patch surrounding each feature, used for keypoint matching.");
+    RTABMAP_PARAM(OdomFovis, MaxPyramidLevel,                 int, 3,        "The maximum Gaussian pyramid level to process the image at. Pyramid level 1 corresponds to the original image.");
+    RTABMAP_PARAM(OdomFovis, MinPyramidLevel,                 int, 0,        "The minimum pyramid level.");
+    RTABMAP_PARAM(OdomFovis, TargetPixelsPerFeature,          int, 250,      "Specifies the desired feature density as a ratio of input image pixels per feature detected.  This number is used to control the adaptive feature thresholding.");
+    RTABMAP_PARAM(OdomFovis, FastThreshold,                   int, 20,       "FAST threshold.");
+    RTABMAP_PARAM(OdomFovis, UseAdaptiveThreshold,            bool, true,    "Use FAST adaptive threshold.");
+    RTABMAP_PARAM(OdomFovis, FastThresholdAdaptiveGain,       double, 0.005, "FAST threshold adaptive gain.");
+    RTABMAP_PARAM(OdomFovis, UseHomographyInitialization,     bool, true,    "Use homography initialization.");
+
+    RTABMAP_PARAM(OdomFovis, UseBucketing,                    bool, true,  "");
+    RTABMAP_PARAM(OdomFovis, BucketWidth,                     int, 80,     "");
+    RTABMAP_PARAM(OdomFovis, BucketHeight,                    int, 80,     "");
+    RTABMAP_PARAM(OdomFovis, MaxKeypointsPerBucket,           int, 25,     "");
+    RTABMAP_PARAM(OdomFovis, UseImageNormalization,           bool, false, "");
+
+    RTABMAP_PARAM(OdomFovis, InlierMaxReprojectionError,      double, 1.5,  "The maximum image-space reprojection error (in pixels) a feature match is allowed to have and still be considered an inlier in the set of features used for motion estimation.");
+    RTABMAP_PARAM(OdomFovis, CliqueInlierThreshold,           double, 0.1,  "See Howard's greedy max-clique algorithm for determining the maximum set of mutually consisten feature matches. This specifies the compatibility threshold, in meters.");
+    RTABMAP_PARAM(OdomFovis, MinFeaturesForEstimate,          int, 20,      "Minimum number of features in the inlier set for the motion estimate to be considered valid.");
+    RTABMAP_PARAM(OdomFovis, MaxMeanReprojectionError,        double, 10.0, "Maximum mean reprojection error over the inlier feature matches for the motion estimate to be considered valid.");
+    RTABMAP_PARAM(OdomFovis, UseSubpixelRefinement,           bool, true,   "Specifies whether or not to refine feature matches to subpixel resolution.");
+    RTABMAP_PARAM(OdomFovis, FeatureSearchWindow,             int, 25,      "Specifies the size of the search window to apply when searching for feature matches across time frames.  The search is conducted around the feature location predicted by the initial rotation estimate.");
+    RTABMAP_PARAM(OdomFovis, UpdateTargetFeaturesWithRefined, bool, false,  "When subpixel refinement is enabled, the refined feature locations can be saved over the original feature locations.  This has a slightly negative impact on frame-to-frame visual odometry, but is likely better when using this library as part of a visual SLAM algorithm.");
+
+    RTABMAP_PARAM(OdomFovis, StereoRequireMutualMatch,        bool, true,  "");
+    RTABMAP_PARAM(OdomFovis, StereoMaxDistEpipolarLine,       double, 1.5, "");
+    RTABMAP_PARAM(OdomFovis, StereoMaxRefinementDisplacement, double, 1.0, "");
+    RTABMAP_PARAM(OdomFovis, StereoMaxDisparity,              int, 128,    "");
+
+    // Odometry viso2
+    RTABMAP_PARAM(OdomViso2, RansacIters,               int, 200,    "Number of RANSAC iterations.");
+    RTABMAP_PARAM(OdomViso2, InlierThreshold,           double, 2.0, "Fundamental matrix inlier threshold.");
+    RTABMAP_PARAM(OdomViso2, Reweighting,               bool, true,  "Lower border weights (more robust to calibration errors).");
+    RTABMAP_PARAM(OdomViso2, MatchNmsN,                 int, 3,      "Non-max-suppression: min. distance between maxima (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchNmsTau,               int, 50,     "Non-max-suppression: interest point peakiness threshold.");
+    RTABMAP_PARAM(OdomViso2, MatchBinsize,              int, 50,     "Matching bin width/height (affects efficiency only).");
+    RTABMAP_PARAM(OdomViso2, MatchRadius,               int, 200,    "Matching radius (du/dv in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchDispTolerance,        int, 2,      "Disparity tolerance for stereo matches (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchOutlierDispTolerance, int, 5,      "Outlier removal: disparity tolerance (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchOutlierFlowTolerance, int, 5,      "Outlier removal: flow tolerance (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchMultiStage,           bool, true,  "Multistage matching (denser and faster).");
+    RTABMAP_PARAM(OdomViso2, MatchHalfResolution,       bool, true,  "Match at half resolution, refine at full resolution.");
+    RTABMAP_PARAM(OdomViso2, MatchRefinement,           int, 1,      "Refinement (0=none,1=pixel,2=subpixel).");
+    RTABMAP_PARAM(OdomViso2, BucketMaxFeatures,         int, 2,      "Maximal number of features per bucket.");
+    RTABMAP_PARAM(OdomViso2, BucketWidth,               double, 50,  "Width of bucket.");
+    RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
+
+    // Odometry ORB_SLAM2
+    RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM(OdomORBSLAM2, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM2, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM2, Fps,             float,  0.0,   "Camera FPS.");
+    RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
+    RTABMAP_PARAM(OdomORBSLAM2, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");
+
+    // Odometry OKVIS
+    RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
+
+    // Odometry LOAM
+    RTABMAP_PARAM(OdomLOAM, Sensor,     int,    2,    "Velodyne sensor: 0=VLP-16, 1=HDL-32, 2=HDL-64E");
+    RTABMAP_PARAM(OdomLOAM, ScanPeriod, float,  0.1,  "Scan period (s)");
+    RTABMAP_PARAM(OdomLOAM, LinVar,     float,  0.01,  "Linear output variance.");
+    RTABMAP_PARAM(OdomLOAM, AngVar,     float,  0.01,  "Angular output variance.");
+    RTABMAP_PARAM(OdomLOAM, LocalMapping, bool,  true,  "Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.");
+
+    // Odometry MSCKF_VIO
+    RTABMAP_PARAM(OdomMSCKF, GridRow,           int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridCol,           int,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMinFeatureNum, int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMaxFeatureNum, int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, PyramidLevels,     int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, PatchSize,         int,  15,  "");
+    RTABMAP_PARAM(OdomMSCKF, FastThreshold,     int,  10,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxIteration,      int,  30,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackPrecision,    double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, RansacThreshold,   double,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, StereoThreshold,   double,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, PositionStdThreshold,    double,  8.0,  "");
+    RTABMAP_PARAM(OdomMSCKF, RotationThreshold,       double,  0.2618,  "");
+    RTABMAP_PARAM(OdomMSCKF, TranslationThreshold,    double,  0.4,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackingRateThreshold,   double,  0.5,  "");
+    RTABMAP_PARAM(OdomMSCKF, OptTranslationThreshold, double,  0,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyro,     double,  0.005,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAcc,      double,  0.05,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyroBias, double,  0.001,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseFeature,  double,  0.035,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovVel,    double,  0.25,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovGyroBias, double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExRot,    double,  0.00030462,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExTrans,  double,  0.000025,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize,  int,  20,  "");
+
+    // Odometry VINS
+    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");
+
    // Common registration parameters
-	RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
+    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");
    RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");

    // Visual registration parameters
-	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
+    RTABMAP_PARAM(Vis, EstimationType,           int, 1,        "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
    RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true,    "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
    RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, PnPReprojError,           float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, PnPFlags,                 int, 0,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 0,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
+#else
+    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
+#endif
+
    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
    RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
-	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
+    RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
 #ifndef RTABMAP_NONFREE
 #ifdef RTABMAP_OPENCV3
    // OpenCV 3 without xFeatures2D module doesn't have BRIEF
-	RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+    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=FREAK.");
+    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=FREAK.");
+    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).");
    RTABMAP_PARAM(Vis, MinDepth,                  float, 0,   "Min depth of the features (0 means no limit).");
+    RTABMAP_PARAM(Vis, DepthAsMask,               bool, true, "Use depth image as mask when extracting features.");
    RTABMAP_PARAM_STR(Vis, RoiRatios,      "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
    RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,      "See cv::cornerSubPix().");
    RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,      "See cv::cornerSubPix(). 0 disables sub pixel refining.");
    RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
+    RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
    RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,      uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 1,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+#else
    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+#endif

    // ICP registration parameters
    RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
    RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
    RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.0,   "Uniform sampling voxel size (0=disabled).");
    RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1,       "Downsampling step size (1=no sampling). This is done before uniform sampling.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1,   "Max distance for point correspondences.");
+#else
    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
+#endif
    RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
    RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
-	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.2,   "Ratio of matching correspondences to accept the transform.");
+    RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+#else
    RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
-	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
+#endif
+    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
+
+    // libpointmatcher
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PM,                       bool, true,    "Use libpointmatcher for ICP registration instead of PCL's implementation.");
+#else
+    RTABMAP_PARAM(Icp, PM,                       bool, false,   "Use libpointmatcher for ICP registration instead of PCL's implementation.");
+#endif
+    RTABMAP_PARAM_STR(Icp, PMConfig,             "",            uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
+    RTABMAP_PARAM(Icp, PMMatcherKnn,             int, 1,        "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMMatcherEpsilon,         float, 0.0,    "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMOutlierRatio,           float, 0.95,   "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");

    // Stereo disparity
    RTABMAP_PARAM(Stereo, WinWidth,              int, 15,       "Window width.");
    RTABMAP_PARAM(Stereo, WinHeight,             int, 3,        "Window height.");
    RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "Maximum iterations.");
-	RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "Maximum pyramid level.");
-	RTABMAP_PARAM(Stereo, MinDisparity,          int, 1,        "Minimum disparity.");
-	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 128,      "Maximum disparity.");
+    RTABMAP_PARAM(Stereo, MaxLevel,              int, 5,        "Maximum pyramid level.");
+    RTABMAP_PARAM(Stereo, MinDisparity,          float, 0.5,    "Minimum disparity.");
+    RTABMAP_PARAM(Stereo, MaxDisparity,          float, 128.0,  "Maximum disparity.");
    RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
    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");
@@ -472,23 +654,41 @@ 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.");
    RTABMAP_PARAM(Grid, DepthDecimation,         int,    4,       uFormat("[%s=true] Decimation of the depth image before creating cloud. 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).", kGridDepthDecimation().c_str()));
-	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridFromDepth().c_str()));
-	RTABMAP_PARAM(Grid, DepthMax,                float,  4.0,     uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, RangeMin,                float,  0.0,     "Minimum range from sensor.");
+    RTABMAP_PARAM(Grid, RangeMax,                float,  5.0,     "Maximum range from sensor. 0=inf.");
    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));
    RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
    RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
    RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridFromDepth().c_str()));
    RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
+    RTABMAP_PARAM(Grid, PreVoxelFiltering,       bool,   true,    uFormat("Input cloud is downsampled by voxel filter (voxel size is \"%s\") before doing segmentation of obstacles and ground.", kGridCellSize().c_str()));
    RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
    RTABMAP_PARAM(Grid, NormalsSegmentation,     bool,   true,    "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
    RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "Maximum obstacles height (0=disabled).");
    RTABMAP_PARAM(Grid, MinGroundHeight,         float,  0.0,     "Minimum ground height (0=disabled).");
-	RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is false.", kGridNormalsSegmentation().c_str()));
    RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  45,      uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
    RTABMAP_PARAM(Grid, NormalK,                 int,    20,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()));
    RTABMAP_PARAM(Grid, ClusterRadius,           float,  0.1,     uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str()));
@@ -502,14 +702,35 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
    RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
    RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
-	RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,     bool,    false,   "Unknown space filled. Only used with 2D laser scans.");
-	RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,         float,   4.0,     "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
-	RTABMAP_PARAM(Grid, ProjRayTracing,               bool,   true,    uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
+    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
+    RTABMAP_PARAM(Grid, RayTracing,                 bool,   false,   uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));

    RTABMAP_PARAM(GridGlobal, FullUpdate,           bool,   true,    "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
+    RTABMAP_PARAM(GridGlobal, UpdateError,          float,  0.01,    "Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value.");
    RTABMAP_PARAM(GridGlobal, FootprintRadius,      float,  0.0,     "Footprint radius (m) used to clear all obstacles under the graph.");
    RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
    RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
+    RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
+    RTABMAP_PARAM(GridGlobal, OccupancyThr,         float,  0.5,     "Occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbHit,              float,  0.7,     "Probability of a hit (value between 0.5 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbMiss,             float,  0.4,     "Probability of a miss (value between 0 and 0.5).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMin,      float,  0.1192,  "Probability clamping minimum (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
+
+    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
+    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
+    RTABMAP_PARAM(Marker, MaxDepthError,          float, 0.01,  uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
+    RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
+    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
+    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+
+    RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
+
+    RTABMAP_PARAM(ImuFilter, ComplementaryGainAcc,          double, 0.01, "Gain parameter for the complementary filter, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, ComplementaryBiasAlpha,        double, 0.01, "Bias estimation gain parameter, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, ComplementaryDoBiasEstimation, bool,   true, "Parameter whether to do bias estimation or not.");
+    RTABMAP_PARAM(ImuFilter, ComplementaryDoAdpativeGain,   bool,   true, "Parameter whether to do adaptive gain or not.");

 public:
    virtual ~Parameters();
@@ -557,7 +778,7 @@ public:
    static ParametersMap getDefaultParameters(const std::string & group);
    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);

-	static void readINI(const std::string & configFile, ParametersMap & parameters);
+    static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
    static void writeINI(const std::string & configFile, const ParametersMap & parameters);

    /**
@@ -28,6 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_

+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {

 class ProgressState
 {
@@ -35,6 +38,8 @@ public:
 	ProgressState():canceled_(false){}
 	virtual bool callback(const std::string & msg) const
 	{
+		if(!msg.empty())
+			UDEBUG("msg=%s", msg.c_str());
 		return true;
 	}
 	virtual ~ProgressState(){}
@@ -52,5 +57,6 @@ private:
 	bool canceled_;
 };

+}

 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_ */
@@ -0,0 +1,56 @@
+/*
+Copyright (c) 2010-2017, 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 RECOVERY_H_
+#define RECOVERY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <string>
+
+namespace rtabmap {
+
+class ProgressState;
+
+/**
+ * Return true on success. The database is
+ * renamed to "*.backup.db" before recovering.
+ * @param corruptedDatabase database to recover
+ * @param keepCorruptedDatabase if false and on recovery success, the backup database is removed
+ * @param errorMsg error message if the function returns false
+ * @param progressState A ProgressState object used to get status of the recovery process
+ */
+bool RTABMAP_EXP databaseRecovery(
+		const std::string & corruptedDatabase,
+		bool keepCorruptedDatabase = true,
+		std::string * errorMsg = 0,
+		ProgressState * progressState = 0);
+
+}
+
+
+#endif /* RECOVERY_H_ */
@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef REGISTRATION_H_
-#define REGISTRATION_H_
+#ifndef RTABMAP_REGISTRATION_H_
+#define RTABMAP_REGISTRATION_H_

 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines

@@ -45,6 +45,7 @@ public:
 		kTypeIcp = 1,
 		kTypeVisIcp = 2
 	};
+	static double COVARIANCE_EPSILON;

 public:
 	static Registration * create(const ParametersMap & parameters);
@@ -58,10 +59,12 @@ public:
 	bool isScanRequired() const;
 	bool isUserDataRequired() const;

+	bool canUseGuess() const;
+
 	int getMinVisualCorrespondences() const;
 	float getMinGeometryCorrespondencesRatio() const;

-	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
+	bool repeatOnce() const {return repeatOnce_;}
 	bool force3DoF() const {return force3DoF_;}

 	// take ownership!
@@ -75,7 +78,7 @@ public:
 	Transform computeTransformation(
 			const SensorData & from,
 			const SensorData & to,
-			Transform SensorData = Transform::getIdentity(),
+			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;

 	Transform computeTransformationMod(
@@ -99,11 +102,12 @@ protected:
 	virtual bool isImageRequiredImpl() const {return false;}
 	virtual bool isScanRequiredImpl() const {return false;}
 	virtual bool isUserDataRequiredImpl() const {return false;}
+	virtual bool canUseGuessImpl() const {return false;}
 	virtual int getMinVisualCorrespondencesImpl() const {return 0;}
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}

 private:
-	bool varianceFromInliersCount_;
+	bool repeatOnce_;
 	bool force3DoF_;
 	Registration * child_;

@@ -111,4 +115,4 @@ private:

 }

-#endif /* REGISTRATION_H_ */
+#endif /* RTABMAP_REGISTRATION_H_ */
@@ -41,7 +41,7 @@ class RTABMAP_EXP RegistrationIcp : public Registration
 public:
 	// take ownership of child
 	RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
-	virtual ~RegistrationIcp() {}
+	virtual ~RegistrationIcp();

 	virtual void parseParameters(const ParametersMap & parameters);

@@ -52,6 +52,7 @@ protected:
 			Transform guess,
 			RegistrationInfo & info) const;
 	virtual bool isScanRequiredImpl() const {return true;}
+	virtual bool canUseGuessImpl() const {return true;}
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}

 private:
@@ -64,7 +65,15 @@ private:
 	float _epsilon;
 	float _correspondenceRatio;
 	bool _pointToPlane;
-	int _pointToPlaneNormalNeighbors;
+	int _pointToPlaneK;
+	float _pointToPlaneRadius;
+	float _pointToPlaneMinComplexity;
+	bool _libpointmatcher;
+	std::string _libpointmatcherConfig;
+	int _libpointmatcherKnn;
+	float _libpointmatcherEpsilon;
+	float _libpointmatcherOutlierRatio;
+	void * _libpointmatcherICP;
 };

 }
@@ -35,30 +35,48 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
-		varianceLin(0),
-		varianceAng(0),
+		totalTime(0.0),
 		inliers(0),
 		matches(0),
 		icpInliersRatio(0),
 		icpTranslation(0.0f),
-		icpRotation(0.0f)
+		icpRotation(0.0f),
+		icpStructuralComplexity(0.0f)
+
 	{
 	}

-	float varianceLin;
-	float varianceAng;
+	RegistrationInfo copyWithoutData() const
+	{
+		RegistrationInfo output;
+		output.totalTime = totalTime;
+		output.covariance = covariance.clone();
+		output.rejectedMsg = rejectedMsg;
+		output.inliers = inliers;
+		output.matches = matches;
+		output.icpInliersRatio = icpInliersRatio;
+		output.icpTranslation = icpTranslation;
+		output.icpRotation = icpRotation;
+		output.icpStructuralComplexity = icpStructuralComplexity;
+		return output;
+	}
+
+	cv::Mat covariance;
 	std::string rejectedMsg;
+	double totalTime;

 	// RegistrationVis
 	int inliers;
 	std::vector<int> inliersIDs;
 	int matches;
 	std::vector<int> matchesIDs;
+	std::vector<int> projectedIDs; // "From" IDs

 	// RegistrationIcp
 	float icpInliersRatio;
 	float icpTranslation;
 	float icpRotation;
+	float icpStructuralComplexity;
 };

 }
@@ -61,6 +61,7 @@ protected:
 			RegistrationInfo & info) const;

 	virtual bool isImageRequiredImpl() const {return true;}
+	virtual bool canUseGuessImpl() const {return _correspondencesApproach != 0 || _guessWinSize>0;}
 	virtual int getMinVisualCorrespondencesImpl() const {return _minInliers;}

 private:
@@ -81,7 +82,9 @@ private:
 	int _flowMaxLevel;
 	float _nndr;
 	int _guessWinSize;
+	bool _guessMatchToProjection;
 	int _bundleAdjustment;
+	bool _depthAsMask;

 	ParametersMap _featureParameters;
 	ParametersMap _bundleParameters;
@@ -71,6 +71,7 @@ public:
 			const SensorData & data,
 			Transform odomPose,
 			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
+			const std::vector<float> & odomVelocity = std::vector<float>(),
 			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 	// for convenience
 	bool process(
@@ -78,6 +79,7 @@ public:
 			Transform odomPose,
 			float odomLinearVariance,
 			float odomAngularVariance,
+			const std::vector<float> & odomVelocity = std::vector<float>(),
 			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 	// for convenience, loop closure detection only
 	bool process(
@@ -116,6 +118,7 @@ public:
 	const Statistics & getStatistics() const;
 	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
 	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
+	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
@@ -126,6 +129,7 @@ public:
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms

+	void setInitialPose(const Transform & initialPose);
 	int triggerNewMap();
 	bool labelLocation(int id, const std::string & label);
 	/**
@@ -149,7 +153,8 @@ public:
 	void parseParameters(const ParametersMap & parameters);
 	const ParametersMap & getParameters() const {return _parameters;}
 	void setWorkingDirectory(std::string path);
-	void rejectLoopClosure(int oldId, int newId);
+	void rejectLastLoopClosure();
+	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses);
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
@@ -161,13 +166,20 @@ 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
 	bool computePath(int targetNode, bool global);
-	bool computePath(const Transform & targetPose); // only in current optimized map
+	bool computePath(const Transform & targetPose, float tolerance = -1.0f); // only in current optimized map, tolerance (m) < 0 means RGBD/LocalRadius, 0 means infinite
 	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
 	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
 	std::vector<int> getPathNextNodes() const;
@@ -177,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;
@@ -186,6 +198,7 @@ private:
 	void optimizeCurrentMap(int id,
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
+			cv::Mat & covariance,
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
@@ -194,6 +207,7 @@ private:
 			const std::set<int> & ids,
 			const std::map<int, Transform> & guessPoses,
 			bool lookInDatabase,
+			cv::Mat & covariance,
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
@@ -209,10 +223,14 @@ private:
 	bool _publishLastSignatureData;
 	bool _publishPdf;
 	bool _publishLikelihood;
+	bool _publishRAMUsage;
+	bool _computeRMSE;
+	bool _saveWMState;
 	float _maxTimeAllowed; // in ms
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
 	float _loopRatio;
+	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
 	bool _rawDataKept;
@@ -222,6 +240,8 @@ private:
 	bool _rgbdSlamMode;
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
+	float _rgbdLinearSpeedUpdate;
+	float _rgbdAngularSpeedUpdate;
 	float _newMapOdomChangeDistance;
 	bool _neighborLinkRefining;
 	bool _proximityByTime;
@@ -237,13 +257,18 @@ private:
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
-	float _optimizationMaxLinearError;
+	float _optimizationMaxError;
 	bool _startNewMapOnLoopClosure;
+	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters
 	bool _goalsSavedInUserData;
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
+	bool _savedLocalizationIgnored;
+	bool _loopCovLimited;
+	bool _loopGPS;
+	int _maxOdomCacheSize;

 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -275,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;
@@ -288,6 +317,5 @@ private:

 };

-#endif /* RTABMAP_H_ */
-
 } // namespace rtabmap
+#endif /* RTABMAP_H_ */
@@ -66,7 +66,6 @@ public:
 		kStateCleanDataBuffer,
 		kStatePublishingMap,
 		kStateTriggeringMap,
-		kStateAddingUserData,
 		kStateSettingGoal,
 		kStateCancellingGoal,
 		kStateLabelling
@@ -114,6 +113,7 @@ private:
 	std::queue<ParametersMap> _stateParam;

 	std::list<OdometryEvent> _dataBuffer;
+	std::list<double> _newMapEvents;
 	UMutex _dataMutex;
 	USemaphore _dataAdded;
 	unsigned int _dataBufferMaxSize;
@@ -125,8 +125,7 @@ private:
 	Rtabmap * _rtabmap;
 	bool _paused;
 	Transform lastPose_;
-	double _rotVariance;
-	double _transVariance;
+	cv::Mat covariance_;

 	cv::Mat _userData;
 	UMutex _userDataMutex;
@@ -33,9 +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/LaserScanInfo.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
 {
@@ -75,8 +79,7 @@ public:

 	// RGB-D constructor + laser scan
 	SensorData(
-			const cv::Mat & laserScan,
-			const LaserScanInfo & laserScanInfo,
+			const LaserScan & laserScan,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const CameraModel & cameraModel,
@@ -95,8 +98,7 @@ public:

 	// Multi-cameras RGB-D constructor + laser scan
 	SensorData(
-			const cv::Mat & laserScan,
-			const LaserScanInfo & laserScanInfo,
+			const LaserScan & laserScan,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const std::vector<CameraModel> & cameraModels,
@@ -115,8 +117,7 @@ public:

 	// Stereo constructor + laser scan
 	SensorData(
-			const cv::Mat & laserScan,
-			const LaserScanInfo & laserScanInfo,
+			const LaserScan & laserScan,
 			const cv::Mat & left,
 			const cv::Mat & right,
 			const StereoCameraModel & cameraModel,
@@ -124,7 +125,13 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());

-	virtual ~SensorData() {}
+	// IMU constructor
+	SensorData(
+			const IMU & imu,
+			int id = 0,
+			double stamp = 0.0);
+
+	virtual ~SensorData();

 	bool isValid() const {
 		return !(_id == 0 &&
@@ -133,32 +140,46 @@ public:
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
 			_depthOrRightCompressed.empty() &&
-			_laserScanRaw.empty() &&
-			_laserScanCompressed.empty() &&
+			_laserScanRaw.isEmpty() &&
+			_laserScanCompressed.isEmpty() &&
 			_cameraModels.size() == 0 &&
 			!_stereoCameraModel.isValidForProjection() &&
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
-			_descriptors.empty());
+			_descriptors.empty() &&
+			imu_.empty());
 	}

 	int id() const {return _id;}
 	void setId(int id) {_id = id;}
 	double stamp() const {return _stamp;}
 	void setStamp(double stamp) {_stamp = stamp;}
-	const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}

 	const cv::Mat & imageCompressed() const {return _imageCompressed;}
 	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
-	const cv::Mat & laserScanCompressed() const {return _laserScanCompressed;}
+	const LaserScan & laserScanCompressed() const {return _laserScanCompressed;}

 	const cv::Mat & imageRaw() const {return _imageRaw;}
 	const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
-	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
-	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
-	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
-	void setLaserScanRaw(const cv::Mat & laserScanRaw, const LaserScanInfo & info) {_laserScanRaw =laserScanRaw;_laserScanInfo = info;}
+	const LaserScan & laserScanRaw() const {return _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;}
@@ -167,34 +188,41 @@ 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,
 			cv::Mat * depthOrRightRaw,
-			cv::Mat * laserScanRaw = 0,
+			LaserScan * laserScanRaw = 0,
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
-			cv::Mat * obstacleCellsRaw = 0);
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0);
 	void uncompressDataConst(
 			cv::Mat * imageRaw,
 			cv::Mat * depthOrRightRaw,
-			cv::Mat * laserScanRaw = 0,
+			LaserScan * laserScanRaw = 0,
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
-			cv::Mat * obstacleCellsRaw = 0) const;
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0) const;

 	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;}

@@ -202,6 +230,7 @@ public:
 	void setOccupancyGrid(
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewPoint);
 	// remove raw occupancy grids
@@ -210,6 +239,8 @@ public:
 	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
 	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
 	const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
+	const cv::Mat & gridEmptyCellsRaw() const {return _emptyCellsRaw;}
+	const cv::Mat & gridEmptyCellsCompressed() const {return _emptyCellsCompressed;}
 	float gridCellSize() const {return _cellSize;}
 	const cv::Point3f & gridViewPoint() const {return _viewPoint;}

@@ -221,7 +252,36 @@ public:
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}

+	void setGlobalPose(const Transform & pose, const cv::Mat & covariance) {globalPose_ = pose; globalPoseCovariance_ = covariance;}
+	const Transform & globalPose() const {return globalPose_;}
+	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
+
+	void setGPS(const GPS & gps) {gps_ = gps;}
+	const GPS & gps() const {return gps_;}
+
+	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
+	/**
+	 * 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

 private:
 	int _id;
@@ -229,17 +289,15 @@ private:

 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image
-	cv::Mat _laserScanCompressed;      // compressed data
+	LaserScan _laserScanCompressed;      // compressed data

 	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
 	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
-	cv::Mat _laserScanRaw;      // CV_32FC2 or CV_32FC3
+	LaserScan _laserScanRaw;

 	std::vector<CameraModel> _cameraModels;
 	StereoCameraModel _stereoCameraModel;

-	LaserScanInfo _laserScanInfo;
-
 	// user data
 	cv::Mat _userDataCompressed;      // compressed data
 	cv::Mat _userDataRaw;
@@ -247,17 +305,32 @@ private:
 	// occupancy grid
 	cv::Mat _groundCellsCompressed;
 	cv::Mat _obstacleCellsCompressed;
+	cv::Mat _emptyCellsCompressed;
 	cv::Mat _groundCellsRaw;
 	cv::Mat _obstacleCellsRaw;
+	cv::Mat _emptyCellsRaw;
 	float _cellSize;
 	cv::Point3f _viewPoint;

+	// environmental sensors
+	EnvSensors _envSensors;
+
+	// landmarks
+	Landmarks _landmarks;
+
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	std::vector<cv::Point3f> _keypoints3D;
 	cv::Mat _descriptors;

 	Transform groundTruth_;
+
+	Transform globalPose_;
+	cv::Mat globalPoseCovariance_; // 6x6 double
+
+	GPS gps_;
+
+	IMU imu_;
 };

 }
@@ -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;}
@@ -115,20 +118,33 @@ public:
 	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
+	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
+		_velocity = std::vector<float>(6,0);
+		_velocity[0]=vx;
+		_velocity[1]=vy;
+		_velocity[2]=vz;
+		_velocity[3]=vroll;
+		_velocity[4]=vpitch;
+		_velocity[5]=vyaw;
+	}

 	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
+	const std::vector<float> & getVelocity() const {return _velocity;}

 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}

+	long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
+
 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
@@ -147,6 +163,7 @@ private:

 	Transform _pose;
 	Transform _groundTruthPose;
+	std::vector<float> _velocity;

 	SensorData _sensorData;
 };
@@ -64,8 +64,13 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Visual_matches,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
+	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
 	RTABMAP_STATS(Loop, Optimization_error, );
 	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,);
@@ -77,7 +82,10 @@ class RTABMAP_EXP Statistics

 	RTABMAP_STATS(NeighborLinkRefining, Accepted,);
 	RTABMAP_STATS(NeighborLinkRefining, Inliers,);
-	RTABMAP_STATS(NeighborLinkRefining, Inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,);
 	RTABMAP_STATS(NeighborLinkRefining, Variance,);
 	RTABMAP_STATS(NeighborLinkRefining, Pts,);

@@ -95,13 +103,17 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
 	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Fast_movement,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
+	RTABMAP_STATS(Memory, RAM_usage, MB);
+	RTABMAP_STATS(Memory, Triangulated_points, );

 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
 	RTABMAP_STATS(Timing, Proximity_by_time, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
 	RTABMAP_STATS(Timing, Reactivation, ms);
@@ -117,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);
@@ -125,19 +138,34 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Subpixel, ms);
 	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
+	RTABMAP_STATS(TimingMem, Rectification, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_3D_motion, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
 	RTABMAP_STATS(TimingMem, Add_new_words, ms);
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
 	RTABMAP_STATS(TimingMem, Post_decimation, ms);
-	RTABMAP_STATS(TimingMem, Scan_downsampling, ms);
-	RTABMAP_STATS(TimingMem, Scan_normals, 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);
 	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);

+	RTABMAP_STATS(Gt, Translational_rmse, m);
+	RTABMAP_STATS(Gt, Translational_mean, m);
+	RTABMAP_STATS(Gt, Translational_median, m);
+	RTABMAP_STATS(Gt, Translational_std, m);
+	RTABMAP_STATS(Gt, Translational_min, m);
+	RTABMAP_STATS(Gt, Translational_max, m);
+	RTABMAP_STATS(Gt, Rotational_rmse, deg);
+	RTABMAP_STATS(Gt, Rotational_mean, deg);
+	RTABMAP_STATS(Gt, Rotational_median, deg);
+	RTABMAP_STATS(Gt, Rotational_std, deg);
+	RTABMAP_STATS(Gt, Rotational_min, deg);
+	RTABMAP_STATS(Gt, Rotational_max, deg);
+
 public:
 	static const std::map<std::string, float> & defaultData();
 	static std::string serializeData(const std::map<std::string, float> & data);
@@ -152,17 +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;}
@@ -170,20 +203,26 @@ public:
 	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
 	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 	void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
+	void setWmState(const std::vector<int> & state) {_wmState = state;}

 	// 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;}
@@ -191,6 +230,7 @@ public:
 	const std::vector<int> & localPath() const {return _localPath;}
 	int currentGoalId() const {return _currentGoalId;}
 	const std::map<int, int> & reducedIds() const {return _reducedIds;}
+	const std::vector<int> & wmState() const {return _wmState;}

 	const std::map<std::string, float> & data() const {return _data;}

@@ -198,17 +238,22 @@ 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;
 	Transform _mapCorrection;
 	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;
@@ -219,6 +264,8 @@ private:

 	std::map<int, int> _reducedIds;

+	std::vector<int> _wmState;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}
@@ -53,8 +53,8 @@ public:
 	cv::Size winSize() const {return cv::Size(winWidth_, winHeight_);}
 	int iterations() const   {return iterations_;}
 	int maxLevel() const     {return maxLevel_;}
-	int minDisparity() const {return minDisparity_;}
-	int maxDisparity() const {return maxDisparity_;}
+	float minDisparity() const {return minDisparity_;}
+	float maxDisparity() const {return maxDisparity_;}
 	bool winSSD() const      {return winSSD_;}

 private:
@@ -62,8 +62,8 @@ private:
 	int winHeight_;
 	int iterations_;
 	int maxLevel_;
-	int minDisparity_;
-	int maxDisparity_;
+	float minDisparity_;
+	float maxDisparity_;
 	bool winSSD_;
 };

@@ -86,6 +86,7 @@ public:
 	bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}

 	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
+	bool isRectificationMapInitialized() {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();}

 	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
 	const std::string & name() const {return name_;}
@@ -96,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;}

@@ -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_ */
--- a/Show More
+++ b/Show More