Update package.xml

Bump package version
RegistrationIcp: Fixed invalid variance set when structural complexity is low and input doesn't have normals. DBViewer: added check on angular error after optimizing the graph when adding a new constraint. Memory: added warning if odom angular variance detected is very high.
2026-09-08 12:30:20 +08:00 · 2018-09-29 11:58:58 -04:00 · 2018-09-28 22:02:07 -04:00 · 2018-09-28 15:37:24 -04:00 · 2018-09-28 09:45:43 -04:00 · 2018-09-27 16:08:55 -04:00
156 changed files with 9428 additions and 1889 deletions
@@ -18,9 +18,11 @@ init:

 install:
  # Qt
-  - set QTDIR=C:\Qt\5.8\msvc2015_64
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
  - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
  # Openni2
  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
  - cmd: OpenNI2.exe -o"C:\Program Files" -y
@@ -36,24 +38,59 @@ install:
  - ECHO "Installed OpenCV:"
  - ps: "ls \"C:/Program Files/opencv/build\""
  - set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
-  # PCL (including QVTK)
-  - ps: wget 'https://dl.dropboxusercontent.com/s/atf4r8kb1xyc1ls/PCL%201.8.1.exe?dl=0' -outfile PCL_1.8.1.exe
+  # VTK (including QVTK)
+  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
+  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/r9tvi9md54zlul2/PCL-1_8_1-July2018-msvc140.exe?dl=0' -outfile PCL_1.8.1.exe
  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
  - ECHO "Installed PCL:"
-  - ps: "ls \"C:/Program Files/PCL 1.8.1\""
-  - set PATH=%PATH%;C:\Program Files\PCL 1.8.1\bin
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
  # zlib
  - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
  - ECHO "Installed zlib:"
  - ps: "ls \"C:/Program Files/zlib\""
  - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
  
 before_build:
  - cd c:\projects\rtabmap\build
  - ECHO %PROGRAMFILES%
  - ECHO %PATH%
-  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL 1.8.1\cmake" -DZLIB_ROOT="C:\Program Files\zlib" ..
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DZLIB_ROOT="C:\Program Files\zlib" -DBUILD_AS_BUNDLE=ON ..
  
 after_build :
  - cmake --build . --config Release --target package
@@ -2,6 +2,7 @@
 .DS_Store
 .settings/language.settings.xml
 .idea/
+.vscode
 cmake-build-debug/
 app/android/.classpath
 app/android/.project
@@ -20,6 +20,7 @@ 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: 
@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 17)
-SET(RTABMAP_PATCH_VERSION 1)
+SET(RTABMAP_PATCH_VERSION 6)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -131,9 +131,9 @@ IF(ANDROID_PREBUILD)
   return()
 ENDIF(ANDROID_PREBUILD)

-IF(APPLE)
-    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
-ENDIF(APPLE)
+IF(APPLE OR WIN32)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle with all embedded dependencies (DragNDrop for Mac, installer for Windows)" OFF)
+ENDIF(APPLE OR WIN32)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
@@ -155,10 +155,12 @@ 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)
@@ -167,6 +169,7 @@ 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(PCL_OMP            "With PCL OMP implementations"         ON)

 FIND_PACKAGE(OpenCV REQUIRED QUIET)
@@ -184,6 +187,11 @@ endif()

 FIND_PACKAGE(ZLIB REQUIRED QUIET)

+FIND_PACKAGE(Sqlite3 QUIET)
+IF(Sqlite3_FOUND)
+   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
+ENDIF(Sqlite3_FOUND)
+
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
   # fix libproj.so not found on Xenial
   list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
@@ -244,7 +252,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)

@@ -327,6 +340,13 @@ IF(WITH_POINTMATCHER)
    ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)

+IF(WITH_LOAM)
+    find_package(loam_velodyne QUIET)
+    IF(loam_velodyne_FOUND)
+       MESSAGE(STATUS "Found loam_velodyne: ${loam_velodyne_INCLUDE_DIRS}")
+    ENDIF(loam_velodyne_FOUND)
+ENDIF(WITH_LOAM)
+
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
    IF(WIN32) # Windows
@@ -371,6 +391,17 @@ 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)
@@ -429,6 +460,14 @@ IF(WITH_OKVIS)
    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_ORB_SLAM2 AND NOT G2O_FOUND)
    FIND_PACKAGE(ORB_SLAM2 QUIET)
    IF(ORB_SLAM2_FOUND)
@@ -445,7 +484,18 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
    ENDIF(ORB_SLAM2_FOUND)
 ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)

-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
+IF(loam_velodyne_FOUND)
+   #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)
   #Newest versions require std11
   IF(NOT MSVC)
     include(CheckCXXCompilerFlag)
@@ -456,10 +506,10 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SL
      ELSEIF(COMPILER_SUPPORTS_CXX0X)
        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
      ELSE()
-        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
      ENDIF()
   ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
+ENDIF()

 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -525,6 +575,9 @@ 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()
@@ -568,6 +621,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()
@@ -603,6 +661,11 @@ IF(NOT okvis_FOUND)
 ELSE()
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) 
 ENDIF()
+IF(NOT msckf_vio_FOUND)
+   SET(MSCKF_VIO "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${msckf_vio_LIBRARIES}) 
+ENDIF()
 IF(NOT ORB_SLAM2_FOUND)
   SET(ORB_SLAM2 "//")
 ELSE()
@@ -618,9 +681,9 @@ IF(VTK_USE_QVTK)
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) 
 ENDIF(VTK_USE_QVTK)

-IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+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 )
@@ -740,7 +803,11 @@ IF(WIN32)
  ELSE() 
    SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES") 
  ENDIF()
-  SET(CPACK_GENERATOR "ZIP;NSIS")
+  IF(BUILD_AS_BUNDLE)
+    SET(CPACK_GENERATOR "ZIP;NSIS")
+  ELSE()
+    SET(CPACK_GENERATOR "ZIP")
+  ENDIF()
  SET(CPACK_SOURCE_GENERATOR "ZIP")
  SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}")
  SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/src/${PROJECT_NAME}.ico")
@@ -795,28 +862,35 @@ 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 "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")

+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 3 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 3 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(Freenect_FOUND)
 MESSAGE(STATUS "  With Freenect             = YES (License: Apache v2 and/or GPLv2)")
 ELSEIF(NOT WITH_FREENECT)
@@ -872,19 +946,19 @@ 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)
@@ -906,11 +980,19 @@ MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()

 IF(libpointmatcher_FOUND)
-MESSAGE(STATUS "  With libpointmatcher      = YES (License: BSD)")
+MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
 ELSEIF(NOT WITH_POINTMATCHER)
-MESSAGE(STATUS "  With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
+MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
 ELSE()
-MESSAGE(STATUS "  With libpointmatcher      = NO (libpointmatcher not found)")
+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)
@@ -940,6 +1022,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)
@@ -990,12 +1080,20 @@ ENDIF()

 IF(okvis_FOUND)
 MESSAGE(STATUS "  With okvis                = YES (License: BSD)")
-ELSEIF(NOT WITH_DVO)
+ELSEIF(NOT WITH_OKVIS)
 MESSAGE(STATUS "  With okvis                = NO (WITH_OKVIS=OFF)")
 ELSE()
 MESSAGE(STATUS "  With okvis                = NO (okvis not found)")
 ENDIF()

+IF(msckf_vio_FOUND)
+MESSAGE(STATUS "  With msckf_vio            = YES (License: Penn Software License)")
+ELSEIF(NOT WITH_MSCKF_VIO)
+MESSAGE(STATUS "  With msckf_vio            = NO (WITH_MSCKF_VIO=OFF)")
+ELSE()
+MESSAGE(STATUS "  With msckf_vio            = NO (msckf_vio not found)")
+ENDIF()
+
 IF(ORB_SLAM2_FOUND)
 MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
 ELSEIF(NOT WITH_ORB_SLAM2)
@@ -50,11 +50,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@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
@@ -62,6 +64,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@VISO2@#define RTABMAP_VISO2
@DVO@#define RTABMAP_DVO
@OKVIS@#define RTABMAP_OKVIS
+@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
@ORB_SLAM2@#define RTABMAP_ORB_SLAM2

 #endif /* VERSION_H_ */
@@ -104,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
        WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
       
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
  SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
  SET(plugin_dest_dir bin)
  SET(qtconf_dest_dir bin)
@@ -189,5 +189,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;
 	}
@@ -49,7 +49,7 @@ int main(int argc, char* argv[])
 	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.
    
@@ -85,9 +85,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);
    }
@@ -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)

@@ -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;}
@@ -79,6 +79,15 @@ namespace rs
 	}
 }

+namespace rs2
+{
+	class context;
+	class device;
+	class syncer;
+}
+struct rs2_intrinsics;
+struct rs2_extrinsics;
+
 typedef struct _freenect_context freenect_context;
 typedef struct _freenect_device freenect_device;

@@ -370,6 +379,7 @@ class RTABMAP_EXP CameraRealSense :
 {
 public:
 	static bool available();
+	enum RGBSource {kColor, kInfrared, kFishEye};

 public:
 	// default local transform z in, x right, y down));
@@ -382,6 +392,8 @@ public:
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();

+	void setDepthScaledToRGBSize(bool enabled);
+	void setRGBSource(RGBSource source);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
@@ -398,6 +410,10 @@ private:
 	int presetRGB_;
 	int presetDepth_;
 	bool computeOdometry_;
+	bool depthScaledToRGBSize_;
+	RGBSource rgbSource_;
+	CameraModel cameraModel_;
+	std::vector<int> rsRectificationTable_;

 	int motionSeq_[2];
 	rs::slam::slam * slam_;
@@ -409,6 +425,53 @@ private:
 	USemaphore dataReady_;
 #endif
 };
+/////////////////////////
+// CameraRealSense2
+/////////////////////////
+class slam_event_handler;
+class RTABMAP_EXP CameraRealSense2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraRealSense2(
+		const std::string & deviceId = "",
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	// parameters are set during initialization
+	void setEmitterEnabled(bool enabled);
+	void setIRDepthFormat(bool enabled);
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE2
+	rs2::context * ctx_;
+	rs2::device * dev_;
+	std::string deviceId_;
+	rs2::syncer * syncer_;
+	float depth_scale_meters_;
+	rs2_intrinsics * depthIntrinsics_;
+	rs2_intrinsics * rgbIntrinsics_;
+	rs2_extrinsics * depthToRGBExtrinsics_;
+	cv::Mat depthBuffer_;
+	cv::Mat rgbBuffer_;
+	CameraModel model_;
+
+	bool emitterEnabled_;
+	bool irDepth_;
+#endif
+};


 /////////////////////////
@@ -123,7 +123,7 @@ public:
 			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
+			bool selfCalibration = true);
 	CameraStereoZed(
 			const std::string & svoFilePath,
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
@@ -132,7 +132,7 @@ public:
 			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
+			bool selfCalibration = true);
 	virtual ~CameraStereoZed();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -226,6 +226,12 @@ public:
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int deviceLeft,
+			int deviceRight,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -245,6 +251,7 @@ private:
 	std::string cameraName_;
 	CameraVideo::Source src_;
 	int usbDevice_;
+	int usbDevice2_;
 };

 } // namespace rtabmap
@@ -179,7 +179,6 @@ public:
 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;
@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
-#include "sqlite3/sqlite3.h"
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;

 namespace rtabmap {

@@ -48,7 +50,7 @@ public:
 	void setSynchronous(int synchronous);
 	void setTempStore(int tempStore);

-private:
+protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
@@ -175,10 +177,12 @@ private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;

-private:
+protected:
 	sqlite3 * _ppDb;
-	long _memoryUsedEstimate;
 	std::string _version;
+
+private:
+	long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;
@@ -34,9 +34,6 @@ public:
 			linearAccelerationCovariance_(linearAccelerationCovariance),
 			localTransform_(localTransform)
 	{
-		UASSERT(!orientationCovariance.empty() && orientationCovariance.cols == 3 && orientationCovariance.rows == 3 && orientationCovariance.type() == CV_64FC1);
-		UASSERT(!angularVelocityCovariance.empty() && angularVelocityCovariance.cols == 3 && angularVelocityCovariance.rows == 3 && angularVelocityCovariance.type() == CV_64FC1);
-		UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
 	}
 	IMU(const cv::Vec3d & angularVelocity,
 		const cv::Mat & angularVelocityCovariance,
@@ -49,8 +46,6 @@ public:
 			linearAccelerationCovariance_(linearAccelerationCovariance),
 			localTransform_(localTransform)
 	{
-		UASSERT(!angularVelocityCovariance.empty() && angularVelocityCovariance.cols == 3 && angularVelocityCovariance.rows == 3 && angularVelocityCovariance.type() == CV_64FC1);
-		UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
 	}

 	const cv::Vec4d & orientation() const {return orientation_;}
@@ -66,7 +61,7 @@ public:

 	bool empty() const
 	{
-		return orientationCovariance_.empty() && angularVelocityCovariance_.empty() && linearAccelerationCovariance_.empty();
+		return localTransform_.isNull();
 	}


@@ -171,6 +171,7 @@ 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;
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
 	Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
@@ -290,8 +291,9 @@ private:
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
-	int _laserScanNormalRadius;
+	float _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
+	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
@@ -300,6 +302,8 @@ private:
 	int _visMaxFeatures;
 	int _visCorType;
 	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
+	bool _covOffDiagonalIgnored;

 	int _idCount;
 	int _idMapCount;
@@ -309,6 +313,8 @@ private:
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	GPS _gpsOrigin;
+	std::vector<CameraModel> _rectCameraModels;
+	StereoCameraModel _rectStereoCameraModel;

 	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
@@ -39,6 +39,17 @@ namespace rtabmap {

 class RTABMAP_EXP OccupancyGrid
 {
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
@@ -53,6 +64,7 @@ public:
 	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
+	const std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > & getCache() const {return cache_;}

 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -87,6 +99,7 @@ public:
 			const cv::Mat & empty);
 	void update(const std::map<int, Transform> & poses);
 	cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getProbMap(float & xMin, float & yMin) const;
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
@@ -125,6 +138,11 @@ private:
 	bool erode_;
 	float footprintRadius_;
 	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;

 	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
 	cv::Mat map_;
@@ -50,7 +50,9 @@ public:
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
 		kTypeORBSLAM2 = 5,
-		kTypeOkvis = 6
+		kTypeOkvis = 6,
+		kTypeLOAM = 7,
+		kTypeMSCKF = 8
 	};

 public:
@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYLOAM_H_
+#define ODOMETRYLOAM_H_
+
+#include <rtabmap/core/Odometry.h>
+
+#ifdef RTABMAP_LOAM
+#include <loam_velodyne/BasicScanRegistration.h>
+#include <loam_velodyne/BasicLaserOdometry.h>
+#include <loam_velodyne/BasicLaserMapping.h>
+#include <loam_velodyne/BasicTransformMaintenance.h>
+#include <loam_velodyne/MultiScanRegistration.h>
+#endif
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryLOAM : public Odometry
+{
+public:
+	OdometryLOAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryLOAM();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeLOAM;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_LOAM
+	std::vector<pcl::PointCloud<pcl::PointXYZI> > segmentScanRings(const pcl::PointCloud<pcl::PointXYZ> & laserCloudIn);
+
+	loam::BasicScanRegistration scanRegistration_;
+	loam::MultiScanMapper scanMapper_;
+	loam::BasicLaserOdometry * laserOdometry_;
+	loam::BasicLaserMapping * laserMapping_;
+	loam::BasicTransformMaintenance transformMaintenance_;
+	Transform lastPose_;
+	float scanPeriod_;
+	float linVar_;
+	float angVar_;
+	bool localMapping_;
+	bool lost_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYLOAM_H_ */
@@ -0,0 +1,65 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYMSCKF_H_
+#define ODOMETRYMSCKF_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace rtabmap {
+
+class ImageProcessorNoROS;
+class MsckfVioNoROS;
+
+class RTABMAP_EXP OdometryMSCKF : public Odometry
+{
+public:
+	OdometryMSCKF(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryMSCKF();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeMSCKF;}
+	virtual bool canProcessRawImages() const {return true;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_MSCKF_VIO
+	ImageProcessorNoROS * imageProcessor_;
+	MsckfVioNoROS * msckf_;
+	IMU lastImu_;
+	ParametersMap parameters_;
+	Transform flipXY_;
+	Transform previousPose_;
+	bool initGravity_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYMSCKF_H_ */
@@ -54,8 +54,9 @@ private:
 #ifdef RTABMAP_ORB_SLAM2
 	ORBSLAM2System * orbslam2_;
 	bool firstFrame_;
-#endif
 	Transform originLocalTransform_;
+	Transform previousPose_;
+#endif

 };

@@ -55,10 +55,12 @@ private:
 #ifdef RTABMAP_OKVIS
 	OkvisCallbackHandler * okvisCallbackHandler_;
 	okvis::ThreadedKFVio * okvisEstimator_;
+	int imagesProcessed_;
+	bool initGravity_;
 #endif
 	ParametersMap okvisParameters_;
 	IMU lastImu_; // only used for initialization
-	int imagesProcessed_;
+	Transform previousPose_;
 };

 }
@@ -95,14 +95,23 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);

+	std::map<int, Transform> optimize(
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				cv::Mat & outputCovariance,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,
@@ -64,12 +64,13 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);

 	virtual std::map<int, Transform> optimize(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & edgeConstraints,
+				cv::Mat & outputCovariance,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);

 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,
@@ -56,6 +56,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
+			cv::Mat & outputCovariance,
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 			double * finalError = 0,
 			int * iterationsDone = 0);
@@ -66,6 +66,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
+			cv::Mat & outputCovariance,
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 			double * finalError = 0,
 			int * iterationsDone = 0);
@@ -175,8 +175,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Rtabmap, PublishRAMUsage,              bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
    RTABMAP_PARAM(Rtabmap, ComputeRMSE,                  bool, true,  "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
    RTABMAP_PARAM(Rtabmap, SaveWMState,                  bool, false, "Save working memory state after each update in statistics.");
-    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for the detector (ms) (0 means infinity).");
-    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      "Maximum signatures in the Working Memory (ms) (0 means infinity).");
+    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for map update (ms) (0 means infinity). When map update time exceeds this fixed time threshold, some nodes in Working Memory (WM) are transferred to Long-Term Memory to limit the size of the WM and decrease the update time.");
+    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      uFormat("Maximum nodes in the Working Memory (0 means infinity). Similar to \"%s\", when the number of nodes in Working Memory (WM) exceeds this treshold, some nodes are transferred to Long-Term Memory to keep WM size fixed.", kRtabmapTimeThr().c_str()));
    RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
    RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
    RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
@@ -186,7 +186,9 @@ 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.");
@@ -218,8 +220,9 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
    RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
    RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       float, 0.0,     "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features.");
+    RTABMAP_PARAM(Mem, 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");
@@ -282,8 +285,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, "Used with FAST GPU.");
    RTABMAP_PARAM(FAST, MinThreshold,       int, 7,       "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
    RTABMAP_PARAM(FAST, MaxThreshold,       int, 200,     "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
-    RTABMAP_PARAM(FAST, GridRows,           int, 4,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
-    RTABMAP_PARAM(FAST, GridCols,           int, 4,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridRows,           int, 0,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridCols,           int, 0,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");

    RTABMAP_PARAM(GFTT, QualityLevel,      double, 0.001, "");
    RTABMAP_PARAM(GFTT, MinDistance,       double, 3,    "");
@@ -348,6 +351,7 @@ 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.");

    // Local/Proximity loop closure detection
@@ -393,7 +397,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");

    // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO");
    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).");
@@ -490,6 +494,42 @@ class RTABMAP_EXP Parameters
    // Odometry OKVIS
    RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");

+    // Odometry LOAM
+    RTABMAP_PARAM(OdomLOAM, Sensor,     int,    2,    "Velodyne sensor: 0=VLP-16, 1=HDL-32, 2=HDL-64E");
+    RTABMAP_PARAM(OdomLOAM, ScanPeriod, float,  0.1,  "Scan period (s)");
+    RTABMAP_PARAM(OdomLOAM, LinVar,     float,  0.01,  "Linear output variance.");
+    RTABMAP_PARAM(OdomLOAM, AngVar,     float,  0.01,  "Angular output variance.");
+    RTABMAP_PARAM(OdomLOAM, LocalMapping, bool,  true,  "Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.");
+
+    // Odometry MSCKF_VIO
+    RTABMAP_PARAM(OdomMSCKF, GridRow,           int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridCol,           int,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMinFeatureNum, int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMaxFeatureNum, int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, PyramidLevels,     int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, PatchSize,         int,  15,  "");
+    RTABMAP_PARAM(OdomMSCKF, FastThreshold,     int,  10,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxIteration,      int,  30,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackPrecision,    double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, RansacThreshold,   double,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, StereoThreshold,   double,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, PositionStdThreshold,    double,  8.0,  "");
+    RTABMAP_PARAM(OdomMSCKF, RotationThreshold,       double,  0.2618,  "");
+    RTABMAP_PARAM(OdomMSCKF, TranslationThreshold,    double,  0.4,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackingRateThreshold,   double,  0.5,  "");
+    RTABMAP_PARAM(OdomMSCKF, OptTranslationThreshold, double,  0,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyro,     double,  0.005,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAcc,      double,  0.05,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyroBias, double,  0.001,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseFeature,  double,  0.035,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovVel,    double,  0.25,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovGyroBias, double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExRot,    double,  0.00030462,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExTrans,  double,  0.000025,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize,  int,  20,  "");
+
    // Common registration parameters
    RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
    RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -639,7 +679,11 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
    RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
    RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
-    RTABMAP_PARAM(GridGlobal, OctoMapOccupancyThr,  float,  0.5,     "OctoMap occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, OccupancyThr,         float,  0.5,     "Occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbHit,              float,  0.7,     "Probability of a hit (value between 0.5 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbMiss,             float,  0.4,     "Probability of a miss (value between 0 and 0.5).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMin,      float,  0.1192,  "Probability clamping minimum (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");

 public:
    virtual ~Parameters();
@@ -153,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,
@@ -171,7 +172,7 @@ public:
 	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;
@@ -190,6 +191,7 @@ private:
 	void optimizeCurrentMap(int id,
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
+			cv::Mat & covariance,
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
@@ -198,6 +200,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;
@@ -247,8 +250,9 @@ private:
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
-	float _optimizationMaxLinearError;
+	float _optimizationMaxError;
 	bool _startNewMapOnLoopClosure;
+	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters
 	bool _goalsSavedInUserData;
 	int _pathStuckIterations;
@@ -67,6 +67,8 @@ class RTABMAP_EXP Statistics
 	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(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);
@@ -104,6 +106,7 @@ class RTABMAP_EXP Statistics
 	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);
@@ -134,6 +137,7 @@ class RTABMAP_EXP Statistics
 	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);
@@ -183,6 +187,7 @@ public:
 	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 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;}
@@ -205,6 +210,7 @@ public:
 	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, int> & weights() const {return _weights;}
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
@@ -230,6 +236,7 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _loopClosureTransform;
+	cv::Mat _localizationCovariance;

 	std::map<int, int> _weights;
 	std::map<int, float> _posterior;
@@ -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_;}
@@ -68,6 +68,8 @@ SET(SRC_FILES
 	OdometryDVO.cpp
 	OdometryOkvis.cpp
 	OdometryORBSLAM2.cpp
+	OdometryLOAM.cpp
+	OdometryMSCKF.cpp
 	
 	IMUThread.cpp
 	
@@ -81,9 +83,7 @@ SET(SRC_FILES
 		
 	rtflann/ext/lz4.c
 	rtflann/ext/lz4hc.c
-	FlannIndex.cpp
-	
-	sqlite3/sqlite3.c	
+	FlannIndex.cpp	
 	
 	#clams stuff
 	clams/discrete_depth_distortion_model_helpers.cpp
@@ -127,6 +127,26 @@ SET(LIBRARIES
 	${ZLIB_LIBRARIES} 
 )

+IF(Sqlite3_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${Sqlite3_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${Sqlite3_LIBRARIES}
+	)
+ELSE()
+	SET(SRC_FILES
+		${SRC_FILES}
+		sqlite3/sqlite3.c
+	)
+	 SET(INCLUDE_DIRS
+	    ${CMAKE_CURRENT_SOURCE_DIR}/sqlite3
+		${INCLUDE_DIRS}
+	)
+ENDIF()
+
 IF(Freenect_FOUND)
 	IF(Freenect_DASH_INCLUDES)
 	   ADD_DEFINITIONS("-DFREENECT_DASH_INCLUDES")
@@ -185,6 +205,24 @@ IF(RealSense_FOUND)
 	)
 ENDIF(RealSense_FOUND)

+IF(realsense2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${realsense2_INCLUDE_DIRS}
+	)
+	IF(WIN32)
+	    SET(LIBRARIES
+			${LIBRARIES}
+			${RealSense2_LIBRARIES}
+		)
+	ELSE()
+		SET(LIBRARIES
+			${LIBRARIES}
+			realsense2
+		)
+	ENDIF()
+ENDIF(realsense2_FOUND)
+
 IF(DC1394_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -263,6 +301,17 @@ IF(libpointmatcher_FOUND)
 	)
 ENDIF(libpointmatcher_FOUND)

+IF(loam_velodyne_FOUND)
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${loam_velodyne_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${loam_velodyne_LIBRARIES}
+	)
+ENDIF(loam_velodyne_FOUND)
+
 IF(ZED_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -349,6 +398,17 @@ IF(okvis_FOUND)
 	)
 ENDIF(okvis_FOUND)

+IF(msckf_vio_FOUND)
+    SET(INCLUDE_DIRS
+		${msckf_vio_INCLUDE_DIRS}
+		${INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${msckf_vio_LIBRARIES}
+		${LIBRARIES}
+	)
+ENDIF(msckf_vio_FOUND)
+
 IF(ORB_SLAM2_FOUND)
    SET(INCLUDE_DIRS
        ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UStl.h>
 #include <opencv2/imgproc/imgproc.hpp>

 namespace rtabmap {
@@ -57,7 +58,7 @@ CameraModel::CameraModel(
 		localTransform_(localTransform)
 {
 	UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
-	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8) && D_.type() == CV_64FC1));
+	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8) && D_.type() == CV_64FC1));
 	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
 	UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
 }
@@ -153,12 +154,33 @@ CameraModel::CameraModel(
 void CameraModel::initRectificationMap()
 {
 	UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
-	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
+	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
 	UASSERT(R_.rows == 3 && R_.cols == 3);
 	UASSERT(P_.rows == 3 && P_.cols == 4);
 	// init rectification map
 	UINFO("Initialize rectify map");
-	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	if(D_.cols == 6)
+	{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+		// Equidistant / FishEye
+		// get only k parameters (k1,k2,p1,p2,k3,k4)
+		cv::Mat D(1, 4, CV_64FC1);
+		D.at<double>(0,0) = D_.at<double>(0,1);
+		D.at<double>(0,1) = D_.at<double>(0,2);
+		D.at<double>(0,2) = D_.at<double>(0,4);
+		D.at<double>(0,3) = D_.at<double>(0,5);
+		cv::fisheye::initUndistortRectifyMap(K_, D, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	}
+	else
+#else
+		UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+				CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+	}
+#endif
+	{
+		// RadialTangential
+		cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	}
 }

 void CameraModel::setImageSize(const cv::Size & size)
@@ -263,6 +285,27 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 				UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
 			}

+			n = fs["distortion_model"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				std::string distortionModel = (std::string)n;
+				if(D_.cols>=4 &&
+				  (uStrContains(distortionModel, "fisheye") ||
+				   uStrContains(distortionModel, "equidistant")))
+				{
+					cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+					D.at<double>(0,0) = D_.at<double>(0,0);
+					D.at<double>(0,1) = D_.at<double>(0,1);
+					D.at<double>(0,4) = D_.at<double>(0,2);
+					D.at<double>(0,5) = D_.at<double>(0,3);
+					D_ = D;
+				}
+			}
+			else
+			{
+				UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
+			}
+
 			n = fs["rectification_matrix"];
 			if(n.type() != cv::FileNode::NONE)
 			{
@@ -347,20 +390,33 @@ bool CameraModel::save(const std::string & directory) const

 		if(!D_.empty())
 		{
+			cv::Mat D = D_;
+			if(D_.cols == 6)
+			{
+				D = cv::Mat(1,4,CV_64FC1);
+				D.at<double>(0,0) = D_.at<double>(0,0);
+				D.at<double>(0,1) = D_.at<double>(0,1);
+				D.at<double>(0,2) = D_.at<double>(0,4);
+				D.at<double>(0,3) = D_.at<double>(0,5);
+			}
 			fs << "distortion_coefficients" << "{";
-			fs << "rows" << D_.rows;
-			fs << "cols" << D_.cols;
-			fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
+			fs << "rows" << D.rows;
+			fs << "cols" << D.cols;
+			fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
 			fs << "}";

 			// compaibility with ROS
-			if(D_.cols > 5)
+			if(D_.cols == 6)
 			{
-				fs << "distortion_model" << "rational_polynomial";
+				fs << "distortion_model" << "equidistant"; // equidistant, fisheye
+			}
+			else if(D.cols > 5)
+			{
+				fs << "distortion_model" << "rational_polynomial"; // rad tan
 			}
 			else
 			{
-				fs << "distortion_model" << "plumb_bob";
+				fs << "distortion_model" << "plumb_bob"; // rad tan
 			}
 		}

@@ -37,6 +37,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>

 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif

 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -120,14 +124,8 @@ CameraImages::CameraImages(const std::string & path,
 CameraImages::~CameraImages()
 {
 	UDEBUG("");
-	if(_dir)
-	{
-		delete _dir;
-	}
-	if(_scanDir)
-	{
-		delete _scanDir;
-	}
+	delete _dir;
+	delete _scanDir;
 }

 bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
@@ -39,6 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>

+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
+
 #ifdef RTABMAP_DC1394
 #include <dc1394/dc1394.h>
 #endif
@@ -361,10 +366,7 @@ CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localT
 CameraStereoDC1394::~CameraStereoDC1394()
 {
 #ifdef RTABMAP_DC1394
-	if(device_)
-	{
-		delete device_;
-	}
+	delete device_;
 #endif
 }

@@ -829,10 +831,7 @@ CameraStereoZed::CameraStereoZed(
 CameraStereoZed::~CameraStereoZed()
 {
 #ifdef RTABMAP_ZED
-	if(zed_)
-	{
-		delete zed_;
-	}
+	delete zed_;
 #endif
 }

@@ -855,7 +854,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	param.depth_mode=(sl::DEPTH_MODE)quality_;
 	param.coordinate_units=sl::UNIT_METER;
 	param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
-	param.sdk_verbose=false;
+	param.sdk_verbose=true;
 	param.sdk_gpu_id=-1;
 	param.depth_minimum_distance=-1;
 	param.camera_disable_self_calib=!selfCalibration_;
@@ -877,7 +876,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str

 	if(r!=sl::ERROR_CODE::SUCCESS)
 	{
-		UERROR("Camera initialization failed: \"%s\"", errorCode2str(r).c_str());
+		UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
 		delete zed_;
 		zed_ = 0;
 		return false;
@@ -888,19 +887,26 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	      quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
 	UDEBUG("");

-	zed_->setConfidenceThreshold(confidenceThr_);
+	if(quality_!=sl::DEPTH_MODE_NONE)
+	{
+		zed_->setConfidenceThreshold(confidenceThr_);
+	}

 	if (computeOdometry_)
 	{
 		sl::TrackingParameters tparam;
 		tparam.enable_spatial_memory=false;
 		zed_->enableTracking(tparam);
+		if(r!=sl::ERROR_CODE::SUCCESS)
+		{
+			UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
+		}
 	}

 	sl::CameraInformation infos = zed_->getCameraInformation();
 	sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
 	sl::Resolution res = stereoParams->left_cam.image_size;
-				
+
 	stereoModel_ = StereoCameraModel(
 		stereoParams->left_cam.fx, 
 		stereoParams->left_cam.fy, 
@@ -993,16 +999,17 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 	{
 		UTimer timer;
 		bool res = zed_->grab(rparam);
-		while (src_ == CameraVideo::kUsbDevice && res && timer.elapsed() < 2.0)
+		while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
 		{
 			// maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
 			uSleep(10);
 			res = zed_->grab(rparam);
 		}
-		if(!res)
+		if(res==sl::SUCCESS)
 		{
 			// get left image
-			sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_LEFT);
+			sl::Mat tmp;
+			zed_->retrieveImage(tmp,sl::VIEW_LEFT);
 			cv::Mat rgbaLeft = slMat2cvMat(tmp);

 			cv::Mat left;
@@ -1032,28 +1039,37 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 			if (computeOdometry_ && info)
 			{
 				sl::Pose pose;
-				zed_->getPosition(pose);
-				int trackingConfidence = pose.pose_confidence;
-				// FIXME What does pose_confidence == -1 mean?
-				if (trackingConfidence>0)
+				sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
+				if (tracking_state == sl::TRACKING_STATE_OK)
 				{
-					info->odomPose = zedPoseToTransform(pose);
-					if (!info->odomPose.isNull())
+					int trackingConfidence = pose.pose_confidence;
+					// FIXME What does pose_confidence == -1 mean?
+					if (trackingConfidence>0)
 					{
-						//transform x->forward, y->left, z->up
-						Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
-						info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
-
-						if (lost_)
+						info->odomPose = zedPoseToTransform(pose);
+						if (!info->odomPose.isNull())
 						{
-							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
-							lost_ = false;
-							UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+							//transform x->forward, y->left, z->up
+							Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+							info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
+
+							if (lost_)
+							{
+								info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+								lost_ = false;
+								UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+							}
+							else
+							{
+								info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+								UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+							}
 						}
 						else
 						{
-							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
-							UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+							lost_ = true;
+							UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
 						}
 					}
 					else
@@ -1065,9 +1081,7 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 				}
 				else
 				{
-					info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
-					lost_ = true;
-					UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
+					UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
 				}
 			}
 		}
@@ -1135,10 +1149,7 @@ CameraStereoImages::CameraStereoImages(
 CameraStereoImages::~CameraStereoImages()
 {
 	UDEBUG("");
-	if(camera2_)
-	{
-		delete camera2_;
-	}
+	delete camera2_;
 	UDEBUG("");
 }

@@ -1279,7 +1290,8 @@ CameraStereoVideo::CameraStereoVideo(
 		path_(path),
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
-		usbDevice_(0)
+		usbDevice_(0),
+		usbDevice2_(-1)
 {
 }

@@ -1294,7 +1306,8 @@ CameraStereoVideo::CameraStereoVideo(
 		path2_(pathRight),
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
-		usbDevice_(0)
+		usbDevice_(0),
+		usbDevice2_(-1)
 {
 }

@@ -1306,7 +1319,22 @@ CameraStereoVideo::CameraStereoVideo(
 	Camera(imageRate, localTransform),
 	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
-	usbDevice_(device)
+	usbDevice_(device),
+	usbDevice2_(-1)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int deviceLeft,
+	int deviceRight,
+	bool rectifyImages,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	rectifyImages_(rectifyImages),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(deviceLeft),
+	usbDevice2_(deviceRight)
 {
 }

@@ -1330,20 +1358,27 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s

 	if (src_ == CameraVideo::kUsbDevice)
 	{
-		ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
 		capture_.open(usbDevice_);
+		if(usbDevice2_ < 0)
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		}
+		else
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on devices %d and %d", usbDevice_, usbDevice2_);
+			capture2_.open(usbDevice2_);
+		}
 	}
 	else if (src_ == CameraVideo::kVideoFile)
 	{
+		capture_.open(path_.c_str());
 		if(path2_.empty())
 		{
 			ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
-			capture_.open(path_.c_str());
 		}
 		else
 		{
 			ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
-			capture_.open(path_.c_str());
 			capture2_.open(path2_.c_str());
 		}
 	}
@@ -1352,7 +1387,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
 	}

-	if(!capture_.isOpened() || (!path2_.empty() && !capture2_.isOpened()))
+	if(!capture_.isOpened() || ((!path2_.empty() || usbDevice2_>=0) && !capture2_.isOpened()))
 	{
 		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
@@ -1411,11 +1446,11 @@ SensorData CameraStereoVideo::captureImage(CameraInfo * info)
 	SensorData data;

 	cv::Mat img;
-	if(capture_.isOpened() && (path2_.empty() || capture2_.isOpened()))
+	if(capture_.isOpened() && ((path2_.empty() && usbDevice2_ < 0) || capture2_.isOpened()))
 	{
 		cv::Mat leftImage;
 		cv::Mat rightImage;
-		if(path2_.empty())
+		if(path2_.empty() && usbDevice2_ < 0)
 		{
 			if(!capture_.read(img))
 			{
@@ -73,14 +73,8 @@ CameraThread::~CameraThread()
 {
 	UDEBUG("");
 	join(true);
-	if(_camera)
-	{
-		delete _camera;
-	}
-	if(_distortionModel)
-	{
-		delete _distortionModel;
-	}
+	delete _camera;
+	delete _distortionModel;
 	delete _stereoDense;
 }

@@ -29,12 +29,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/VisualWord.h"
+#include "rtabmap/core/DBDriverSqlite3.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
-#include "DBDriverSqlite3.h"

 namespace rtabmap {

@@ -25,7 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#include "DBDriverSqlite3.h"
+#include "rtabmap/core/DBDriverSqlite3.h"
+#include <sqlite3.h>

 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/VisualWord.h"
@@ -42,8 +43,8 @@ namespace rtabmap {
 DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
 	DBDriver(parameters),
 	_ppDb(0),
-	_memoryUsedEstimate(0),
 	_version("0.0.0"),
+	_memoryUsedEstimate(0),
 	_dbInMemory(Parameters::defaultDbSqlite3InMemory()),
 	_cacheSize(Parameters::defaultDbSqlite3CacheSize()),
 	_journalMode(Parameters::defaultDbSqlite3JournalMode()),
@@ -3957,7 +3958,7 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const

 					//execute query
 					rc=sqlite3_step(ppStmt);
-					UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (word=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), w->id()).c_str());

 					rc = sqlite3_reset(ppStmt);
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/DBDriver.h"
-#include "DBDriverSqlite3.h"

 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
@@ -131,7 +130,7 @@ bool DBReader::init(

 	rtabmap::ParametersMap parameters;
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
-	_dbDriver = new DBDriverSqlite3(parameters);
+	_dbDriver = DBDriver::create(parameters);
 	if(!_dbDriver)
 	{
 		UERROR("Driver doesn't exist.");
@@ -923,11 +923,11 @@ void ORB::parseParameters(const ParametersMap & parameters)
 		gpu_ = false;
 	}
 #endif
-	if(gpu_)
+	if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
 	{
-		UWARN("GPU version of ORB available but not implemented yet! Using CPU version instead...");
+		UWARN("GPU version of ORB not available (no GPU found)! Using CPU version instead...");
+		gpu_ = false;
 	}
-	gpu_ = false;
 #endif
 	if(gpu_)
 	{
@@ -940,7 +940,7 @@ void ORB::parseParameters(const ParametersMap & parameters)
 #endif
 #else
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
-		UFATAL("not implemented");
+		_gpuOrb = CV_ORB_GPU::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_, fastThreshold_);
 #endif
 #endif
 	}
@@ -977,7 +977,14 @@ std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, cons
 #endif
 #else
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
-		UFATAL("not implemented");
+		cv::cuda::GpuMat d_image(imgRoi);
+		cv::cuda::GpuMat d_mask(maskRoi);
+		try {
+			_gpuOrb->detectAndCompute(d_image, d_mask, keypoints, cv::cuda::GpuMat(), false);
+		} catch (cv::Exception& e) {
+			const char* err_msg = e.what();
+			UWARN("OpenCV exception caught: %s", err_msg);
+		}
 #endif
 #endif
 	}
@@ -1018,10 +1025,23 @@ cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyP
 		UERROR("GPU version of ORB not available (OpenCV not built with gpu/cuda module)! Using CPU version instead...");
 #endif
 #else
-		cv::cuda::GpuMat imgGpu(image);
-		cv::cuda::GpuMat descriptorsGPU;
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
-		UFATAL("not implemented");
+		cv::cuda::GpuMat d_image(image);
+		cv::cuda::GpuMat d_descriptors;
+		try {
+			_gpuOrb->detectAndCompute(d_image, cv::cuda::GpuMat(), keypoints, d_descriptors, true);
+		} catch (cv::Exception& e) {
+			const char* err_msg = e.what();
+			UWARN("OpenCV exception caught: %s", err_msg);
+		}
+		// Download descriptors
+		if (d_descriptors.empty())
+			descriptors = cv::Mat();
+		else
+		{
+			UASSERT(d_descriptors.type() == CV_32F || d_descriptors.type() == CV_8U);
+			d_descriptors.download(descriptors);
+		}
 #endif
 #endif
 	}
@@ -140,7 +140,7 @@ void IMUThread::mainLoop()
 		}
 		previousStamp_ = stamp;

-		IMU imu(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), localTransform_);
+		IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_);
 		this->post(new IMUEvent(imu, stamp));
 	}
 	else if(!this->isKilled())
@@ -57,9 +57,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/Stereo.h"
+#include "rtabmap/core/OptimizerG2O.h"
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
 #include <rtabmap/core/OccupancyGrid.h>
+#include <opencv2/imgproc/types_c.h>

 namespace rtabmap {

@@ -93,6 +95,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
 	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
+	_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
@@ -101,6 +104,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_visCorType(Parameters::defaultVisCorType()),
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
+	_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
+	_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -334,7 +339,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 		}
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));

-		if(_vwd->getUnusedWordsSize())
+		if(_vwd->getUnusedWordsSize() && _vwd->isIncremental())
 		{
 			UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
 		}
@@ -427,26 +432,11 @@ Memory::~Memory()
 	{
 		UWARN("Please call Memory::close() before");
 	}
-	if(_feature2D)
-	{
-		delete _feature2D;
-	}
-	if(_vwd)
-	{
-		delete _vwd;
-	}
-	if(_registrationPipeline)
-	{
-		delete _registrationPipeline;
-	}
-	if(_registrationIcpMulti)
-	{
-		delete _registrationIcpMulti;
-	}
-	if(_occupancy)
-	{
-		delete _occupancy;
-	}
+	delete _feature2D;
+	delete _vwd;
+	delete _registrationPipeline;
+	delete _registrationIcpMulti;
+	delete _occupancy;
 }

 void Memory::parseParameters(const ParametersMap & parameters)
@@ -480,6 +470,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
 	Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
 	Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
+	Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
 	Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
 	Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
@@ -496,6 +487,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
 	}
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
+	Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
+	Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);


 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
@@ -808,7 +801,30 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
 					UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
-					cv::Mat infMatrix = covariance.inv();
+					double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
+					if(maxAngVar != 1.0 && maxAngVar > 0.1)
+					{
+						UWARN("Very large angular variance (%f) detected! Please fix odometry "
+								"twist covariance, otherwise poor graph optimizations are "
+								"expected and wrong loop closure detections creating a lot "
+								"of errors in the map could be accepted.", maxAngVar);
+					}
+
+					cv::Mat infMatrix;
+					if(_covOffDiagonalIgnored)
+					{
+						infMatrix = cv::Mat::zeros(6,6,CV_64FC1);
+						infMatrix.at<double>(0,0) = 1.0 / covariance.at<double>(0,0);
+						infMatrix.at<double>(1,1) = 1.0 / covariance.at<double>(1,1);
+						infMatrix.at<double>(2,2) = 1.0 / covariance.at<double>(2,2);
+						infMatrix.at<double>(3,3) = 1.0 / covariance.at<double>(3,3);
+						infMatrix.at<double>(4,4) = 1.0 / covariance.at<double>(4,4);
+						infMatrix.at<double>(5,5) = 1.0 / covariance.at<double>(5,5);
+					}
+					else
+					{
+						infMatrix = covariance.inv();
+					}
 					if((uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0.0) &&
 						!(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0.0))
 					{
@@ -833,16 +849,19 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 						*_stMem.rbegin(), signature->id(),
 						_signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());

-				//Tag the first node of the map
-				std::string tag = uFormat("map%d", signature->mapId());
-				if(getSignatureIdByLabel(tag, false) == 0)
+				if(_mapLabelsAdded && isIncremental())
 				{
-					UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
-					signature->setLabel(tag);
+					//Tag the first node of the map
+					std::string tag = uFormat("map%d", signature->mapId());
+					if(getSignatureIdByLabel(tag, false) == 0)
+					{
+						UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
+						signature->setLabel(tag);
+					}
 				}
 			}
 		}
-		else if(_mapLabelsAdded)
+		else if(_mapLabelsAdded && isIncremental())
 		{
 			//Tag the first node of the map
 			std::string tag = uFormat("map%d", signature->mapId());
@@ -1373,6 +1392,16 @@ std::string Memory::getDatabaseVersion() const
 	return version;
 }

+std::string Memory::getDatabaseUrl() const
+{
+	std::string url = "";
+	if(_dbDriver)
+	{
+		url = _dbDriver->getUrl();
+	}
+	return url;
+}
+
 double Memory::getDbSavingTime() const
 {
 	return _dbDriver?_dbDriver->getEmptyTrashesTime():0;
@@ -1488,6 +1517,8 @@ void Memory::clear()
 	_memoryChanged = false;
 	_linksChanged = false;
 	_gpsOrigin = GPS();
+	_rectCameraModels.clear();
+	_rectStereoCameraModel = StereoCameraModel();

 	if(_dbDriver)
 	{
@@ -2112,7 +2143,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 			_lastGlobalLoopClosureId = 0;
 		}

-		if(	(_notLinkedNodesKeptInDb || keepLinkedToGraph) &&
+		if(	(_notLinkedNodesKeptInDb || keepLinkedToGraph || s->isSaved()) &&
 			_dbDriver &&
 			s->id()>0 &&
 			(_incrementalMemory || s->isSaved()))
@@ -2385,6 +2416,7 @@ Transform Memory::computeTransform(
 		RegistrationInfo * info,
 		bool useKnownCorrespondencesIfPossible) const
 {
+	UDEBUG("");
 	Transform transform;

 	// make sure we have all data needed
@@ -2459,6 +2491,173 @@ Transform Memory::computeTransform(
 				transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 			}
 		}
+		else if(_localBundleOnLoopClosure &&
+				_registrationPipeline->isImageRequired() &&
+			   !_registrationPipeline->isScanRequired() &&
+			   !_registrationPipeline->isUserDataRequired() &&
+			   !tmpTo.getWordsDescriptors().empty() &&
+			   !tmpTo.getWords().empty() &&
+			   !tmpFrom.getWordsDescriptors().empty() &&
+			   !tmpFrom.getWords().empty() &&
+			   !tmpFrom.getWords3().empty())
+		{
+			std::multimap<int, cv::Point3f> words3DMap;
+			std::multimap<int, cv::KeyPoint> wordsMap;
+			std::multimap<int, cv::Mat> wordsDescriptorsMap;
+
+			const std::map<int, Link> & links = fromS.getLinks();
+			{
+				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
+				UDEBUG("fromS.getWords3()=%d  uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
+				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
+				{
+					if(util3d::isFinite(jter->second))
+					{
+						words3DMap.insert(*jter);
+						wordsMap.insert(*fromS.getWords().find(jter->first));
+						wordsDescriptorsMap.insert(*fromS.getWordsDescriptors().find(jter->first));
+					}
+				}
+			}
+			UDEBUG("words3DMap=%d", (int)words3DMap.size());
+
+			for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				int id = iter->first;
+				const Signature * s = this->getSignature(id);
+				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
+				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
+				{
+					if( jter->first > 0 &&
+						util3d::isFinite(jter->second) &&
+					    words3DMap.find(jter->first) == words3DMap.end())
+					{
+						words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
+						wordsMap.insert(*s->getWords().find(jter->first));
+						wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
+					}
+				}
+			}
+			UDEBUG("words3DMap=%d", (int)words3DMap.size());
+			Signature tmpFrom2(fromS.id());
+			tmpFrom2.setWords3(words3DMap);
+			tmpFrom2.setWords(wordsMap);
+			tmpFrom2.setWordsDescriptors(wordsDescriptorsMap);
+
+			transform = _registrationPipeline->computeTransformationMod(tmpFrom2, tmpTo, guess, info);
+
+			if(!transform.isNull() && info)
+			{
+				std::map<int, cv::Point3f> points3DMap = uMultimapToMapUnique(tmpFrom2.getWords3());
+				std::map<int, Transform> bundlePoses;
+				std::multimap<int, Link> bundleLinks;
+				std::map<int, CameraModel> bundleModels;
+				std::map<int, std::map<int, cv::Point3f> > wordReferences;
+
+				std::map<int, Link> links = fromS.getLinks();
+				links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
+				links.insert(std::make_pair(fromS.id(), Link()));
+
+				for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+				{
+					int id = iter->first;
+					const Signature * s;
+					if(id == tmpTo.id())
+					{
+						s = &tmpTo; // reuse matched words
+					}
+					else
+					{
+						s = this->getSignature(id);
+					}
+					CameraModel model;
+					if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
+					{
+						model = s->sensorData().cameraModels()[0];
+					}
+					else if(s->sensorData().stereoCameraModel().isValidForProjection())
+					{
+						model = s->sensorData().stereoCameraModel().left();
+						// Set Tx for stereo BA
+						model = CameraModel(model.fx(),
+								model.fy(),
+								model.cx(),
+								model.cy(),
+								model.localTransform(),
+								-s->sensorData().stereoCameraModel().baseline()*model.fx());
+					}
+					else
+					{
+						UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
+					}
+					bundleModels.insert(std::make_pair(id, model));
+					Transform invLocalTransform = model.localTransform().inverse();
+					if(iter->second.isValid())
+					{
+						bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
+						bundlePoses.insert(std::make_pair(id, iter->second.transform()));
+					}
+					else
+					{
+						bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
+					}
+					const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
+					for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
+					{
+						if(points3DMap.find(jter->first)!=points3DMap.end() &&
+							(id == tmpTo.id() || jter->first > 0))
+						{
+							std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
+							cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
+							wordReferences.insert(std::make_pair(jter->first, std::map<int, cv::Point3f>()));
+							wordReferences.at(jter->first).insert(std::make_pair(id, cv::Point3f(jter->second.pt.x, jter->second.pt.y, pt3d.z)));
+						}
+					}
+				}
+
+				UDEBUG("sba...start");
+				// set root negative to fix all other poses
+				std::set<int> sbaOutliers;
+				UTimer bundleTimer;
+				OptimizerG2O sba;
+				UTimer bundleTime;
+				bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
+				UDEBUG("sba...end");
+
+				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)wordReferences.size(), (int)sbaOutliers.size());
+
+				UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
+				if(!bundlePoses.rbegin()->second.isNull())
+				{
+					if(sbaOutliers.size())
+					{
+						std::vector<int> newInliers(info->inliersIDs.size());
+						int oi=0;
+						for(unsigned int i=0; i<info->inliersIDs.size(); ++i)
+						{
+							if(sbaOutliers.find(info->inliersIDs[i]) == sbaOutliers.end())
+							{
+								newInliers[oi++] = info->inliersIDs[i];
+							}
+						}
+						newInliers.resize(oi);
+						UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(info->inliersIDs.size()));
+						info->inliers = (int)newInliers.size();
+						info->inliersIDs = newInliers;
+					}
+					if(info->inliers < _registrationPipeline->getMinVisualCorrespondences())
+					{
+						info->rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", info->inliers, _registrationPipeline->getMinVisualCorrespondences());
+						transform.setNull();
+					}
+					else
+					{
+						transform = bundlePoses.rbegin()->second;
+					}
+				}
+				UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
+			}
+		}
 		else
 		{
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
@@ -3499,25 +3698,41 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}

-	if(!_imagesAlreadyRectified && !data.imageRaw().empty())
+	bool imagesRectified = _imagesAlreadyRectified;
+	// Stereo must be always rectified because of the stereo correspondence approach
+	if(!imagesRectified && !data.imageRaw().empty() && !(_rectifyOnlyFeatures && data.rightRaw().empty()))
 	{
-		if(!data.depthRaw().empty())
-		{
-			UERROR("RGB-D images should be already rectified! Make sure they are and set %s parameter back to true.",
-					Parameters::kRtabmapImagesAlreadyRectified().c_str());
-			return 0;
-		}
+		// we assume that once rtabmap is receiving data, the calibration won't change over time
 		if(data.cameraModels().size())
 		{
+			// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
 			UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
 			int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
 			cv::Mat rectifiedImages(data.imageRaw().size(), data.imageRaw().type());
+			bool initRectMaps = _rectCameraModels.empty();
+			if(initRectMaps)
+			{
+				_rectCameraModels.resize(data.cameraModels().size());
+			}
 			for(unsigned int i=0; i<data.cameraModels().size(); ++i)
 			{
 				if(data.cameraModels()[i].isValidForRectification())
 				{
-					cv::Mat rectifiedImage = data.cameraModels()[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					if(initRectMaps)
+					{
+						_rectCameraModels[i] = data.cameraModels()[i];
+						if(!_rectCameraModels[i].isRectificationMapInitialized())
+						{
+							UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
+							_rectCameraModels[i].initRectificationMap();
+							UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
+						}
+					}
+					UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
+							_rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
+					cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
 					rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					imagesRectified = true;
 				}
 				else
 				{
@@ -3532,8 +3747,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 		else if(data.stereoCameraModel().isValidForRectification())
 		{
-			data.setImageRaw(data.stereoCameraModel().left().rectifyImage(data.imageRaw()));
-			data.setDepthOrRightRaw(data.stereoCameraModel().right().rectifyImage(data.rightRaw()));
+			if(!_rectStereoCameraModel.isValidForRectification())
+			{
+				_rectStereoCameraModel = data.stereoCameraModel();
+				if(!_rectStereoCameraModel.isRectificationMapInitialized())
+				{
+					UWARN("Initializing rectification maps (only done for the first image received)...");
+					_rectStereoCameraModel.initRectificationMap();
+					UWARN("Initializing rectification maps (only done for the first image received)...done!");
+				}
+			}
+			UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
+			UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
+			data.setImageRaw(_rectStereoCameraModel.left().rectifyImage(data.imageRaw()));
+			data.setDepthOrRightRaw(_rectStereoCameraModel.right().rectifyImage(data.rightRaw()));
+			imagesRectified = true;
 		}
 		else
 		{
@@ -3548,10 +3776,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}

 	int treeSize= int(_workingMem.size() + _stMem.size());
-	int meanWordsPerLocation = 0;
-	if(treeSize > 0)
+	int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
+	if(treeSize > 1)
 	{
-		meanWordsPerLocation = _vwd->getTotalActiveReferences() / treeSize;
+		meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
 	}

 	if(_parallelized && !isIntermediateNode)
@@ -3562,11 +3790,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

 	int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
+	SensorData decimatedData;
 	if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{
-			SensorData decimatedData = data;
+			decimatedData = data;
 			if(_imagePreDecimation > 1)
 			{
 				preDecimation = _imagePreDecimation;
@@ -3602,7 +3831,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}

 			cv::Mat depthMask;
-			if(!decimatedData.depthRaw().empty() && _depthAsMask)
+			if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
 			{
 				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3646,13 +3875,134 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				descriptors = cv::Mat();
 			}
-			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+			else
 			{
-				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
-				t = timer.ticks();
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+				if(!imagesRectified && decimatedData.cameraModels().size())
+				{
+					std::vector<cv::KeyPoint> keypointsValid;
+					keypointsValid.reserve(keypoints.size());
+					cv::Mat descriptorsValid;
+					descriptorsValid.reserve(descriptors.rows);
+
+					//undistort keypoints before projection (RGB-D)
+					if(decimatedData.cameraModels().size() == 1)
+					{
+						std::vector<cv::Point2f> pointsIn, pointsOut;
+						cv::KeyPoint::convert(keypoints,pointsIn);
+						if(decimatedData.cameraModels()[0].D_raw().cols == 6)
+						{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+							// Equidistant / FishEye
+							// get only k parameters (k1,k2,p1,p2,k3,k4)
+							cv::Mat D(1, 4, CV_64FC1);
+							D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+							D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+							D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
+							D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
+							cv::fisheye::undistortPoints(pointsIn, pointsOut,
+									decimatedData.cameraModels()[0].K_raw(),
+									D,
+									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[0].P());
+						}
+						else
+#else
+							UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+									CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+						}
+#endif
+						{
+							//RadialTangential
+							cv::undistortPoints(pointsIn, pointsOut,
+									decimatedData.cameraModels()[0].K_raw(),
+									decimatedData.cameraModels()[0].D_raw(),
+									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[0].P());
+						}
+						UASSERT(pointsOut.size() == keypoints.size());
+						for(unsigned int i=0; i<pointsOut.size(); ++i)
+						{
+							if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
+							   pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
+							{
+								keypointsValid.push_back(keypoints.at(i));
+								keypointsValid.back().pt.x = pointsOut.at(i).x;
+								keypointsValid.back().pt.y = pointsOut.at(i).y;
+								descriptorsValid.push_back(descriptors.row(i));
+							}
+						}
+					}
+					else
+					{
+						UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
+						float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
+						for(unsigned int i=0; i<keypoints.size(); ++i)
+						{
+							int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+							UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
+									uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+											cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
+
+							std::vector<cv::Point2f> pointsIn, pointsOut;
+							pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+							if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
+							{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+								// Equidistant / FishEye
+								// get only k parameters (k1,k2,p1,p2,k3,k4)
+								cv::Mat D(1, 4, CV_64FC1);
+								D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+								D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+								D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+								D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+								cv::fisheye::undistortPoints(pointsIn, pointsOut,
+										decimatedData.cameraModels()[cameraIndex].K_raw(),
+										D,
+										decimatedData.cameraModels()[cameraIndex].R(),
+										decimatedData.cameraModels()[cameraIndex].P());
+							}
+							else
+#else
+								UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+										CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+							}
+#endif
+							{
+								//RadialTangential
+								cv::undistortPoints(pointsIn, pointsOut,
+										decimatedData.cameraModels()[cameraIndex].K_raw(),
+										decimatedData.cameraModels()[cameraIndex].D_raw(),
+										decimatedData.cameraModels()[cameraIndex].R(),
+										decimatedData.cameraModels()[cameraIndex].P());
+							}
+
+							if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
+							   pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
+							{
+								keypointsValid.push_back(keypoints.at(i));
+								keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+								keypointsValid.back().pt.y = pointsOut[0].y;
+								descriptorsValid.push_back(descriptors.row(i));
+							}
+						}
+					}
+
+					keypoints = keypointsValid;
+					descriptors = descriptorsValid;
+
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+					UDEBUG("time rectification = %fs", t);
+				}
+
+				if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
+				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+				{
+					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+				}
 			}
 		}
 		else if(data.imageRaw().empty())
@@ -3704,6 +4054,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				imageMono = data.imageRaw();
 			}

+			UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
+
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 		}
 		t = timer.ticks();
@@ -3824,6 +4176,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				allWordIds[quantizedToRawIndices[i]] = *iter;
 				++i;
 			}
+			int negIndex = -1;
+			for(i=0; i<(int)allWordIds.size(); ++i)
+			{
+				if(allWordIds[i] < 0)
+				{
+					allWordIds[i] = negIndex--;
+				}
+			}
 			wordIds = uVectorToList(allWordIds);
 		}

@@ -3844,7 +4204,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UASSERT(wordIds.size() == keypoints.size());
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
-		float decimationRatio = preDecimation / _imagePostDecimation;
+		float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
 		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
@@ -3870,47 +4230,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}

-	if(!pose.isNull() &&
-		data.cameraModels().size() == 1 &&
-		words.size() &&
-		words3D.size() == 0 &&
-		_signatures.size() &&
-		_signatures.rbegin()->second->mapId() == _idMapCount) // same map
-	{
-		UDEBUG("Generate 3D words using odometry");
-		Signature * previousS = _signatures.rbegin()->second;
-		if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
-		{
-			Transform cameraTransform = pose.inverse() * previousS->getPose();
-			// compute 3D words by epipolar geometry with the previous signature
-			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
-					uMultimapToMapUnique(words),
-					uMultimapToMapUnique(previousS->getWords()),
-					data.cameraModels()[0],
-					cameraTransform);
-
-			// words3D should have the same size than words
-			float bad_point = std::numeric_limits<float>::quiet_NaN ();
-			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
-			{
-				std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
-				if(jter != inliers.end())
-				{
-					words3D.insert(std::make_pair(iter->first, jter->second));
-				}
-				else
-				{
-					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
-				}
-			}
-
-			t = timer.ticks();
-			UASSERT(words3D.size() == words.size());
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
-		}
-	}
-
 	cv::Mat image = data.imageRaw();
 	cv::Mat depthOrRightImage = data.depthOrRightRaw();
 	std::vector<CameraModel> cameraModels = data.cameraModels();
@@ -3919,19 +4238,29 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// apply decimation?
 	if(_imagePostDecimation > 1 && !isIntermediateNode)
 	{
-		if(!data.rightRaw().empty() ||
-			(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
+		if(_imagePostDecimation == preDecimation && decimatedData.isValid())
 		{
-			depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
+			image = decimatedData.imageRaw();
+			depthOrRightImage = decimatedData.depthOrRightRaw();
+			cameraModels = decimatedData.cameraModels();
+			stereoCameraModel = decimatedData.stereoCameraModel();
 		}
-		image = util2d::decimate(image, _imagePostDecimation);
-		for(unsigned int i=0; i<cameraModels.size(); ++i)
+		else
 		{
-			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
-		}
-		if(stereoCameraModel.isValidForProjection())
-		{
-			stereoCameraModel.scale(1.0/double(_imagePostDecimation));
+			if(!data.rightRaw().empty() ||
+				(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
+			{
+				depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
+			}
+			image = util2d::decimate(image, _imagePostDecimation);
+			for(unsigned int i=0; i<cameraModels.size(); ++i)
+			{
+				cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
+			}
+			if(stereoCameraModel.isValidForProjection())
+			{
+				stereoCameraModel.scale(1.0/double(_imagePostDecimation));
+			}
 		}

 		t = timer.ticks();
@@ -3939,6 +4268,91 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("time post-decimation = %fs", t);
 	}

+	bool triangulateWordsWithoutDepth = !_depthAsMask;
+	if(!pose.isNull() &&
+		cameraModels.size() == 1 &&
+		words.size() &&
+		(words3D.size() == 0 || (triangulateWordsWithoutDepth && words.size() == words3D.size())) &&
+		_registrationPipeline->isImageRequired() &&
+		_signatures.size() &&
+		_signatures.rbegin()->second->mapId() == _idMapCount) // same map
+	{
+		UDEBUG("Generate 3D words using odometry");
+		Signature * previousS = _signatures.rbegin()->second;
+		if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
+		{
+			UDEBUG("Previous pose(%d) = %s", previousS->id(), previousS->getPose().prettyPrint().c_str());
+			UDEBUG("Current pose(%d) = %s", id, pose.prettyPrint().c_str());
+			Transform cameraTransform = pose.inverse() * previousS->getPose();
+
+			Signature cpPrevious(2);
+			// IDs should be unique so that registration doesn't override them
+			std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
+			std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
+			cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
+			cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
+			cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
+			Signature cpCurrent(1);
+			uniqueWords = uMultimapToMapUnique(words);
+			uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
+			cpCurrent.sensorData().setCameraModels(cameraModels);
+			cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
+			cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
+			// This will force comparing descriptors between both images directly
+			Transform tmpt = _registrationPipeline->computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
+			UDEBUG("t=%s", tmpt.prettyPrint().c_str());
+
+			// compute 3D words by epipolar geometry with the previous signature
+			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
+					uMultimapToMapUnique(cpCurrent.getWords()),
+					uMultimapToMapUnique(cpPrevious.getWords()),
+					cameraModels[0],
+					cameraTransform);
+
+			UDEBUG("inliers=%d", (int)inliers.size());
+
+			// words3D should have the same size than words if not empty
+			float bad_point = std::numeric_limits<float>::quiet_NaN ();
+			UASSERT(words3D.size() == 0 || words.size() == words3D.size());
+			bool words3DWasEmpty = words3D.empty();
+			int added3DPointsWithoutDepth = 0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+			{
+				std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
+				std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
+				if(iter3D == words3D.end())
+				{
+					if(jter != inliers.end())
+					{
+						words3D.insert(std::make_pair(iter->first, jter->second));
+						++added3DPointsWithoutDepth;
+					}
+					else
+					{
+						words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+					}
+				}
+				else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
+				{
+					iter3D->second = jter->second;
+					++added3DPointsWithoutDepth;
+				}
+				else if(words3DWasEmpty && jter == inliers.end())
+				{
+					// duplicate
+					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+				}
+			}
+			UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
+			if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
+
+			t = timer.ticks();
+			UASSERT(words3D.size() == words.size());
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
+			UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
+		}
+	}
+
 	// Filter the laser scan?
 	LaserScan laserScan = data.laserScanRaw();
 	if(!isIntermediateNode && laserScan.size())
@@ -74,6 +74,11 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	erode_(Parameters::defaultGridGlobalEroded()),
 	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
 	updateError_(Parameters::defaultGridGlobalUpdateError()),
+	occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()),
+	probHit_(logodds(Parameters::defaultGridGlobalProbHit())),
+	probMiss_(logodds(Parameters::defaultGridGlobalProbMiss())),
+	probClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())),
+	probClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())),
 	xMin_(0.0f),
 	yMin_(0.0f),
 	cloudAssembling_(false),
@@ -131,6 +136,27 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
 	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);

+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_);
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit_))
+	{
+		probHit_ = logodds(probHit_);
+		UASSERT_MSG(probHit_ >= 0.0f, uFormat("probHit_=%f",probHit_).c_str());
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss_))
+	{
+		probMiss_ = logodds(probMiss_);
+		UASSERT_MSG(probMiss_ <= 0.0f, uFormat("probMiss_=%f",probMiss_).c_str());
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), probClampingMin_))
+	{
+		probClampingMin_ = logodds(probClampingMin_);
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), probClampingMax_))
+	{
+		probClampingMax_ = logodds(probClampingMax_);
+	}
+	UASSERT(probClampingMax_ > probClampingMin_);
+
 	UASSERT(minMapSize_ >= 0.0f);

 	// convert ROI from string to vector
@@ -205,7 +231,23 @@ void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float ce
 		UASSERT(cellSize > 0.0f);
 		UASSERT(map.type() == CV_8SC1);
 		map_ = map.clone();
-		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC3);
+		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
+		for(int i=0; i<map_.rows; ++i)
+		{
+			for(int j=0; j<map_.cols; ++j)
+			{
+				const char value = map_.at<char>(i,j);
+				float * info = mapInfo_.ptr<float>(i,j);
+				if(value == 0)
+				{
+					info[3] = probClampingMin_;
+				}
+				else if(value == 100)
+				{
+					info[3] = probClampingMax_;
+				}
+			}
+		}
 		xMin_ = xMin;
 		yMin_ = yMin;
 		cellSize_ = cellSize;
@@ -563,11 +605,71 @@ cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 {
 	xMin = xMin_;
 	yMin = yMin_;
-	if(erode_ && !map_.empty())
+
+	cv::Mat map = map_;
+
+	UTimer t;
+	if(occupancyThr_ != 0.0f && !map.empty())
 	{
-		return util3d::erodeMap(map_);
+		float occThr = logodds(occupancyThr_);
+		map = cv::Mat(map.size(), map.type());
+		UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else if(info[3] >= occThr)
+				{
+					map.at<char>(i, j) = 100; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = 0; // empty
+				}
+			}
+		}
+		UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
 	}
-	return map_;
+
+	if(erode_ && !map.empty())
+	{
+		map = util3d::erodeMap(map);
+		UDEBUG("Eroding map = %fs", t.ticks());
+	}
+	return map;
+}
+
+cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
+{
+	xMin = xMin_;
+	yMin = yMin_;
+
+	cv::Mat map;
+	if(!mapInfo_.empty())
+	{
+		map = cv::Mat(mapInfo_.size(), map_.type());
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
+				}
+			}
+		}
+	}
+	return map;
 }

 void OccupancyGrid::addToCache(
@@ -786,7 +888,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	{
 		if(addedNodes_.find(iter->first) == addedNodes_.end())
 		{
-			UDEBUG("Pose %d not found in current added poses, it be added to map", iter->first);
+			UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
 			poses.push_back(*iter);
 		}
 	}
@@ -996,7 +1098,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 			{
 				UDEBUG("Map empty!");
 				map = cv::Mat::ones(newMapSize, CV_8S)*-1;
-				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC3);
+				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
 			}
 			else
 			{
@@ -1110,6 +1212,20 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 								cter->second.first+=1;
 							}
 							value = 0; // free space
+
+							// update odds
+							if(nodeId != kter->first)
+							{
+								info[3] += probMiss_;
+								if (info[3] < probClampingMin_)
+								{
+									info[3] = probClampingMin_;
+								}
+								if (info[3] > probClampingMax_)
+								{
+									info[3] = probClampingMax_;
+								}
+							}
 						}
 					}
 				}
@@ -1220,6 +1336,20 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 								cter->second.second+=1;
 							}
 							value = 100; // obstacles
+
+							// update odds
+							if(nodeId != kter->first)
+							{
+								info[3] += probHit_;
+								if (info[3] < probClampingMin_)
+								{
+									info[3] = probClampingMin_;
+								}
+								if (info[3] > probClampingMax_)
+								{
+									info[3] = probClampingMax_;
+								}
+							}
 						}
 					}
 				}
@@ -276,11 +276,30 @@ OctoMap::OctoMap(const ParametersMap & parameters) :
 	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
 	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;

-	float occupancyThr = Parameters::defaultGridGlobalOctoMapOccupancyThr();
-	Parameters::parse(parameters, Parameters::kGridGlobalOctoMapOccupancyThr(), occupancyThr);
+	float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	float probHit = Parameters::defaultGridGlobalProbHit();
+	float probMiss = Parameters::defaultGridGlobalProbMiss();
+	float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
+	float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);

 	octree_ = new RtabmapColorOcTree(cellSize);
+	if(occupancyThr <= 0.0f)
+	{
+		UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
+				Parameters::kGridGlobalOccupancyThr().c_str(),
+				Parameters::defaultGridGlobalOccupancyThr());
+		occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	}
 	octree_->setOccupancyThres(occupancyThr);
+	octree_->setProbHit(probHit);
+	octree_->setProbMiss(probMiss);
+	octree_->setClampingThresMin(clampingMin);
+	octree_->setClampingThresMax(clampingMax);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryDVO.h"
 #include "rtabmap/core/OdometryOkvis.h"
 #include "rtabmap/core/OdometryORBSLAM2.h"
+#include "rtabmap/core/OdometryLOAM.h"
+#include "rtabmap/core/OdometryMSCKF.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -80,6 +82,12 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeOkvis:
 		odometry = new OdometryOkvis(parameters);
 		break;
+	case Odometry::kTypeLOAM:
+		odometry = new OdometryLOAM(parameters);
+		break;
+	case Odometry::kTypeMSCKF:
+		odometry = new OdometryMSCKF(parameters);
+		break;
 	default:
 		odometry = new OdometryF2M(parameters);
 		type = Odometry::kTypeF2M;
@@ -289,7 +297,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 							R(0,0), R(0,1), R(0,2), 0,
 							R(1,0), R(1,1), R(1,2), 0,
 							R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
-					_pose *= rotation;
+					this->reset(rotation);
 					success = true;
 				}
 			}
@@ -308,7 +316,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
 	if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
 	{
-		if(guessFromMotion_)
+		if(guessFromMotion_ && !data.imageRaw().empty())
 		{
 			UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
 		}
@@ -55,18 +55,9 @@ OdometryDVO::OdometryDVO(const ParametersMap & parameters) :
 OdometryDVO::~OdometryDVO()
 {
 #ifdef RTABMAP_DVO
-	if(dvo_)
-	{
-		delete dvo_;
-	}
-	if(reference_)
-	{
-		delete reference_;
-	}
-	if(camera_)
-	{
-		delete camera_;
-	}
+	delete dvo_;
+	delete reference_;
+	delete camera_;
 #endif
 }

@@ -147,10 +147,7 @@ OdometryF2M::~OdometryF2M()
 	bundleLinks_.clear();
 	bundleModels_.clear();
 	bundlePoseReferences_.clear();
-	if(sba_)
-	{
-		delete sba_;
-	}
+	delete sba_;
 	delete regPipeline_;
 	UDEBUG("");
 }
@@ -335,7 +332,7 @@ Transform OdometryF2M::computeTransform(
 							}
 							else
 							{
-								UFATAL("no valid camera model!");
+								UFATAL("no valid camera model to do odometry bundle adjustment!");
 							}
 							bundleModels.insert(std::make_pair(lastFrame_->id(), model));
 							Transform invLocalTransform = model.localTransform().inverse();
@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"

 #ifdef RTABMAP_FOVIS
-#include <fovis.hpp>
+#include <libfovis/fovis.hpp>
 #endif

 namespace rtabmap {
@@ -60,26 +60,11 @@ OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
 OdometryFovis::~OdometryFovis()
 {
 #ifdef RTABMAP_FOVIS
-	if(fovis_)
-	{
-		delete fovis_;
-	}
-	if(rect_)
-	{
-		delete rect_;
-	}
-	if(stereoCalib_)
-	{
-		delete stereoCalib_;
-	}
-	if(depthImage_)
-	{
-		delete depthImage_;
-	}
-	if(stereoDepth_)
-	{
-		delete stereoDepth_;
-	}
+	delete fovis_;
+	delete rect_;
+	delete stereoCalib_;
+	delete depthImage_;
+	delete stereoDepth_;
 #endif
 }

@@ -0,0 +1,304 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/OdometryLOAM.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/core/util3d.h"
+#include <pcl/common/transforms.h>
+
+float SCAN_PERIOD = 0.1f;
+
+namespace rtabmap {
+
+/**
+ * https://github.com/laboshinl/loam_velodyne/pull/66
+ */
+
+OdometryLOAM::OdometryLOAM(const ParametersMap & parameters) :
+	Odometry(parameters)
+#ifdef RTABMAP_LOAM
+	,lastPose_(Transform::getIdentity())
+	,scanPeriod_(Parameters::defaultOdomLOAMScanPeriod())
+	,linVar_(Parameters::defaultOdomLOAMLinVar())
+	,angVar_(Parameters::defaultOdomLOAMAngVar())
+	,localMapping_(Parameters::defaultOdomLOAMLocalMapping())
+	,lost_(false)
+#endif
+{
+#ifdef RTABMAP_LOAM
+	int velodyneType = 0;
+	Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), velodyneType);
+	Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scanPeriod_);
+	UASSERT(scanPeriod_>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
+	UASSERT(linVar_>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_);
+	UASSERT(angVar_>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMLocalMapping(), localMapping_);
+	if(velodyneType == 1)
+	{
+		scanMapper_ = loam::MultiScanMapper::Velodyne_HDL_32();
+	}
+	else if(velodyneType == 2)
+	{
+		scanMapper_ = loam::MultiScanMapper::Velodyne_HDL_64E();
+	}
+	else
+	{
+		scanMapper_ = loam::MultiScanMapper::Velodyne_VLP_16();
+	}
+	laserOdometry_ =  new loam::BasicLaserOdometry(scanPeriod_);
+	laserMapping_ = new loam::BasicLaserMapping(scanPeriod_);
+#endif
+}
+
+OdometryLOAM::~OdometryLOAM()
+{
+#ifdef RTABMAP_LOAM
+	delete laserOdometry_;
+	delete laserMapping_;
+#endif
+}
+
+void OdometryLOAM::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_LOAM
+	lastPose_.setIdentity();
+	scanRegistration_ = loam::BasicScanRegistration();
+	loam::RegistrationParams regParams;
+	regParams.scanPeriod = scanPeriod_;
+	scanRegistration_.configure(regParams);
+	delete laserOdometry_;
+	laserOdometry_ =  new loam::BasicLaserOdometry(scanPeriod_);
+	delete laserMapping_;
+	laserMapping_ = new loam::BasicLaserMapping(scanPeriod_);
+	transformMaintenance_ = loam::BasicTransformMaintenance();
+	lost_ = false;
+#endif
+}
+
+#ifdef RTABMAP_LOAM
+std::vector<pcl::PointCloud<pcl::PointXYZI> > OdometryLOAM::segmentScanRings(const pcl::PointCloud<pcl::PointXYZ> & laserCloudIn)
+{
+	std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans;
+
+	size_t cloudSize = laserCloudIn.size();
+
+	// determine scan start and end orientations
+	float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);
+	float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,
+			laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
+	if (endOri - startOri > 3 * M_PI) {
+		endOri -= 2 * M_PI;
+	} else if (endOri - startOri < M_PI) {
+		endOri += 2 * M_PI;
+	}
+
+	bool halfPassed = false;
+	pcl::PointXYZI point;
+	laserCloudScans.resize(scanMapper_.getNumberOfScanRings());
+	// clear all scanline points
+	std::for_each(laserCloudScans.begin(), laserCloudScans.end(), [](auto&&v) {v.clear(); });
+
+	// extract valid points from input cloud
+	for (size_t i = 0; i < cloudSize; i++) {
+		point.x = laserCloudIn[i].y;
+		point.y = laserCloudIn[i].z;
+		point.z = laserCloudIn[i].x;
+
+		// skip NaN and INF valued points
+		if (!pcl_isfinite(point.x) ||
+				!pcl_isfinite(point.y) ||
+				!pcl_isfinite(point.z)) {
+			continue;
+		}
+
+		// skip zero valued points
+		if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
+			continue;
+		}
+
+		// calculate vertical point angle and scan ID
+		float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
+		int scanID = scanMapper_.getRingForAngle(angle);
+		if (scanID >= scanMapper_.getNumberOfScanRings() || scanID < 0 ){
+			continue;
+		}
+
+		// calculate horizontal point angle
+		float ori = -std::atan2(point.x, point.z);
+		if (!halfPassed) {
+			if (ori < startOri - M_PI / 2) {
+				ori += 2 * M_PI;
+			} else if (ori > startOri + M_PI * 3 / 2) {
+				ori -= 2 * M_PI;
+			}
+
+			if (ori - startOri > M_PI) {
+				halfPassed = true;
+			}
+		} else {
+			ori += 2 * M_PI;
+
+			if (ori < endOri - M_PI * 3 / 2) {
+				ori += 2 * M_PI;
+			} else if (ori > endOri + M_PI / 2) {
+				ori -= 2 * M_PI;
+			}
+		}
+
+		// calculate relative scan time based on point orientation
+		float relTime = SCAN_PERIOD * (ori - startOri) / (endOri - startOri);
+		point.intensity = scanID + relTime;
+
+		// imu not used...
+		//scanRegistration_.projectPointToStartOfSweep(point, relTime);
+
+		laserCloudScans[scanID].push_back(point);
+	}
+
+	return laserCloudScans;
+}
+#endif
+
+// return not null transform if odometry is correctly computed
+Transform OdometryLOAM::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+#ifdef RTABMAP_LOAM
+	UTimer timer;
+
+	if(data.laserScanRaw().isEmpty())
+	{
+		UERROR("LOAM works only with laser scans and the current input is empty. Aborting odometry update...");
+		return t;
+	}
+	else if(data.laserScanRaw().is2d())
+	{
+		UERROR("LOAM version used works only with 3D laser scans from Velodyne. Aborting odometry update...");
+		return t;
+	}
+
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
+	if(!lost_)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudInPtr = util3d::laserScanToPointCloud(data.laserScanRaw());
+		std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans = segmentScanRings(*laserCloudInPtr);
+
+		ros::Time stampT;
+		stampT.fromSec(data.stamp());
+		loam::Time scanTime = loam::fromROSTime(stampT);
+		scanRegistration_.processScanlines(scanTime, laserCloudScans);
+
+		*laserOdometry_->cornerPointsSharp() = scanRegistration_.cornerPointsSharp();
+		*laserOdometry_->cornerPointsLessSharp() = scanRegistration_.cornerPointsLessSharp();
+		*laserOdometry_->surfPointsFlat() = scanRegistration_.surfacePointsFlat();
+		*laserOdometry_->surfPointsLessFlat() = scanRegistration_.surfacePointsLessFlat();
+		*laserOdometry_->laserCloud() = scanRegistration_.laserCloud();
+		pcl::PointCloud<pcl::PointXYZ> imuTrans;
+		imuTrans.resize(4);
+		laserOdometry_->updateIMU(imuTrans);
+		laserOdometry_->process();
+
+		if(localMapping_)
+		{
+			laserMapping_->laserCloudCornerLast() = *laserOdometry_->lastCornerCloud();
+			laserMapping_->laserCloudSurfLast() = *laserOdometry_->lastSurfaceCloud();
+			laserMapping_->laserCloud() = *laserOdometry_->laserCloud();
+			laserMapping_->updateOdometry(laserOdometry_->transformSum());
+			laserMapping_->process(scanTime);
+		}
+
+		transformMaintenance_.updateOdometry(
+				laserOdometry_->transformSum().rot_x.rad(),
+				laserOdometry_->transformSum().rot_y.rad(),
+				laserOdometry_->transformSum().rot_z.rad(),
+				laserOdometry_->transformSum().pos.x(),
+				laserOdometry_->transformSum().pos.y(),
+				laserOdometry_->transformSum().pos.z());
+		transformMaintenance_.updateMappingTransform(laserMapping_->transformAftMapped(), laserMapping_->transformBefMapped());
+		transformMaintenance_.transformAssociateToMap();
+		const float * tm = transformMaintenance_.transformMapped();
+		Transform pose = Transform(tm[5], tm[3], tm[4], tm[2], tm[0], tm[1]);
+
+		if(!pose.isNull())
+		{
+			covariance = cv::Mat::eye(6,6,CV_64FC1);
+			covariance(cv::Range(0,3), cv::Range(0,3)) *= linVar_;
+			covariance(cv::Range(3,6), cv::Range(3,6)) *= angVar_;
+
+			t = lastPose_.inverse() * pose; // incremental
+			lastPose_ = pose;
+
+			const Transform & localTransform = data.laserScanRaw().localTransform();
+			if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+			{
+				// from laser frame to base frame
+				t = localTransform * t * localTransform.inverse();
+			}
+
+			if(info)
+			{
+				info->type = (int)kTypeLOAM;
+				info->localScanMapSize = laserMapping_->laserCloudSurroundDS().size();
+				if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
+				{
+					info->reg.covariance = covariance;
+				}
+
+				if(this->isInfoDataFilled())
+				{
+					Transform rot(0,0,1,0,1,0,0,0,0,1,0,0);
+					pcl::PointCloud<pcl::PointXYZI> out;
+					pcl::transformPointCloud(laserMapping_->laserCloudSurroundDS(), out, rot.toEigen3f());
+					info->localScanMap = LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(out), 0, data.laserScanRaw().maxRange(), data.laserScanRaw().localTransform());
+				}
+			}
+		}
+		else
+		{
+			lost_ = true;
+			UWARN("LOAM failed to register the latest scan, odometry should be reset.");
+		}
+	}
+	UINFO("Odom update time = %fs, lost=%s", timer.elapsed(), lost_?"true":"false");
+
+#else
+	UERROR("RTAB-Map is not built with LOAM support! Select another odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap
@@ -0,0 +1,994 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+ * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "rtabmap/core/OdometryMSCKF.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UThread.h"
+
+#ifdef RTABMAP_MSCKF_VIO
+#include <msckf_vio/image_processor.h>
+#include <msckf_vio/msckf_vio.h>
+#include <msckf_vio/math_utils.hpp>
+#include <eigen_conversions/eigen_msg.h>
+#include <boost/math/distributions/chi_squared.hpp>
+#include <pcl/common/transforms.h>
+#endif
+
+namespace rtabmap {
+
+#ifdef RTABMAP_MSCKF_VIO
+class ImageProcessorNoROS: public msckf_vio::ImageProcessor
+{
+public:
+	ImageProcessorNoROS(
+			const ParametersMap & parameters_in,
+			const Transform & imuLocalTransform,
+			const StereoCameraModel & model,
+			bool rectified) :
+				msckf_vio::ImageProcessor(0)
+	{
+		UDEBUG("");
+		// Camera calibration parameters
+		if(model.left().D_raw().cols == 6)
+		{
+			//equidistant
+			cam0_distortion_model = "equidistant";
+			cam0_distortion_coeffs[0] = rectified?0:model.left().D_raw().at<double>(0,0);
+			cam0_distortion_coeffs[1] = rectified?0:model.left().D_raw().at<double>(0,1);
+			cam0_distortion_coeffs[2] = rectified?0:model.left().D_raw().at<double>(0,4);
+			cam0_distortion_coeffs[3] = rectified?0:model.left().D_raw().at<double>(0,5);
+		}
+		else
+		{
+			//radtan
+			cam0_distortion_model = "radtan";
+			cam0_distortion_coeffs[0] = rectified?0:model.left().D_raw().at<double>(0,0);
+			cam0_distortion_coeffs[1] = rectified?0:model.left().D_raw().at<double>(0,1);
+			cam0_distortion_coeffs[2] = rectified?0:model.left().D_raw().at<double>(0,2);
+			cam0_distortion_coeffs[3] = rectified?0:model.left().D_raw().at<double>(0,3);
+		}
+		if(model.right().D_raw().cols == 6)
+		{
+			//equidistant
+			cam1_distortion_model = "equidistant";
+			cam1_distortion_coeffs[0] = rectified?0:model.right().D_raw().at<double>(0,0);
+			cam1_distortion_coeffs[1] = rectified?0:model.right().D_raw().at<double>(0,1);
+			cam1_distortion_coeffs[2] = rectified?0:model.right().D_raw().at<double>(0,4);
+			cam1_distortion_coeffs[3] = rectified?0:model.right().D_raw().at<double>(0,5);
+		}
+		else
+		{
+			//radtan
+			cam1_distortion_model = "radtan";
+			cam1_distortion_coeffs[0] = rectified?0:model.right().D_raw().at<double>(0,0);
+			cam1_distortion_coeffs[1] = rectified?0:model.right().D_raw().at<double>(0,1);
+			cam1_distortion_coeffs[2] = rectified?0:model.right().D_raw().at<double>(0,2);
+			cam1_distortion_coeffs[3] = rectified?0:model.right().D_raw().at<double>(0,3);
+		}
+
+		cam0_resolution[0] = model.left().imageWidth();
+		cam0_resolution[1] = model.left().imageHeight();
+
+		cam1_resolution[0] = model.right().imageWidth();
+		cam1_resolution[1] = model.right().imageHeight();
+
+		cam0_intrinsics[0] = rectified?model.left().fx():model.left().K_raw().at<double>(0,0);
+		cam0_intrinsics[1] = rectified?model.left().fy():model.left().K_raw().at<double>(1,1);
+		cam0_intrinsics[2] = rectified?model.left().cx():model.left().K_raw().at<double>(0,2);
+		cam0_intrinsics[3] = rectified?model.left().cy():model.left().K_raw().at<double>(1,2);
+
+		cam1_intrinsics[0] = rectified?model.right().fx():model.right().K_raw().at<double>(0,0);
+		cam1_intrinsics[1] = rectified?model.right().fy():model.right().K_raw().at<double>(1,1);
+		cam1_intrinsics[2] = rectified?model.right().cx():model.right().K_raw().at<double>(0,2);
+		cam1_intrinsics[3] = rectified?model.right().cy():model.right().K_raw().at<double>(1,2);
+
+		Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
+		cv::Mat     T_imu_cam0 = imuCam.dataMatrix();
+		cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
+		cv::Vec3d   t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3));
+		R_cam0_imu = R_imu_cam0.t();
+		t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0;
+
+		Transform cam0cam1;
+		if(rectified)
+		{
+			cam0cam1 = Transform(
+					1, 0, 0, -model.baseline(),
+					0, 1, 0, 0,
+					0, 0, 1, 0);
+		}
+		else
+		{
+			cam0cam1 = model.stereoTransform();
+		}
+
+		UASSERT(!cam0cam1.isNull());
+		Transform imuCam1 = cam0cam1 * imuCam;
+		cv::Mat T_imu_cam1 = imuCam1.dataMatrix();
+		cv::Matx33d R_imu_cam1(T_imu_cam1(cv::Rect(0,0,3,3)));
+		cv::Vec3d   t_imu_cam1 = T_imu_cam1(cv::Rect(3,0,1,3));
+		R_cam1_imu = R_imu_cam1.t();
+		t_cam1_imu = -R_imu_cam1.t() * t_imu_cam1;
+		// Processor parameters
+		// get all OdomMSCFK group to make sure all parameters are set
+		ParametersMap parameters = Parameters::getDefaultParameters("OdomMSCKF");
+		uInsert(parameters, parameters_in);
+
+		Parameters::parse(parameters, Parameters::kOdomMSCKFGridRow(), processor_config.grid_row); //4
+		Parameters::parse(parameters, Parameters::kOdomMSCKFGridCol(), processor_config.grid_col); //4
+		Parameters::parse(parameters, Parameters::kOdomMSCKFGridMinFeatureNum(), processor_config.grid_min_feature_num); //2
+		Parameters::parse(parameters, Parameters::kOdomMSCKFGridMaxFeatureNum(), processor_config.grid_max_feature_num); //4
+		Parameters::parse(parameters, Parameters::kOdomMSCKFPyramidLevels(), processor_config.pyramid_levels); //3
+		Parameters::parse(parameters, Parameters::kOdomMSCKFPatchSize(), processor_config.patch_size); //31
+		Parameters::parse(parameters, Parameters::kOdomMSCKFFastThreshold(), processor_config.fast_threshold); //20
+		Parameters::parse(parameters, Parameters::kOdomMSCKFMaxIteration(), processor_config.max_iteration); //30
+		Parameters::parse(parameters, Parameters::kOdomMSCKFTrackPrecision(), processor_config.track_precision); //0.01
+		Parameters::parse(parameters, Parameters::kOdomMSCKFRansacThreshold(), processor_config.ransac_threshold); //3
+		Parameters::parse(parameters, Parameters::kOdomMSCKFStereoThreshold(), processor_config.stereo_threshold); //3
+
+		UINFO("===========================================");
+		UINFO("cam0_resolution: %d, %d",
+				cam0_resolution[0], cam0_resolution[1]);
+		UINFO("cam0_intrinscs: %f, %f, %f, %f",
+				cam0_intrinsics[0], cam0_intrinsics[1],
+				cam0_intrinsics[2], cam0_intrinsics[3]);
+		UINFO("cam0_distortion_model: %s",
+				cam0_distortion_model.c_str());
+		UINFO("cam0_distortion_coefficients: %f, %f, %f, %f",
+				cam0_distortion_coeffs[0], cam0_distortion_coeffs[1],
+				cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]);
+
+		UINFO("cam1_resolution: %d, %d",
+				cam1_resolution[0], cam1_resolution[1]);
+		UINFO("cam1_intrinscs: %f, %f, %f, %f",
+				cam1_intrinsics[0], cam1_intrinsics[1],
+				cam1_intrinsics[2], cam1_intrinsics[3]);
+		UINFO("cam1_distortion_model: %s",
+				cam1_distortion_model.c_str());
+		UINFO("cam1_distortion_coefficients: %f, %f, %f, %f",
+				cam1_distortion_coeffs[0], cam1_distortion_coeffs[1],
+				cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]);
+
+		std::cout << "R_imu_cam0: " << R_imu_cam0 << std::endl;
+		std::cout << "t_imu_cam0.t(): " << t_imu_cam0.t() << std::endl;
+		std::cout << "R_imu_cam1: " << R_imu_cam1 << std::endl;
+		std::cout << "t_imu_cam1.t(): " << t_imu_cam1.t() << std::endl;
+
+		UINFO("grid_row: %d",
+				processor_config.grid_row);
+		UINFO("grid_col: %d",
+				processor_config.grid_col);
+		UINFO("grid_min_feature_num: %d",
+				processor_config.grid_min_feature_num);
+		UINFO("grid_max_feature_num: %d",
+				processor_config.grid_max_feature_num);
+		UINFO("pyramid_levels: %d",
+				processor_config.pyramid_levels);
+		UINFO("patch_size: %d",
+				processor_config.patch_size);
+		UINFO("fast_threshold: %d",
+				processor_config.fast_threshold);
+		UINFO("max_iteration: %d",
+				processor_config.max_iteration);
+		UINFO("track_precision: %f",
+				processor_config.track_precision);
+		UINFO("ransac_threshold: %f",
+				processor_config.ransac_threshold);
+		UINFO("stereo_threshold: %f",
+				processor_config.stereo_threshold);
+		UINFO("===========================================");
+
+		// Create feature detector.
+		detector_ptr = cv::FastFeatureDetector::create(
+		  processor_config.fast_threshold);
+	}
+
+	virtual ~ImageProcessorNoROS() {}
+
+	msckf_vio::CameraMeasurementPtr stereoCallback2(
+			const sensor_msgs::ImageConstPtr& cam0_img,
+			const sensor_msgs::ImageConstPtr& cam1_img) {
+
+
+		//cout << "==================================" << endl;
+
+		// Get the current image.
+		cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
+				sensor_msgs::image_encodings::MONO8);
+		cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
+				sensor_msgs::image_encodings::MONO8);
+
+		// Build the image pyramids once since they're used at multiple places
+		createImagePyramids();
+
+		// Detect features in the first frame.
+		if (is_first_img) {
+			//ros::Time start_time = ros::Time::now();
+			initializeFirstFrame();
+			//UINFO("Detection time: %f",
+			//    (ros::Time::now()-start_time).toSec());
+			is_first_img = false;
+
+			// Draw results.
+			//start_time = ros::Time::now();
+			//drawFeaturesStereo();
+			//UINFO("Draw features: %f",
+			//    (ros::Time::now()-start_time).toSec());
+		} else {
+			// Track the feature in the previous image.
+			//ros::Time start_time = ros::Time::now();
+			trackFeatures();
+			//UINFO("Tracking time: %f",
+			//    (ros::Time::now()-start_time).toSec());
+
+			// Add new features into the current image.
+			//start_time = ros::Time::now();
+			addNewFeatures();
+			//UINFO("Addition time: %f",
+			//    (ros::Time::now()-start_time).toSec());
+
+			// Add new features into the current image.
+			//start_time = ros::Time::now();
+			pruneGridFeatures();
+			//UINFO("Prune grid features: %f",
+			//    (ros::Time::now()-start_time).toSec());
+
+			// Draw results.
+			//start_time = ros::Time::now();
+			//drawFeaturesStereo();
+			//UINFO("Draw features: %f",
+			//    (ros::Time::now()-start_time).toSec());
+		}
+
+		//ros::Time start_time = ros::Time::now();
+		//updateFeatureLifetime();
+		//UINFO("Statistics: %f",
+		//    (ros::Time::now()-start_time).toSec());
+
+		// Publish features in the current image.
+		//ros::Time start_time = ros::Time::now();
+		msckf_vio::CameraMeasurementPtr measurements = publish();
+		//UINFO("Publishing: %f",
+		//    (ros::Time::now()-start_time).toSec());
+
+		// Update the previous image and previous features.
+		cam0_prev_img_ptr = cam0_curr_img_ptr;
+		prev_features_ptr = curr_features_ptr;
+		std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);
+
+		// Initialize the current features to empty vectors.
+		curr_features_ptr.reset(new GridFeatures());
+		for (int code = 0; code <
+		processor_config.grid_row*processor_config.grid_col; ++code) {
+			(*curr_features_ptr)[code] = std::vector<FeatureMetaData>(0);
+		}
+
+		return measurements;
+	}
+
+	msckf_vio::CameraMeasurementPtr publish() {
+
+		// Publish features.
+		msckf_vio::CameraMeasurementPtr feature_msg_ptr(new msckf_vio::CameraMeasurement);
+		feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
+
+		std::vector<FeatureIDType> curr_ids(0);
+		std::vector<cv::Point2f> curr_cam0_points(0);
+		std::vector<cv::Point2f> curr_cam1_points(0);
+
+		for (const auto& grid_features : (*curr_features_ptr)) {
+			for (const auto& feature : grid_features.second) {
+				curr_ids.push_back(feature.id);
+				curr_cam0_points.push_back(feature.cam0_point);
+				curr_cam1_points.push_back(feature.cam1_point);
+			}
+		}
+
+		std::vector<cv::Point2f> curr_cam0_points_undistorted(0);
+		std::vector<cv::Point2f> curr_cam1_points_undistorted(0);
+
+		undistortPoints(
+				curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
+				cam0_distortion_coeffs, curr_cam0_points_undistorted);
+		undistortPoints(
+				curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
+				cam1_distortion_coeffs, curr_cam1_points_undistorted);
+
+		for (unsigned int i = 0; i < curr_ids.size(); ++i) {
+			feature_msg_ptr->features.push_back(msckf_vio::FeatureMeasurement());
+			feature_msg_ptr->features[i].id = curr_ids[i];
+			feature_msg_ptr->features[i].u0 = curr_cam0_points_undistorted[i].x;
+			feature_msg_ptr->features[i].v0 = curr_cam0_points_undistorted[i].y;
+			feature_msg_ptr->features[i].u1 = curr_cam1_points_undistorted[i].x;
+			feature_msg_ptr->features[i].v1 = curr_cam1_points_undistorted[i].y;
+		}
+
+		//feature_pub.publish(feature_msg_ptr);
+
+		// Publish tracking info.
+		/*TrackingInfoPtr tracking_info_msg_ptr(new TrackingInfo());
+		tracking_info_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
+		tracking_info_msg_ptr->before_tracking = before_tracking;
+		tracking_info_msg_ptr->after_tracking = after_tracking;
+		tracking_info_msg_ptr->after_matching = after_matching;
+		tracking_info_msg_ptr->after_ransac = after_ransac;
+		tracking_info_pub.publish(tracking_info_msg_ptr);*/
+
+		return feature_msg_ptr;
+	}
+};
+
+class MsckfVioNoROS: public msckf_vio::MsckfVio
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	MsckfVioNoROS(const ParametersMap & parameters_in,
+			const Transform & imuLocalTransform,
+			const StereoCameraModel & model,
+			bool rectified) :
+			msckf_vio::MsckfVio(0)
+	{
+		UDEBUG("");
+		// get all OdomMSCFK group to make sure all parameters are set
+		parameters_ = Parameters::getDefaultParameters("OdomMSCKF");
+		uInsert(parameters_, parameters_in);
+
+		// Frame id
+		publish_tf = false;
+		frame_rate = 1.0;
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFPositionStdThreshold(), position_std_threshold); //8.0
+
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFRotationThreshold(), rotation_threshold); //0.2618
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFTranslationThreshold(), translation_threshold); //0.4
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFTrackingRateThreshold(), tracking_rate_threshold); //0.5
+
+		// Feature optimization parameters
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFOptTranslationThreshold(), msckf_vio::Feature::optimization_config.translation_threshold); //0.2
+
+		// Noise related parameters
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseGyro(), msckf_vio::IMUState::gyro_noise); //0.001
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseAcc(), msckf_vio::IMUState::acc_noise); //0.01
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseGyroBias(), msckf_vio::IMUState::gyro_bias_noise); //0.001
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseAccBias(), msckf_vio::IMUState::acc_bias_noise); //0.01
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseFeature(), msckf_vio::Feature::observation_noise); //0.01
+
+		// Use variance instead of standard deviation.
+		msckf_vio::IMUState::gyro_noise *= msckf_vio::IMUState::gyro_noise;
+		msckf_vio::IMUState::acc_noise *= msckf_vio::IMUState::acc_noise;
+		msckf_vio::IMUState::gyro_bias_noise *= msckf_vio::IMUState::gyro_bias_noise;
+		msckf_vio::IMUState::acc_bias_noise *= msckf_vio::IMUState::acc_bias_noise;
+		msckf_vio::Feature::observation_noise *= msckf_vio::Feature::observation_noise;
+
+		// Set the initial IMU state.
+		// The intial orientation and position will be set to the origin
+		// implicitly. But the initial velocity and bias can be
+		// set by parameters.
+		// TODO: is it reasonable to set the initial bias to 0?
+		//Parameters::parse(parameters_, "initial_state/velocity/x", state_server.imu_state.velocity(0)); //0.0
+		//Parameters::parse(parameters_, "initial_state/velocity/y", state_server.imu_state.velocity(1)); //0.0
+		//Parameters::parse(parameters_, "initial_state/velocity/z", state_server.imu_state.velocity(2)); //0.0
+
+		// The initial covariance of orientation and position can be
+		// set to 0. But for velocity, bias and extrinsic parameters,
+		// there should be nontrivial uncertainty.
+		double gyro_bias_cov, acc_bias_cov, velocity_cov;
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovVel(), velocity_cov); //0.25
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovGyroBias(), gyro_bias_cov); //1e-4
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovAccBias(), acc_bias_cov); //1e-2
+
+		double extrinsic_rotation_cov, extrinsic_translation_cov;
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExRot(), extrinsic_rotation_cov); //3.0462e-4
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExTrans(), extrinsic_translation_cov); //1e-4
+
+		state_server.state_cov = Eigen::MatrixXd::Zero(21, 21);
+		for (int i = 3; i < 6; ++i)
+			state_server.state_cov(i, i) = gyro_bias_cov;
+		for (int i = 6; i < 9; ++i)
+			state_server.state_cov(i, i) = velocity_cov;
+		for (int i = 9; i < 12; ++i)
+			state_server.state_cov(i, i) = acc_bias_cov;
+		for (int i = 15; i < 18; ++i)
+			state_server.state_cov(i, i) = extrinsic_rotation_cov;
+		for (int i = 18; i < 21; ++i)
+			state_server.state_cov(i, i) = extrinsic_translation_cov;
+
+		// Transformation offsets between the frames involved.
+		Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
+		Eigen::Isometry3d T_imu_cam0(imuCam.toEigen4d());
+		Eigen::Isometry3d T_cam0_imu = T_imu_cam0.inverse();
+
+		state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
+		state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
+		Transform cam0cam1;
+		if(rectified)
+		{
+			cam0cam1 = Transform(
+					1, 0, 0, -model.baseline(),
+					0, 1, 0, 0,
+					0, 0, 1, 0);
+		}
+		else
+		{
+			cam0cam1 = model.stereoTransform();
+		}
+		msckf_vio::CAMState::T_cam0_cam1 = cam0cam1.toEigen3d().matrix();
+		msckf_vio::IMUState::T_imu_body = Transform::getIdentity().toEigen3d().matrix();
+
+		// Maximum number of camera states to be stored
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFMaxCamStateSize(), max_cam_state_size); //30
+
+		UINFO("===========================================");
+		UINFO("fixed frame id: %s", fixed_frame_id.c_str());
+		UINFO("child frame id: %s", child_frame_id.c_str());
+		UINFO("publish tf: %d", publish_tf);
+		UINFO("frame rate: %f", frame_rate);
+		UINFO("position std threshold: %f", position_std_threshold);
+		UINFO("Keyframe rotation threshold: %f", rotation_threshold);
+		UINFO("Keyframe translation threshold: %f", translation_threshold);
+		UINFO("Keyframe tracking rate threshold: %f", tracking_rate_threshold);
+		UINFO("gyro noise: %.10f", msckf_vio::IMUState::gyro_noise);
+		UINFO("gyro bias noise: %.10f", msckf_vio::IMUState::gyro_bias_noise);
+		UINFO("acc noise: %.10f", msckf_vio::IMUState::acc_noise);
+		UINFO("acc bias noise: %.10f", msckf_vio::IMUState::acc_bias_noise);
+		UINFO("observation noise: %.10f", msckf_vio::Feature::observation_noise);
+		UINFO("initial velocity: %f, %f, %f",
+				state_server.imu_state.velocity(0),
+				state_server.imu_state.velocity(1),
+				state_server.imu_state.velocity(2));
+		UINFO("initial gyro bias cov: %f", gyro_bias_cov);
+		UINFO("initial acc bias cov: %f", acc_bias_cov);
+		UINFO("initial velocity cov: %f", velocity_cov);
+		UINFO("initial extrinsic rotation cov: %f",
+				extrinsic_rotation_cov);
+		UINFO("initial extrinsic translation cov: %f",
+				extrinsic_translation_cov);
+
+		std::cout << "T_imu_cam0.linear(): " << T_imu_cam0.linear() << std::endl;
+		std::cout << "T_imu_cam0.translation().transpose(): " << T_imu_cam0.translation().transpose() << std::endl;
+		std::cout << "CAMState::T_cam0_cam1.linear(): " << msckf_vio::CAMState::T_cam0_cam1.linear() << std::endl;
+		std::cout << "CAMState::T_cam0_cam1.translation().transpose(): " << msckf_vio::CAMState::T_cam0_cam1.translation().transpose() << std::endl;
+		std::cout << "IMUState::T_imu_body.linear(): " << msckf_vio::IMUState::T_imu_body.linear() << std::endl;
+		std::cout << "IMUState::T_imu_body.translation().transpose(): " << msckf_vio::IMUState::T_imu_body.translation().transpose() << std::endl;
+
+		UINFO("max camera state #: %d", max_cam_state_size);
+		UINFO("===========================================");
+
+		//if (!loadParameters()) return false;
+		//UINFO("Finish loading ROS parameters...");
+
+		// Initialize state server
+		state_server.continuous_noise_cov =
+				Eigen::Matrix<double, 12, 12>::Zero();
+		state_server.continuous_noise_cov.block<3, 3>(0, 0) =
+				Eigen::Matrix3d::Identity()*msckf_vio::IMUState::gyro_noise;
+		state_server.continuous_noise_cov.block<3, 3>(3, 3) =
+				Eigen::Matrix3d::Identity()*msckf_vio::IMUState::gyro_bias_noise;
+		state_server.continuous_noise_cov.block<3, 3>(6, 6) =
+				Eigen::Matrix3d::Identity()*msckf_vio::IMUState::acc_noise;
+		state_server.continuous_noise_cov.block<3, 3>(9, 9) =
+				Eigen::Matrix3d::Identity()*msckf_vio::IMUState::acc_bias_noise;
+
+		// Initialize the chi squared test table with confidence
+		// level 0.95.
+		for (int i = 1; i < 100; ++i) {
+			boost::math::chi_squared chi_squared_dist(i);
+			chi_squared_test_table[i] =
+					boost::math::quantile(chi_squared_dist, 0.05);
+		}
+
+		// if (!createRosIO()) return false;
+		//UINFO("Finish creating ROS IO...");
+	}
+	virtual ~MsckfVioNoROS() {}
+
+
+	nav_msgs::Odometry featureCallback2(
+			const msckf_vio::CameraMeasurementConstPtr& msg,
+			pcl::PointCloud<pcl::PointXYZ>::Ptr & localMap) {
+
+		nav_msgs::Odometry odom;
+
+		// Return if the gravity vector has not been set.
+		if (!is_gravity_set)
+		{
+			UINFO("Gravity not set yet... waiting for 200 IMU msgs (%d/200)...", (int)imu_msg_buffer.size());
+			return odom;
+		}
+
+		// Start the system if the first image is received.
+		// The frame where the first image is received will be
+		// the origin.
+		if (is_first_img) {
+			is_first_img = false;
+			state_server.imu_state.time = msg->header.stamp.toSec();
+		}
+
+		//static double max_processing_time = 0.0;
+		//static int critical_time_cntr = 0;
+		//double processing_start_time = ros::Time::now().toSec();
+
+		// Propogate the IMU state.
+		// that are received before the image msg.
+		//ros::Time start_time = ros::Time::now();
+		batchImuProcessing(msg->header.stamp.toSec());
+
+		//double imu_processing_time = (
+		//    ros::Time::now()-start_time).toSec();
+
+		// Augment the state vector.
+		//start_time = ros::Time::now();
+		stateAugmentation(msg->header.stamp.toSec());
+		//double state_augmentation_time = (
+		//    ros::Time::now()-start_time).toSec();
+
+		// Add new observations for existing features or new
+		// features in the map server.
+		//start_time = ros::Time::now();
+		addFeatureObservations(msg);
+		//double add_observations_time = (
+		//    ros::Time::now()-start_time).toSec();
+
+		// Perform measurement update if necessary.
+		//start_time = ros::Time::now();
+		removeLostFeatures();
+		//double remove_lost_features_time = (
+		//		ros::Time::now()-start_time).toSec();
+
+		//start_time = ros::Time::now();
+		pruneCamStateBuffer();
+		//double prune_cam_states_time = (
+		//		ros::Time::now()-start_time).toSec();
+
+		// Publish the odometry.
+		//start_time = ros::Time::now();
+		odom = publish(localMap);
+		//double publish_time = (
+		//    ros::Time::now()-start_time).toSec();
+
+		// Reset the system if necessary.
+		onlineReset2();
+
+		/*double processing_end_time = ros::Time::now().toSec();
+		double processing_time =
+				processing_end_time - processing_start_time;
+		if (processing_time > 1.0/frame_rate) {
+			++critical_time_cntr;
+			UINFO("\033[1;31mTotal processing time %f/%d...\033[0m",
+					processing_time, critical_time_cntr);
+			//printf("IMU processing time: %f/%f\n",
+			//    imu_processing_time, imu_processing_time/processing_time);
+			//printf("State augmentation time: %f/%f\n",
+			//    state_augmentation_time, state_augmentation_time/processing_time);
+			//printf("Add observations time: %f/%f\n",
+			//    add_observations_time, add_observations_time/processing_time);
+			printf("Remove lost features time: %f/%f\n",
+					remove_lost_features_time, remove_lost_features_time/processing_time);
+			printf("Remove camera states time: %f/%f\n",
+					prune_cam_states_time, prune_cam_states_time/processing_time);
+			//printf("Publish time: %f/%f\n",
+			//    publish_time, publish_time/processing_time);
+		}*/
+
+		return odom;
+	}
+
+	void onlineReset2() {
+
+		// Never perform online reset if position std threshold
+		// is non-positive.
+		if (position_std_threshold <= 0) return;
+		static long long int online_reset_counter = 0;
+
+		// Check the uncertainty of positions to determine if
+		// the system can be reset.
+		double position_x_std = std::sqrt(state_server.state_cov(12, 12));
+		double position_y_std = std::sqrt(state_server.state_cov(13, 13));
+		double position_z_std = std::sqrt(state_server.state_cov(14, 14));
+
+		if (position_x_std < position_std_threshold &&
+				position_y_std < position_std_threshold &&
+				position_z_std < position_std_threshold) return;
+
+		UWARN("Start %lld online reset procedure...",
+				++online_reset_counter);
+		UINFO("Stardard deviation in xyz: %f, %f, %f",
+				position_x_std, position_y_std, position_z_std);
+
+		// Remove all existing camera states.
+		state_server.cam_states.clear();
+
+		// Clear all exsiting features in the map.
+		map_server.clear();
+
+		// Reset the state covariance.
+		double gyro_bias_cov, acc_bias_cov, velocity_cov;
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovVel(), velocity_cov); //0.25
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovGyroBias(), gyro_bias_cov); //1e-4
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovAccBias(), acc_bias_cov); //1e-2
+
+		double extrinsic_rotation_cov, extrinsic_translation_cov;
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExRot(), extrinsic_rotation_cov); //3.0462e-4
+		Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExTrans(), extrinsic_translation_cov); //1e-4
+
+
+		state_server.state_cov = Eigen::MatrixXd::Zero(21, 21);
+		for (int i = 3; i < 6; ++i)
+			state_server.state_cov(i, i) = gyro_bias_cov;
+		for (int i = 6; i < 9; ++i)
+			state_server.state_cov(i, i) = velocity_cov;
+		for (int i = 9; i < 12; ++i)
+			state_server.state_cov(i, i) = acc_bias_cov;
+		for (int i = 15; i < 18; ++i)
+			state_server.state_cov(i, i) = extrinsic_rotation_cov;
+		for (int i = 18; i < 21; ++i)
+			state_server.state_cov(i, i) = extrinsic_translation_cov;
+
+		UWARN("%lld online reset complete...", online_reset_counter);
+		return;
+	}
+
+	nav_msgs::Odometry publish(pcl::PointCloud<pcl::PointXYZ>::Ptr & feature_msg_ptr) {
+
+		// Convert the IMU frame to the body frame.
+		const msckf_vio::IMUState& imu_state = state_server.imu_state;
+		Eigen::Isometry3d T_i_w = Eigen::Isometry3d::Identity();
+		T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
+		T_i_w.translation() = imu_state.position;
+
+		Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w *
+				msckf_vio::IMUState::T_imu_body.inverse();
+		Eigen::Vector3d body_velocity =
+				msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;
+
+		// Publish tf
+		/*if (publish_tf) {
+	    tf::Transform T_b_w_tf;
+	    tf::transformEigenToTF(T_b_w, T_b_w_tf);
+	    tf_pub.sendTransform(tf::StampedTransform(
+	          T_b_w_tf, time, fixed_frame_id, child_frame_id));
+	  }*/
+
+		// Publish the odometry
+		nav_msgs::Odometry odom_msg;
+		//odom_msg.header.stamp = time;
+		odom_msg.header.frame_id = fixed_frame_id;
+		odom_msg.child_frame_id = child_frame_id;
+
+		tf::poseEigenToMsg(T_b_w, odom_msg.pose.pose);
+		tf::vectorEigenToMsg(body_velocity, odom_msg.twist.twist.linear);
+
+		// Convert the covariance.
+		Eigen::Matrix3d P_oo = state_server.state_cov.block<3, 3>(0, 0);
+		Eigen::Matrix3d P_op = state_server.state_cov.block<3, 3>(0, 12);
+		Eigen::Matrix3d P_po = state_server.state_cov.block<3, 3>(12, 0);
+		Eigen::Matrix3d P_pp = state_server.state_cov.block<3, 3>(12, 12);
+		Eigen::Matrix<double, 6, 6> P_imu_pose = Eigen::Matrix<double, 6, 6>::Zero();
+		P_imu_pose << P_pp, P_po, P_op, P_oo;
+
+		Eigen::Matrix<double, 6, 6> H_pose = Eigen::Matrix<double, 6, 6>::Zero();
+		H_pose.block<3, 3>(0, 0) = msckf_vio::IMUState::T_imu_body.linear();
+		H_pose.block<3, 3>(3, 3) = msckf_vio::IMUState::T_imu_body.linear();
+		Eigen::Matrix<double, 6, 6> P_body_pose = H_pose *
+				P_imu_pose * H_pose.transpose();
+
+		for (int i = 0; i < 6; ++i)
+			for (int j = 0; j < 6; ++j)
+				odom_msg.pose.covariance[6*i+j] = P_body_pose(i, j);
+
+		// Construct the covariance for the velocity.
+		Eigen::Matrix3d P_imu_vel = state_server.state_cov.block<3, 3>(6, 6);
+		Eigen::Matrix3d H_vel = msckf_vio::IMUState::T_imu_body.linear();
+		Eigen::Matrix3d P_body_vel = H_vel * P_imu_vel * H_vel.transpose();
+		for (int i = 0; i < 3; ++i)
+			for (int j = 0; j < 3; ++j)
+				odom_msg.twist.covariance[i*6+j] = P_body_vel(i, j);
+
+		// odom_pub.publish(odom_msg);
+
+		// Publish the 3D positions of the features that
+		// has been initialized.
+		feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
+	  feature_msg_ptr->header.frame_id = fixed_frame_id;
+	  feature_msg_ptr->height = 1;
+	  for (const auto& item : map_server) {
+	    const auto& feature = item.second;
+	    if (feature.is_initialized) {
+	      Eigen::Vector3d feature_position =
+	        msckf_vio::IMUState::T_imu_body.linear() * feature.position;
+	      feature_msg_ptr->points.push_back(pcl::PointXYZ(
+	            feature_position(0), feature_position(1), feature_position(2)));
+	    }
+	  }
+	  feature_msg_ptr->width = feature_msg_ptr->points.size();
+
+	  //feature_pub.publish(feature_msg_ptr);
+
+		return odom_msg;
+	}
+
+private:
+	ParametersMap parameters_;
+};
+#endif
+
+OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
+									Odometry(parameters)
+#ifdef RTABMAP_MSCKF_VIO
+,
+imageProcessor_(0),
+msckf_(0),
+parameters_(parameters),
+flipXY_(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0),
+previousPose_(Transform::getIdentity()),
+initGravity_(false)
+#endif
+{
+}
+
+OdometryMSCKF::~OdometryMSCKF()
+{
+	UDEBUG("");
+#ifdef RTABMAP_MSCKF_VIO
+	delete imageProcessor_;
+	delete msckf_;
+#endif
+}
+
+void OdometryMSCKF::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_MSCKF_VIO
+	if(!initGravity_)
+	{
+		if(imageProcessor_)
+		{
+			delete imageProcessor_;
+			imageProcessor_ = 0;
+		}
+		if(msckf_)
+		{
+			delete msckf_;
+			msckf_ = 0;
+		}
+		lastImu_ = IMU();
+		previousPose_.setIdentity();
+	}
+	initGravity_ = false;
+#endif
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryMSCKF::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	UDEBUG("");
+	Transform t;
+
+#ifdef RTABMAP_MSCKF_VIO
+	UTimer timer;
+
+	if(!data.imu().empty())
+	{
+		UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(),
+				data.imu().linearAcceleration()[0],
+				data.imu().linearAcceleration()[1],
+				data.imu().linearAcceleration()[2],
+				data.imu().angularVelocity()[0],
+				data.imu().angularVelocity()[1],
+				data.imu().angularVelocity()[2]);
+		if(imageProcessor_ && msckf_)
+		{
+			sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
+			msg->angular_velocity.x = data.imu().angularVelocity()[0];
+			msg->angular_velocity.y = data.imu().angularVelocity()[1];
+			msg->angular_velocity.z = data.imu().angularVelocity()[2];
+			msg->linear_acceleration.x = data.imu().linearAcceleration()[0];
+			msg->linear_acceleration.y = data.imu().linearAcceleration()[1];
+			msg->linear_acceleration.z = data.imu().linearAcceleration()[2];
+			msg->header.stamp.fromSec(data.stamp());
+			imageProcessor_->imuCallback(msg);
+			msckf_->imuCallback(msg);
+		}
+		else
+		{
+			UWARN("Ignoring IMU, waiting for an image to initialize...");
+			lastImu_ = data.imu();
+		}
+	}
+
+	if(!data.imageRaw().empty() && !data.rightRaw().empty())
+	{
+		UDEBUG("Image update stamp=%f", data.stamp());
+		if(data.stereoCameraModel().isValidForProjection())
+		{
+			if(msckf_ == 0)
+			{
+				UINFO("Initialization");
+				if(lastImu_.empty())
+				{
+					UWARN("Ignoring Image, waiting for imu to initialize...");
+					return t;
+				}
+				UINFO("Creating ImageProcessorNoROS...");
+				imageProcessor_ = new ImageProcessorNoROS(
+						parameters_,
+						lastImu_.localTransform(),
+						data.stereoCameraModel(),
+						this->imagesAlreadyRectified());
+				UINFO("Creating MsckfVioNoROS...");
+				msckf_ = new MsckfVioNoROS(
+						parameters_,
+						lastImu_.localTransform(),
+						data.stereoCameraModel(),
+						this->imagesAlreadyRectified());
+			}
+
+			// Convert to ROS
+			cv_bridge::CvImage cam0;
+			cv_bridge::CvImage cam1;
+			cam0.header.stamp.fromSec(data.stamp());
+			cam1.header.stamp.fromSec(data.stamp());
+
+			if(data.imageRaw().type() == CV_8UC3)
+			{
+				cv::cvtColor(data.imageRaw(), cam0.image, CV_BGR2GRAY);
+			}
+			else
+			{
+				cam0.image = data.imageRaw();
+			}
+			if(data.rightRaw().type() == CV_8UC3)
+			{
+				cv::cvtColor(data.rightRaw(), cam1.image, CV_BGR2GRAY);
+			}
+			else
+			{
+				cam1.image = data.rightRaw();
+			}
+
+			sensor_msgs::ImagePtr cam0Msg(new sensor_msgs::Image);
+			sensor_msgs::ImagePtr cam1Msg(new sensor_msgs::Image);
+			cam0.toImageMsg(*cam0Msg);
+			cam1.toImageMsg(*cam1Msg);
+			cam0Msg->encoding = sensor_msgs::image_encodings::MONO8;
+			cam1Msg->encoding = sensor_msgs::image_encodings::MONO8;
+
+			msckf_vio::CameraMeasurementPtr measurements = imageProcessor_->stereoCallback2(cam0Msg, cam1Msg);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr localMap;
+			nav_msgs::Odometry odom = msckf_->featureCallback2(measurements, localMap);
+
+			Transform p = Transform(
+					odom.pose.pose.position.x,
+					odom.pose.pose.position.y,
+					odom.pose.pose.position.z,
+					odom.pose.pose.orientation.x,
+					odom.pose.pose.orientation.y,
+					odom.pose.pose.orientation.z,
+					odom.pose.pose.orientation.w);
+
+			if(!p.isNull())
+			{
+				// pose in rtabmap/ros coordinates
+				p = flipXY_*p*lastImu_.localTransform();
+
+				if(this->getPose().rotation().isIdentity())
+				{
+					initGravity_ = true;
+					this->reset(this->getPose()*p.rotation());
+				}
+
+				if(previousPose_.isIdentity())
+				{
+					previousPose_ = p;
+				}
+
+				// make it incremental
+				Transform previousPoseInv = previousPose_.inverse();
+				t = previousPoseInv*p;
+				previousPose_ = p;
+
+				if(info)
+				{
+					info->type = this->getType();
+					info->features = measurements->features.size();
+
+					info->reg.covariance = cv::Mat::zeros(6, 6, CV_64FC1);
+					cv::Mat twistCov(6,6,CV_64FC1, odom.twist.covariance.elems);
+					// twist covariance is not in base frame, but in world frame,
+					// we have to convert the covariance in base frame
+					cv::Matx31f covWorldFrame(twistCov.at<double>(0, 0),
+											  twistCov.at<double>(1, 1),
+											  twistCov.at<double>(2, 2));
+					cv::Matx31f covBaseFrame = cv::Matx33f(previousPoseInv.rotationMatrix()) * covWorldFrame;
+					// we set only diagonal values as there is an issue with g2o and off-diagonal values
+					info->reg.covariance.at<double>(0, 0) = fabs(covBaseFrame.val[0])/10.0;
+					info->reg.covariance.at<double>(1, 1) = fabs(covBaseFrame.val[1])/10.0;
+					info->reg.covariance.at<double>(2, 2) = fabs(covBaseFrame.val[2])/10.0;
+					if(info->reg.covariance.at<double>(0, 0) < 0.0001)
+					{
+						info->reg.covariance.at<double>(0, 0) = 0.0001;
+					}
+					if(info->reg.covariance.at<double>(1, 1) < 0.0001)
+					{
+						info->reg.covariance.at<double>(1, 1) = 0.0001;
+					}
+					if(info->reg.covariance.at<double>(2, 2) < 0.0001)
+					{
+						info->reg.covariance.at<double>(2, 2) = 0.0001;
+					}
+					info->reg.covariance.at<double>(3, 3) = msckf_vio::IMUState::gyro_noise*10.0;
+					info->reg.covariance.at<double>(4, 4) = info->reg.covariance.at<double>(3, 3);
+					info->reg.covariance.at<double>(5, 5) = info->reg.covariance.at<double>(3, 3);
+
+					if(this->isInfoDataFilled())
+					{
+						if(localMap.get() && localMap->size())
+						{
+							Eigen::Affine3f flip = (this->getPose()*previousPoseInv*flipXY_).toEigen3f();
+							for(unsigned int i=0; i<localMap->size(); ++i)
+							{
+								pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), flip);
+								info->localMap.insert(std::make_pair(i, cv::Point3f(pt.x, pt.y, pt.z)));
+							}
+						}
+						if(this->imagesAlreadyRectified())
+						{
+							info->newCorners.resize(measurements->features.size());
+							float fx = data.stereoCameraModel().left().fx();
+							float fy = data.stereoCameraModel().left().fy();
+							float cx = data.stereoCameraModel().left().cx();
+							float cy = data.stereoCameraModel().left().cy();
+							info->reg.inliersIDs.resize(measurements->features.size());
+							for(unsigned int i=0; i<measurements->features.size(); ++i)
+							{
+								info->newCorners[i].x = measurements->features[i].u0*fx+cx;
+								info->newCorners[i].y = measurements->features[i].v0*fy+cy;
+								info->reg.inliersIDs[i] = i;
+							}
+						}
+					}
+				}
+			}
+
+			UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
+		}
+	}
+
+#else
+	UERROR("RTAB-Map is not built with MSCKF_VIO support! Select another visual odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UDirectory.h"
+#include <pcl/common/transforms.h>

 #ifdef RTABMAP_ORB_SLAM2
 #include <System.h>
@@ -736,10 +737,7 @@ public:
 	virtual ~ORBSLAM2System()
 	{
 		shutdown();
-		if(mpVocabulary)
-		{
-			delete mpVocabulary;
-		}
+		delete mpVocabulary;
 	}

 	void shutdown()
@@ -813,7 +811,8 @@ OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
 #ifdef RTABMAP_ORB_SLAM2
    ,
 	orbslam2_(0),
-	firstFrame_(true)
+	firstFrame_(true),
+	previousPose_(Transform::getIdentity())
 #endif
 {
 #ifdef RTABMAP_ORB_SLAM2
@@ -841,6 +840,7 @@ void OdometryORBSLAM2::reset(const Transform & initialPose)
 	}
 	firstFrame_ = true;
 	originLocalTransform_.setNull();
+	previousPose_.setIdentity();
 #endif
 }

@@ -859,7 +859,8 @@ Transform OdometryORBSLAM2::computeTransform(
 		data.imageRaw().rows != data.depthOrRightRaw().rows ||
 		data.imageRaw().cols != data.depthOrRightRaw().cols)
 	{
-		UERROR("Not supported input!");
+		UERROR("Not supported input! RGB (%dx%d) and depth (%dx%d) should have the same size.",
+				data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
 		return t;
 	}

@@ -907,23 +908,29 @@ Transform OdometryORBSLAM2::computeTransform(
 		Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
 	}

+	Transform previousPoseInv = previousPose_.inverse();
 	if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST)
 	{
 		covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
 	}
 	else if(Tcw.cols == 4 && Tcw.rows == 4)
 	{
-		t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)));
+		Transform p = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)));

-		if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+		if(!p.isNull())
 		{
-			if(originLocalTransform_.isNull())
+			if(!localTransform.isNull())
 			{
-				originLocalTransform_ = localTransform;
+				if(originLocalTransform_.isNull())
+				{
+					originLocalTransform_ = localTransform;
+				}
+				// transform in base frame
+				p = originLocalTransform_ * p.inverse() * localTransform.inverse();
 			}
-			t = originLocalTransform_ * t.inverse() * localTransform.inverse();
-			t = this->getPose().inverse() * t;
+			t = previousPoseInv*p;
 		}
+		previousPose_ = p;

 		if(firstFrame_)
 		{
@@ -1003,10 +1010,12 @@ Transform OdometryORBSLAM2::computeTransform(
 			info->reg.matches = oi;

 			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
+			Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)
 			{
-				cv::Mat pt = mapPoints[i]->GetWorldPos();
-				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_)));
+				cv::Point3f pt(mapPoints[i]->GetWorldPos());
+				pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt.x, pt.y, pt.z), fixRot);
+				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
 			}
 		}
 	}
@@ -131,9 +131,11 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
 #ifdef RTABMAP_OKVIS
 	okvisCallbackHandler_(new OkvisCallbackHandler),
 	okvisEstimator_(0),
+	imagesProcessed_(0),
+	initGravity_(false),
 #endif
 	okvisParameters_(parameters),
-	imagesProcessed_(0)
+	previousPose_(Transform::getIdentity())
 {
 #ifdef RTABMAP_OKVIS
 	Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
@@ -148,10 +150,7 @@ OdometryOkvis::~OdometryOkvis()
 {
 	UDEBUG("");
 #ifdef RTABMAP_OKVIS
-	if(okvisEstimator_)
-	{
-		delete okvisEstimator_;
-	}
+	delete okvisEstimator_;
 	delete okvisCallbackHandler_;
 #endif
 }
@@ -160,17 +159,22 @@ void OdometryOkvis::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
 #ifdef RTABMAP_OKVIS
-	if(okvisEstimator_)
+	if(!initGravity_)
 	{
-		delete okvisEstimator_;
-		okvisEstimator_ = 0;
-	}
-	lastImu_ = IMU();
+		if(okvisEstimator_)
+		{
+			delete okvisEstimator_;
+			okvisEstimator_ = 0;
+		}
+		lastImu_ = IMU();
+		imagesProcessed_ = 0;
+		previousPose_.setIdentity();

-	delete okvisCallbackHandler_;
-	okvisCallbackHandler_ = new OkvisCallbackHandler();
+		delete okvisCallbackHandler_;
+		okvisCallbackHandler_ = new OkvisCallbackHandler();
+	}
+	initGravity_ = false;
 #endif
-	imagesProcessed_ = 0;
 }

 // return not null transform if odometry is correctly computed
@@ -304,6 +308,7 @@ Transform OdometryOkvis::computeTransform(

 				// non-hard coded parameters
 				parameters.imu.T_BS = okvis::kinematics::Transformation(lastImu_.localTransform().toEigen4d());
+				UINFO("Images are already rectified = %s", imagesAlreadyRectified()?"true":"false");
 				for(unsigned int i=0; i<models.size(); ++i)
 				{
 					okvis::cameras::NCameraSystem::DistortionType distType = okvis::cameras::NCameraSystem::NoDistortion;
@@ -451,8 +456,21 @@ Transform OdometryOkvis::computeTransform(
 		if(!p.isNull())
 		{
 			p = fixPos * p * fixRot;
+
+			if(this->getPose().rotation().isIdentity())
+			{
+				initGravity_ = true;
+				this->reset(this->getPose()*p.rotation());
+			}
+
+			if(previousPose_.isIdentity())
+			{
+				previousPose_ = p;
+			}
+
 			// make it incremental
-			t = this->getPose().inverse()*p;
+			t = previousPose_.inverse()*p;
+			previousPose_ = p;

 			if(info)
 			{
@@ -468,7 +486,10 @@ Transform OdometryOkvis::computeTransform(
 				}*/
 			}
 		}
-		UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
+		if(imageUpdated)
+		{
+			UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
+		}
 	}
 #else
 	UERROR("RTAB-Map is not built with OKVIS support! Select another visual odometry approach.");
@@ -51,10 +51,7 @@ OdometryThread::~OdometryThread()
 {
 	this->unregisterFromEventsManager();
 	this->join(true);
-	if(_odometry)
-	{
-		delete _odometry;
-	}
+	delete _odometry;
 	UDEBUG("");
 }

@@ -119,9 +116,9 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		UDEBUG("Processing data...");
 		Transform pose = _odometry->process(data, &info);
-		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
-		if(!data.imageRaw().empty() || pose.isNull())
+		if(!data.imageRaw().empty() || (pose.isNull() && data.imu().empty()))
 		{
+			UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
 			// a null pose notify that odometry could not be computed
 			this->post(new OdometryEvent(data, pose, info));
 		}
@@ -196,7 +193,7 @@ bool OdometryThread::getData(SensorData & data)
 		if(!_dataBuffer.empty() || !_imuBuffer.empty())
 		{
 			if(_dataBuffer.empty() ||
-				(!_dataBuffer.empty() && !_imuBuffer.empty() && _imuBuffer.front().stamp() <= _dataBuffer.front().stamp()))
+				(!_dataBuffer.empty() && !_imuBuffer.empty() && _imuBuffer.front().stamp() < _dataBuffer.front().stamp()))
 			{
 				data = _imuBuffer.front();
 				_imuBuffer.pop_front();
@@ -71,10 +71,7 @@ OdometryViso2::OdometryViso2(const ParametersMap & parameters) :
 OdometryViso2::~OdometryViso2()
 {
 #ifdef RTABMAP_VISO2
-	if(viso2_)
-	{
-		delete viso2_;
-	}
+	delete viso2_;
 #endif
 }

@@ -328,10 +328,29 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	return std::map<int, Transform>();
 }

+std::map<int, Transform> Optimizer::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
+{
+	cv::Mat covariance;
+	return optimize(rootId,
+			poses,
+			edgeConstraints,
+			covariance,
+			intermediateGraphes,
+			finalError,
+			iterationsDone);
+}
+
 std::map<int, Transform> Optimizer::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,
 		double * finalError,
 		int * iterationsDone)
@@ -46,7 +46,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
 #include "g2o/core/robust_kernel_impl.h"
-#include "g2o/core/linear_solver.h"
+namespace g2o {
+// bug #include "g2o/core/eigen_types.h" not found on Indigo
+typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXD;
+}

 #ifdef RTABMAP_G2O
 #include "g2o/types/sba/types_sba.h"
@@ -164,10 +167,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes,
 		double * finalError,
 		int * iterationsDone)
 {
+	outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
 	std::map<int, Transform> optimizedPoses;
 #ifdef RTABMAP_G2O
 	UDEBUG("Optimizing graph...");
@@ -530,7 +535,14 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		UDEBUG("Initial optimization...");
 		optimizer.initializeOptimization();

-		UASSERT(optimizer.verifyInformationMatrices());
+		UASSERT_MSG(optimizer.verifyInformationMatrices(true),
+				"This error can be caused by (1) bad covariance matrix "
+				"set in odometry messages "
+				"(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
+				"or that (2) PCL and g2o hadn't "
+				"been built both with or without \"-march=native\" compilation "
+				"flag (if one library is built with this flag and not the other, "
+				"this is causing Eigen to not work properly, resulting in segmentation faults).");

 		UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		int it = 0;
@@ -659,6 +671,41 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					UERROR("Vertex %d not found!?", iter->first);
 				}
 			}
+
+			g2o::VertexSE2* v = (g2o::VertexSE2*)optimizer.vertex(poses.rbegin()->first);
+			if(v)
+			{
+				UTimer t;
+				g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
+				optimizer.computeMarginals(spinv, v);
+				UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
+				if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
+				{
+					g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
+					UASSERT(block && block->cols() == 3 && block->cols() == 3);
+					outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
+					outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
+					outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
+					outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
+					outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
+					outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
+					outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
+					outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
+					outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
+				}
+				else if(v->hessianIndex() < 0)
+				{
+					UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
+				}
+				else
+				{
+					UWARN("Computing marginals: vertex %d has hessian not valid (%d > block size=%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex(), (int)spinv.blockCols().size());
+				}
+			}
+			else
+			{
+				UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
+			}
 		}
 		else
 		{
@@ -676,6 +723,35 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					UERROR("Vertex %d not found!?", iter->first);
 				}
 			}
+
+			g2o::VertexSE3* v = (g2o::VertexSE3*)optimizer.vertex(poses.rbegin()->first);
+			if(v)
+			{
+				UTimer t;
+				g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
+				optimizer.computeMarginals(spinv, v);
+				UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
+				if(v->hessianIndex() >= 0 && v->hessianIndex() < (int)spinv.blockCols().size())
+				{
+					g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
+					UASSERT(block && block->cols() == 6 && block->cols() == 6);
+					memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
+				}
+				else if(v->hessianIndex() < 0)
+				{
+					UWARN("Computing marginals: vertex %d has negative hessian index (%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex());
+				}
+#ifdef RTABMAP_G2O_CPP11
+				else
+				{
+					UWARN("Computing marginals: vertex %d has hessian not valid (%d > block size=%d). Cannot compute last pose covariance.", poses.rbegin()->first, v->hessianIndex(), (int)spinv.blockCols().size());
+				}
+#endif
+			}
+			else
+			{
+				UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
+			}
 		}
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
@@ -975,16 +1051,31 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 		UDEBUG("fill 3D points to g2o...");
 		const int stepVertexId = poses.rbegin()->first+1;
+		int negVertexOffset = stepVertexId;
+		if(wordReferences.size() && wordReferences.rbegin()->first>0)
+		{
+			negVertexOffset += wordReferences.rbegin()->first;
+		}
+		UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
 		std::list<g2o::OptimizableGraph::Edge*> edges;
 		for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 		{
-			if(points3DMap.find(iter->first) != points3DMap.end())
+			int id = iter->first;
+			if(points3DMap.find(id) != points3DMap.end())
 			{
-				cv::Point3f pt3d = points3DMap.at(iter->first);
+				cv::Point3f pt3d = points3DMap.at(id);
 				g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();

 				vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
-				vpt3d->setId(stepVertexId + iter->first);
+				if(id<0)
+				{
+					vpt3d->setId(negVertexOffset + id*-1);
+				}
+				else
+				{
+					vpt3d->setId(stepVertexId + id);
+				}
+				UASSERT(vpt3d->id() > 0);
 				vpt3d->setMarginalized(true);
 				optimizer.addVertex(vpt3d);

@@ -1142,7 +1233,15 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #endif

-						cv::Point3f pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+						cv::Point3f pt3d;
+						if((*iter)->vertex(0)->id() > negVertexOffset)
+						{
+							pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
+						}
+						else
+						{
+							pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+						}
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));

 						if(outliers)
@@ -1218,7 +1317,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 		for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 		{
-			const g2o::VertexSBAPointXYZ* v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + iter->first);
+			const g2o::VertexSBAPointXYZ* v;
+			int id = iter->first;
+			if(id<0)
+			{
+				v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(negVertexOffset + id*-1);
+			}
+			else
+			{
+				v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + id);
+			}
+
 			if(v)
 			{
 				cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
@@ -79,10 +79,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes,
 		double * finalError,
 		int * iterationsDone)
 {
+	outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
 	std::map<int, Transform> optimizedPoses;
 #ifdef RTABMAP_GTSAM

@@ -381,6 +383,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
 				optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());

+		gtsam::Marginals marginals(graph, optimizer->values());
 		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
 		{
 			if(iter->value.dim() > 1)
@@ -397,6 +400,40 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 			}
 		}
+
+		// compute marginals
+		try {
+			UTimer t;
+			gtsam::Marginals marginals(graph, optimizer->values());
+			gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
+			UINFO("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
+			if(isSlam2d())
+			{
+				UASSERT(info.cols() == 3 && info.cols() == 3);
+				outputCovariance.at<double>(0,0) = info(0,0); // x-x
+				outputCovariance.at<double>(0,1) = info(0,1); // x-y
+				outputCovariance.at<double>(0,5) = info(0,2); // x-theta
+				outputCovariance.at<double>(1,0) = info(1,0); // y-x
+				outputCovariance.at<double>(1,1) = info(1,1); // y-y
+				outputCovariance.at<double>(1,5) = info(1,2); // y-theta
+				outputCovariance.at<double>(5,0) = info(2,0); // theta-x
+				outputCovariance.at<double>(5,1) = info(2,1); // theta-y
+				outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
+			}
+			else
+			{
+				UASSERT(info.cols() == 6 && info.cols() == 6);
+				Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
+				mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
+				mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
+				mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
+				mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
+				memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
+			}
+		} catch(std::exception& e) {
+			cout << e.what() << endl;
+		}
+
 		delete optimizer;
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
@@ -55,10 +55,12 @@ std::map<int, Transform> OptimizerTORO::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
 		double * finalError,
 		int * iterationsDone)
 {
+	outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
 	std::map<int, Transform> optimizedPoses;
 #ifdef RTABMAP_TORO
 	UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
@@ -312,6 +314,8 @@ std::map<int, Transform> OptimizerTORO::optimize(
 				optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
 			}
 		}
+
+		// TORO doesn't compute marginals...
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
 	{
@@ -227,6 +227,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

+		// 0.17.5
+		removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr",     std::make_pair(true,  Parameters::kGridGlobalOccupancyThr())));
+
 		// 0.17.0
 		removedParameters_.insert(std::make_pair("Grid/Scan2dMaxFilledRange",     std::make_pair(false,  Parameters::kGridRangeMax())));

@@ -514,7 +517,15 @@ void Parameters::parse(const ParametersMap & parameters, ParametersMap & paramet

 const char * Parameters::showUsage()
 {
-	return  "Logger options:\n"
+	return  "RTAB-Map options:\n"
+			"   --help                         Show usage.\n"
+			"   --version                      Show version of rtabmap and its dependencies.\n"
+			"   --params                       Show all parameters with their default value and description\n"
+			"   --\"parameter name\" \"value\"     Overwrite a specific RTAB-Map's parameter :\n"
+			"                                    --SURF/HessianThreshold 150\n"
+			"                                   For parameters in table format, add ',' between values :\n"
+			"                                    --Kp/RoiRatios 0,0,0.1,0\n"
+			"Logger options:\n"
 			"   --nolog              Disable logger\n"
 			"   --logconsole         Set logger console type\n"
 			"   --logfile \"path\"     Set logger file type\n"
@@ -526,12 +537,6 @@ const char * Parameters::showUsage()
 			"   --logtime \"bool\"     Print time when logging\n"
 			"   --logwhere \"bool\"    Print where when logging\n"
 			"   --logthread \"bool\"   Print thread id when logging\n"
-			"RTAB-Map options:\n"
-			"   --params                       Show all parameters with their default value and description\n"
-			"   --\"parameter name\" \"value\"     Overwrite a specific RTAB-Map's parameter :\n"
-			"                                    --SURF/HessianThreshold 150\n"
-			"                                   For parameters in table format, add ',' between values :\n"
-			"                                    --Kp/RoiRatios 0,0,0.1,0\n"
 			;
 }

@@ -545,7 +550,188 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 		bool checkParameters = onlyParameters;
 		if(!checkParameters)
 		{
-			if(strcmp(argv[i], "--nolog") == 0)
+			if(strcmp(argv[i], "--help") == 0)
+			{
+				std::cout << showUsage() << std::endl;
+				exit(0);
+			}
+			else if(strcmp(argv[i], "--version") == 0)
+			{
+				std::string str = "RTAB-Map:";
+
+				int spacing = 30;
+				std::cout << str << std::setw(spacing - str.size()) << RTABMAP_VERSION << std::endl;
+				str = "OpenCV:";
+#ifdef RTABMAP_OPENCV3
+				std::cout << str << std::setw(spacing - str.size()) << "3" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "2" << std::endl;
+#endif
+				str = "With OpenCV nonfree:";
+#ifdef RTABMAP_NONFREE
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With TORO:";
+#ifdef RTABMAP_TORO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With g2o:";
+#ifdef RTABMAP_G2O
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With GTSAM:";
+#ifdef RTABMAP_GTSAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Vertigo:";
+#ifdef RTABMAP_VERTIGO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With CVSBA:";
+#ifdef RTABMAP_CVSBA
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OpenNI2:";
+#ifdef RTABMAP_OPENNI2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Freenect:";
+#ifdef RTABMAP_FREENECT
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Freenect2:";
+#ifdef RTABMAP_FREENECT2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With K4W2:";
+#ifdef RTABMAP_K4W2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With DC1394:";
+#ifdef RTABMAP_DC1394
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With FlyCapture2:";
+#ifdef RTABMAP_FLYCAPTURE2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With ZED:";
+#ifdef RTABMAP_ZED
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With RealSense:";
+#ifdef RTABMAP_REALSENSE
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With RealSense SLAM:";
+#ifdef RTABMAP_REALSENSE_SLAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With RealSense2:";
+#ifdef RTABMAP_REALSENSE2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With libpointmatcher:";
+#ifdef RTABMAP_POINTMATCHER
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With octomap:";
+#ifdef RTABMAP_OCTOMAP
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With cpu-tsdf:";
+#ifdef RTABMAP_CPUTSDF
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With open chisel:";
+#ifdef RTABMAP_OPENCHISEL
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With LOAM:";
+#ifdef RTABMAP_LOAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With FOVIS:";
+#ifdef RTABMAP_FOVIS
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Viso2:";
+#ifdef RTABMAP_VISO2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With DVO:";
+#ifdef RTABMAP_DVO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With ORB_SLAM2:";
+#ifdef RTABMAP_ORB_SLAM2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OKVIS:";
+#ifdef RTABMAP_OKVIS
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With MSCKF_VIO:";
+#ifdef RTABMAP_MSCKF_VIO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				exit(0);
+			}
+			else if(strcmp(argv[i], "--nolog") == 0)
 			{
 				ULogger::setType(ULogger::kTypeNoLog);
 			}
@@ -679,6 +865,18 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 					{
 						ignore = true;
 					}
+#endif
+#ifndef RTABMAP_LOAM
+					if(group.compare("OdomLOAM") == 0)
+					{
+						ignore = true;
+					}
+#endif
+#ifndef RTABMAP_MSCKF_VIO
+					if(group.compare("OdomMSCKF") == 0)
+					{
+						ignore = true;
+					}
 #endif
 					if(!ignore)
 					{
@@ -796,17 +994,15 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 				key = uReplaceChar(key, '\\', '/'); // Ini files use \ by default for separators, so replace them

 				// look for old parameter name
-				bool addParameter = true;
 				std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
 				if(oldIter!=Parameters::getRemovedParameters().end())
 				{
-					addParameter = oldIter->second.first;
-					if(addParameter)
+					if(oldIter->second.first)
 					{
 						if(parameters.find(oldIter->second.second) == parameters.end())
 						{
 							key = oldIter->second.second;
-							UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
+							UINFO("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
 									oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second, Parameters::getDefaultParameters().at(oldIter->second.second).c_str());
 						}
 					}
@@ -73,10 +73,7 @@ Registration::Registration(const ParametersMap & parameters, Registration * chil

 Registration::~Registration()
 {
-	if(child_)
-	{
-		delete child_;
-	}
+	delete child_;
 }
 void Registration::parseParameters(const ParametersMap & parameters)
 {
@@ -389,10 +389,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 RegistrationIcp::~RegistrationIcp()
 {
 #ifdef RTABMAP_POINTMATCHER
-	if(_libpointmatcherICP)
-	{
-		delete (PM::ICP*)_libpointmatcherICP;
-	}
+	delete (PM::ICP*)_libpointmatcherICP;
 #endif
 }

@@ -1049,17 +1046,29 @@ Transform RegistrationIcp::computeTransformationImpl(
 							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
 							icpT = guess * t.inverse() * guessInv;

-							// we were using normals, so compute correspondences using normals
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
-							pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+							if(fromScan.hasNormals() && toScan.hasNormals())
+							{
+								// we were using normals, so compute correspondences using normals
+								pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
+								pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());

-							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
-									toCloudNormals,
-									_maxCorrespondenceDistance,
-									_maxRotation,
-									variance,
-									correspondences);
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudNormalsRegistered,
+										toCloudNormals,
+										_maxCorrespondenceDistance,
+										_maxRotation,
+										variance,
+										correspondences);
+							}
+							else
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudRegistered,
+										toCloudFiltered,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
+							}
 						}
 						else
 						{
@@ -1100,6 +1109,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					// verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
 					int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
 					UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
+
 					if(maxLaserScans)
 					{
 						correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -1128,10 +1138,17 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);

-					info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					if(correspondences == 0)
+					{
+						UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
+					}
+					else
+					{
+						info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					}
 					info.icpInliersRatio = correspondencesRatio;

-					if(correspondencesRatio < _correspondenceRatio)
+					if(correspondencesRatio <= _correspondenceRatio)
 					{
 						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
 								correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
@@ -114,8 +114,9 @@ Rtabmap::Rtabmap() :
 	_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
-	_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
+	_optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
+	_startNewMapOnGoodSignature(Parameters::defaultRtabmapStartNewMapOnGoodSignature()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
 	_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
@@ -454,8 +455,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_proximityAngle *= M_PI/180.0f;
 	}
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
-	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
+	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
 	Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
 	Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
@@ -795,7 +797,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,

 		if(optimized)
 		{
-			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+			cv::Mat covariance;
+			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
 		}
 		else
 		{
@@ -844,7 +847,8 @@ void Rtabmap::resetMemory()
 		_memory->init(_databasePath, true, _parameters, true);
 		if(_memory->getLastWorkingSignature())
 		{
-			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
+			cv::Mat covariance;
+			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
 		}
 		if(_bayesFilter)
 		{
@@ -1090,7 +1094,7 @@ bool Rtabmap::process(
 		}
 		else if(signature->getWeight() >= 0)
 		{
-			if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
+			if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f)
 			{
 				//============================================================
 				// Minimum displacement required to add to Memory
@@ -1105,12 +1109,15 @@ bool Rtabmap::process(
 					{
 						float x,y,z, roll,pitch,yaw;
 						links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-						bool isMoving = fabs(x) > _rgbdLinearUpdate ||
-										fabs(y) > _rgbdLinearUpdate ||
-										fabs(z) > _rgbdLinearUpdate ||
-										fabs(roll) > _rgbdAngularUpdate ||
-										fabs(pitch) > _rgbdAngularUpdate ||
-										fabs(yaw) > _rgbdAngularUpdate;
+						bool isMoving = (_rgbdLinearUpdate>0.0f && (
+											fabs(x) > _rgbdLinearUpdate ||
+											fabs(y) > _rgbdLinearUpdate ||
+											fabs(z) > _rgbdLinearUpdate))
+										||
+									    (_rgbdAngularUpdate>0.0f && (
+											fabs(roll) > _rgbdAngularUpdate ||
+											fabs(pitch) > _rgbdAngularUpdate ||
+											fabs(yaw) > _rgbdAngularUpdate));
 						if(!isMoving)
 						{
 							// This will disable global loop closure detection, only retrieval will be done.
@@ -1196,6 +1203,7 @@ bool Rtabmap::process(
 						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
 						if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
 						{
+							std::cout << info.covariance << std::endl;
 							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 						}
 					}
@@ -1895,6 +1903,8 @@ bool Rtabmap::process(
 	std::list<std::pair<int, int> > loopClosureLinksAdded;
 	int loopClosureVisualInliers = 0; // for statistics
 	int loopClosureVisualMatches = 0;
+	float loopClosureLinearVariance = 0.0f;
+	float loopClosureAngularVariance = 0.0f;
 	if(_loopClosureHypothesis.first>0)
 	{
 		//Compute transform if metric data are present
@@ -1906,6 +1916,11 @@ bool Rtabmap::process(
 			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
 			loopClosureVisualInliers = info.inliers;
 			loopClosureVisualMatches = info.matches;
+			if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
+			{
+				loopClosureLinearVariance = info.covariance.at<double>(0,0);
+				loopClosureAngularVariance = info.covariance.at<double>(3,3);
+			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
@@ -2031,19 +2046,18 @@ bool Rtabmap::process(
 									_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, info.covariance.inv()));
 									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

-									if(loopClosureVisualInliers == 0)
-									{
-										loopClosureVisualInliers = info.inliers;
-									}
-									if(loopClosureVisualMatches == 0)
-									{
-										loopClosureVisualMatches = info.matches;
-									}
-
 									if(_loopClosureHypothesis.first == 0)
 									{
 										++proximityDetectionsAddedVisually;
 										lastProximitySpaceClosureId = nearestId;
+
+										loopClosureVisualInliers = info.inliers;
+										loopClosureVisualMatches = info.matches;
+										if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
+										{
+											loopClosureLinearVariance = info.covariance.at<double>(0,0);
+											loopClosureAngularVariance = info.covariance.at<double>(3,3);
+										}
 									}
 								}
 								else
@@ -2114,7 +2128,8 @@ bool Rtabmap::process(
 							if(_proximityRawPosesUsed)
 							{
 								//optimize the path's poses locally
-								path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
+								cv::Mat covariance;
+								path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false, covariance);
 								// transform local poses in optimized graph referential
 								UASSERT(uContains(path, nearestId));
 								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
@@ -2230,8 +2245,11 @@ bool Rtabmap::process(
 	//============================================================
 	float maxLinearError = 0.0f;
 	float maxLinearErrorRatio = 0.0f;
+	float maxAngularError = 0.0f;
+	float maxAngularErrorRatio = 0.0f;
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
+	cv::Mat localizationCovariance;
 	if(_rgbdSlamMode &&
 		(_loopClosureHypothesis.first>0 ||
 	     lastProximitySpaceClosureId>0 || // can be different map of the current one
@@ -2280,6 +2298,7 @@ bool Rtabmap::process(
 			{
 				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
 			}
+			localizationCovariance = localizationLinks.begin()->second.infMatrix().inv();
 		}
 		else
 		{
@@ -2304,7 +2323,8 @@ bool Rtabmap::process(
 			}

 			std::multimap<int, Link> constraints;
-			optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);
+			cv::Mat covariance;
+			optimizeCurrentMap(signature->id(), false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);

 			// Check added loop closures have broken the graph
 			// (in case of wrong loop closures).
@@ -2323,12 +2343,13 @@ bool Rtabmap::process(
 				rejectedHypothesis = true;
 			}
 			else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
-			  _optimizationMaxLinearError > 0.0f &&
+			  _optimizationMaxError > 0.0f &&
 			  loopClosureLinksAdded.size() &&
 			  optimizationIterations > 0 &&
 			  constraints.size())
 			{
 				const Link * maxLinearLink = 0;
+				const Link * maxAngularLink = 0;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
 					// ignore links with high variance
@@ -2341,45 +2362,90 @@ bool Rtabmap::process(
 								fabs(iter->second.transform().x() - t.x()),
 								fabs(iter->second.transform().y() - t.y()),
 								fabs(iter->second.transform().z() - t.z()));
-						if(linearError > maxLinearError)
+						float opt_roll,opt__pitch,opt__yaw;
+						float link_roll,link_pitch,link_yaw;
+						t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
+						iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+						float angularError = uMax3(
+								fabs(opt_roll - link_roll),
+								fabs(opt__pitch - link_pitch),
+								fabs(opt__yaw - link_yaw));
+						float stddevLinear = sqrt(iter->second.transVariance());
+						float linearErrorRatio = linearError/stddevLinear;
+						if(linearErrorRatio > maxLinearErrorRatio)
 						{
 							maxLinearError = linearError;
+							maxLinearErrorRatio = linearErrorRatio;
 							maxLinearLink = &iter->second;
 						}
+						float stddevAngular = sqrt(iter->second.rotVariance());
+						float angularErrorRatio = angularError/stddevAngular;
+						if(angularErrorRatio > maxAngularErrorRatio)
+						{
+							maxAngularError = angularError;
+							maxAngularErrorRatio = angularErrorRatio;
+							maxAngularLink = &iter->second;
+						}
 					}
 				}
+				bool reject = false;
 				if(maxLinearLink)
 				{
-					UINFO("Max optimization error = %f m (link %d->%d, var=%f, %f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-
-					float stddev = sqrt(maxLinearLink->transVariance());
-					maxLinearErrorRatio = maxLinearError/stddev;
-					if(maxLinearErrorRatio > _optimizationMaxLinearError)
+					UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
+					if(maxLinearErrorRatio > _optimizationMaxError)
 					{
 						UWARN("Rejecting all added loop closures (%d) in this "
 							  "iteration because a wrong loop closure has been "
 							  "detected after graph optimization, resulting in "
-							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f, stddev=%f). The "
-							  "maximum error ratio parameter is %f of std deviation.",
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
 							  (int)loopClosureLinksAdded.size(),
 							  maxLinearErrorRatio,
 							  maxLinearLink->from(),
 							  maxLinearLink->to(),
 							  maxLinearLink->type(),
 							  maxLinearError,
-							  stddev,
-							  _optimizationMaxLinearError);
-						for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
-						{
-							_memory->removeLink(iter->first, iter->second);
-							UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
-						}
-						updateConstraints = false;
-						_loopClosureHypothesis.first = 0;
-						lastProximitySpaceClosureId = 0;
-						rejectedHypothesis = true;
+							  sqrt(maxLinearLink->transVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
 					}
 				}
+				if(maxAngularLink)
+				{
+					UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+					if(maxAngularErrorRatio > _optimizationMaxError)
+					{
+						UWARN("Rejecting all added loop closures (%d) in this "
+							  "iteration because a wrong loop closure has been "
+							  "detected after graph optimization, resulting in "
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
+							  (int)loopClosureLinksAdded.size(),
+							  maxAngularErrorRatio,
+							  maxAngularLink->from(),
+							  maxAngularLink->to(),
+							  maxAngularLink->type(),
+							  maxAngularError*180.0f/CV_PI,
+							  sqrt(maxAngularLink->rotVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
+					}
+				}
+
+				if(reject)
+				{
+					for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
+					{
+						_memory->removeLink(iter->first, iter->second);
+						UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
+					}
+					updateConstraints = false;
+					_loopClosureHypothesis.first = 0;
+					lastProximitySpaceClosureId = 0;
+					rejectedHypothesis = true;
+				}
 			}

 			if(updateConstraints)
@@ -2387,6 +2453,7 @@ bool Rtabmap::process(
 				UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
 				_optimizedPoses = poses;
 				_constraints = constraints;
+				localizationCovariance = covariance;
 			}
 		}

@@ -2455,6 +2522,8 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
+			statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
+			statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
@@ -2477,6 +2546,7 @@ bool Rtabmap::process(
 			}
 			statistics_.setMapCorrection(_mapCorrection);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
+			statistics_.setLocalizationCovariance(localizationCovariance);

 			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -2578,6 +2648,15 @@ bool Rtabmap::process(
 			signaturesRemoved.push_back(signature->id());
 			_memory->deleteLocation(signature->id());
 		}
+		else if(_startNewMapOnGoodSignature &&
+				signature->isBadSignature() &&
+				graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0)     // alone in the current map
+		{
+			UWARN("Ignoring location %d because a good signature (with enough features) is required before starting a new map!",
+					signature->id());
+			signaturesRemoved.push_back(signature->id());
+			_memory->deleteLocation(signature->id());
+		}
 		else if((smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
 		{
 			// Don't delete the location if a loop closure is detected
@@ -2986,21 +3065,111 @@ void Rtabmap::setWorkingDirectory(std::string path)
 	}
 }

-void Rtabmap::rejectLoopClosure(int oldId, int newId)
+void Rtabmap::rejectLastLoopClosure()
 {
-	UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
-	if(_loopClosureHypothesis.first)
+	if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
 	{
-		_loopClosureHypothesis.first = 0;
-		if(_memory)
+		std::map<int, Link> links = _memory->getLinks(getLastLocationId(), false);
+		bool linksRemoved = false;
+		for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
-			_memory->removeLink(oldId, newId);
+			if(iter->second.type() == Link::kGlobalClosure ||
+				iter->second.type() == Link::kLocalSpaceClosure ||
+				iter->second.type() == Link::kLocalTimeClosure ||
+				iter->second.type() == Link::kUserClosure)
+			{
+				_memory->removeLink(iter->second.from(), iter->second.to());
+				std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
+				if(jter!=_constraints.end())
+				{
+					_constraints.erase(jter);
+					// second time if link is also inverted
+					jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
+					if(jter!=_constraints.end())
+					{
+						_constraints.erase(jter);
+					}
+				}
+				linksRemoved = true;
+			}
 		}
-		if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
+
+		if(linksRemoved)
 		{
-			statistics_.addStatistic(rtabmap::Statistics::kLoopRejectedHypothesis(), 1.0f);
+			_loopClosureHypothesis.first = 0;
+
+			// we have to re-optimize the graph without the rejected links
+			if(_memory->isIncremental() && _optimizedPoses.size())
+			{
+				UINFO("Update graph");
+				std::map<int, Transform> poses = _optimizedPoses;
+				std::multimap<int, Link> constraints;
+				cv::Mat covariance;
+				optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints);
+
+				if(poses.empty())
+				{
+					UWARN("Graph optimization failed after removing loop closure links from last location!");
+				}
+				else
+				{
+					UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
+					_optimizedPoses = poses;
+					_constraints = constraints;
+					_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
+				}
+			}
+		}
+	}
+}
+
+void Rtabmap::deleteLastLocation()
+{
+	if(_memory && _memory->getStMem().size())
+	{
+		int lastId = *_memory->getStMem().rbegin();
+		_memory->deleteLocation(lastId);
+		// we have to re-optimize the graph without the deleted location
+		if(_memory->isIncremental() && _optimizedPoses.size())
+		{
+			UINFO("Update graph");
+			_optimizedPoses.erase(lastId);
+			std::map<int, Transform> poses = _optimizedPoses;
+			//remove all constraints with last localization id
+			for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
+			{
+				if(iter->second.from() == lastId || iter->second.to() == lastId)
+				{
+					_constraints.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+
+			if(poses.empty())
+			{
+				_mapCorrection.setIdentity();
+			}
+			else
+			{
+				std::multimap<int, Link> constraints;
+				cv::Mat covariance;
+				optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints);
+
+				if(poses.empty())
+				{
+					UWARN("Graph optimization failed after deleting the last location!");
+				}
+				else
+				{
+					_optimizedPoses = poses;
+					_constraints = constraints;
+					_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
+				}
+			}
 		}
-		statistics_.setLoopClosureId(0);
 	}
 }

@@ -3210,6 +3379,7 @@ void Rtabmap::optimizeCurrentMap(
 		int id,
 		bool lookInDatabase,
 		std::map<int, Transform> & optimizedPoses,
+		cv::Mat & covariance,
 		std::multimap<int, Link> * constraints,
 		double * error,
 		int * iterationsDone) const
@@ -3226,7 +3396,7 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());

-		std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, constraints, error, iterationsDone);
+		std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, covariance, constraints, error, iterationsDone);
 		UINFO("optimize time %f s", timer.ticks());

 		if(poses.size())
@@ -3256,6 +3426,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 		const std::set<int> & ids,
 		const std::map<int, Transform> & guessPoses,
 		bool lookInDatabase,
+		cv::Mat & covariance,
 		std::multimap<int, Link> * constraints,
 		double * error,
 		int * iterationsDone) const
@@ -3337,7 +3508,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	}
 	else
 	{
-		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, 0, error, iterationsDone);
+		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);

 		if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
 		{
@@ -3511,7 +3682,8 @@ void Rtabmap::get3DMap(
 			if(optimized)
 			{
 				poses = _optimizedPoses; // guess
-				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+				cv::Mat covariance;
+				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
 			}
 			else
 			{
@@ -3598,7 +3770,8 @@ void Rtabmap::getGraph(
 			if(optimized)
 			{
 				poses = _optimizedPoses; // guess
-				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+				cv::Mat covariance;
+				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
 			}
 			else
 			{
@@ -3752,7 +3925,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 					if(!t.isNull())
 					{
 						bool updateConstraints = true;
-						if(_optimizationMaxLinearError > 0.0f)
+						if(_optimizationMaxError > 0.0f)
 						{
 							//optimize the graph to see if the new constraint is globally valid

@@ -3826,7 +3999,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 									UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
 								}

-								if(maxLinearError > _optimizationMaxLinearError)
+								if(maxLinearError > _optimizationMaxError)
 								{
 									msg = uFormat("Rejecting edge %d->%d because "
 											  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
@@ -3840,7 +4013,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 											  maxAngularLink?maxAngularLink->from():0,
 											  maxAngularLink?maxAngularLink->to():0,
 											  Parameters::kRGBDOptimizeMaxError().c_str(),
-											  _optimizationMaxLinearError);
+											  _optimizationMaxError);
 								}
 							}
 							else
@@ -4098,8 +4271,13 @@ bool Rtabmap::computePath(int targetNode, bool global)
 	return false;
 }

-bool Rtabmap::computePath(const Transform & targetPose)
+bool Rtabmap::computePath(const Transform & targetPose, float tolerance)
 {
+	if(tolerance < 0.0f)
+	{
+		tolerance = _localRadius;
+	}
+
 	UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());

 	this->clearPath(0);
@@ -4152,7 +4330,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
 	}

 	int nearestId;
-	if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < _localRadius)
+	if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < tolerance)
 	{
 		// target can be reached from the current node
 		nearestId = currentNode;
@@ -4164,10 +4342,10 @@ bool Rtabmap::computePath(const Transform & targetPose)
 	UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
 	if(nearestId > 0)
 	{
-		if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius)
+		if(tolerance != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > tolerance)
 		{
 			UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
-					_localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId);
+					tolerance, targetPose.getDistance(nodes.at(nearestId)), nearestId);
 		}
 		else
 		{
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>

 namespace rtabmap {

@@ -58,8 +58,15 @@ Transform::Transform(const cv::Mat & transformationMatrix)
 {
 	UASSERT(transformationMatrix.cols == 4 &&
 			transformationMatrix.rows == 3 &&
-			transformationMatrix.type() == CV_32FC1);
-	data_ = transformationMatrix;
+			(transformationMatrix.type() == CV_32FC1 || transformationMatrix.type() == CV_64FC1));
+	if(transformationMatrix.type() == CV_32FC1)
+	{
+		data_ = transformationMatrix;
+	}
+	else
+	{
+		transformationMatrix.convertTo(data_, CV_32F);
+	}
 }

 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
@@ -154,7 +154,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 					driver->load(this, false);
 					for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
 					{
-						iter->second->setSaved(false);
+						iter->second->setSaved(true);
 					}
 					_incrementalDictionary = _visualWords.size()==0;
 					driver->closeConnection(false);
@@ -222,6 +222,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 								}

 								VisualWord * vw = new VisualWord(id, descriptor, 0);
+								vw->setSaved(true);
 								_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
 								_notIndexedWords.insert(_notIndexedWords.end(), id);
 								_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord*>(id, vw));
@@ -586,6 +587,12 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
 	_unusedWords.clear();
 	_flannIndex->release();
 	useDistanceL1_ = false;
+
+	if(!_incrementalDictionary)
+	{
+		// reload the fixed dictionary
+		this->setFixedDictionary(_dictionaryPath);
+	}
 }

 int VWDictionary::getNextId()
@@ -595,21 +602,18 @@ int VWDictionary::getNextId()

 void VWDictionary::addWordRef(int wordId, int signatureId)
 {
-	if(signatureId > 0)
+	VisualWord * vw = 0;
+	vw = uValue(_visualWords, wordId, vw);
+	if(vw)
 	{
-		VisualWord * vw = 0;
-		vw = uValue(_visualWords, wordId, vw);
-		if(vw)
-		{
-			vw->addRef(signatureId);
-			_totalActiveReferences += 1;
+		vw->addRef(signatureId);
+		_totalActiveReferences += 1;

-			_unusedWords.erase(vw->id());
-		}
-		else
-		{
-			UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
-		}
+		_unusedWords.erase(vw->id());
+	}
+	else
+	{
+		UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
 	}
 }

@@ -630,8 +634,6 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
 std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 							   int signatureId)
 {
-	UASSERT(signatureId > 0);
-
 	UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows);
 	UTimer timer;
 	std::list<int> wordIds;
@@ -802,8 +804,17 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			for(int j=0; j<dists.cols; ++j)
 			{
 				float d = dists.at<float>(i,j);
-				int id = uValue(_mapIndexId, (int)results.at<size_t>(i,j));
-				if(d >= 0.0f && id > 0)
+				int index;
+				if (sizeof(size_t) == 8)
+				{
+					index = *((size_t*)&results.at<double>(i, j));
+				}
+				else
+				{
+					index = *((size_t*)&results.at<int>(i, j));
+				}
+				int id = uValue(_mapIndexId, index);
+				if(d >= 0.0f && id != 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
 				}
@@ -819,7 +830,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			{
 				float d = matches.at(i).at(j).distance;
 				int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
-				if(d >= 0.0f && id > 0)
+				if(d >= 0.0f && id != 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
 				}
@@ -842,7 +853,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			{
 				float d = matchesNewWords.at(0).at(j).distance;
 				int id = newWordsId[matchesNewWords.at(0).at(j).trainIdx];
-				if(d >= 0.0f && id > 0)
+				if(d >= 0.0f && id != 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
 				}
@@ -1150,8 +1161,18 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				for(int j=0; j<dists.cols; ++j)
 				{
 					float d = dists.at<float>(i,j);
-					int id = uValue(_mapIndexId, (int)results.at<size_t>(i,j));
-					if(d >= 0.0f && id > 0)
+					int index;
+
+					if (sizeof(size_t) == 8)
+					{
+						index = *((size_t*)&results.at<double>(i, j));
+					}
+					else
+					{
+						index = *((size_t*)&results.at<int>(i, j));
+					}
+					int id = uValue(_mapIndexId, index);
+					if(d >= 0.0f && id != 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));
 					}
@@ -1163,7 +1184,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					float d = matches.at(i).at(j).distance;
 					int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
-					if(d >= 0.0f && id > 0)
+					if(d >= 0.0f && id != 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));
 					}
@@ -1177,7 +1198,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					float d = matchesNotIndexed.at(i).at(j).distance;
 					int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
-					if(d >= 0.0f && id > 0)
+					if(d >= 0.0f && id != 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));
 					}
@@ -198,7 +198,7 @@ namespace clams

 	cv::Mat roiA(targetImage, cv::Rect( 0, 0, overview.cols/2, overview.rows ));
 	cv::Mat(overview, cv::Rect( 0, 0, overview.cols/2, overview.rows )).copyTo(roiA);
-	cv::Mat roiB( targetImage, cvRect( overview.cols/2, 0, pub.cols, pub.rows ) );
+	cv::Mat roiB( targetImage, cv::Rect( overview.cols/2, 0, pub.cols, pub.rows ) );
 	pub.copyTo(roiB);

    return targetImage;
@@ -46,6 +46,9 @@

 #include <opencv2/core/core.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/calib3d/calib3d_c.h>
+#endif

 namespace cv3 {

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
 #include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/types_c.h>
 #include <map>
 #include <Eigen/Core>

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/ply_io.h>
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>

 namespace rtabmap
 {
@@ -294,6 +294,20 @@ std::map<int, cv::Point3f> generateWords3DMono(
 					if(useCameraTransformGuess)
 					{
 						Transform t = (cameraModel.localTransform().inverse()*cameraTransform*cameraModel.localTransform()).inverse();
+
+						if(ULogger::level() == ULogger::kDebug)
+						{
+							UDEBUG("Guess = %s", t.prettyPrint().c_str());
+							UDEBUG("Epipolar = %s", Transform(P).prettyPrint().c_str());
+							Transform PT = Transform(P);
+							float scale = t.getNorm()/PT.getNorm();
+							UDEBUG("Scale= %f", scale);
+							PT.x()*=scale;
+							PT.y()*=scale;
+							PT.z()*=scale;
+							UDEBUG("Epipolar scaled= %s", PT.prettyPrint().c_str());
+						}
+
 						P = (cv::Mat_<double>(3,4) <<
 								(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
 								(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
@@ -574,6 +574,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float scanMaxRange)
 {
 	UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
+
+	// local scans contain end points of each ray in map frame (pose+localTransform)
 	std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;

 	pcl::PointCloud<pcl::PointXYZ> minMax;
@@ -629,12 +631,14 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
 		float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;

-		//UWARN("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
-		//		xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
+		UDEBUG("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
+				xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);

 		UTimer timer;

 		map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
+		UDEBUG("map size = %dx%d", map.cols, map.rows);
+
 		int j=0;
 		float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
 		for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
@@ -647,16 +651,15 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 				viewpoint = kter->second;
 			}
 			cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
-			cv::Point2f startf(pose.x()+viewpoint.x, pose.y()+viewpoint.y);

 			// Set obstacles first
 			for(int i=0; i<iter->second.first.cols; ++i)
 			{
 				const float * ptr = iter->second.first.ptr<float>(0, i);
-				bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]+cellSize, ptr[1]+cellSize) > scanMaxRangeSqr;
+				bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
 				if(!ignore)
 				{
-					cv::Point2i end((ptr[0]+startf.x-xMin)/cellSize, (ptr[1]+startf.y-yMin)/cellSize);
+					cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
 					if(end!=start)
 					{
 						map.at<char>(end.y, end.x) = 100; // obstacle
@@ -669,17 +672,20 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			{
 				const float * ptr = iter->second.first.ptr<float>(0, i);

-				cv::Vec2f v(ptr[0], ptr[1]);
+				cv::Vec2f pt(ptr[0], ptr[1]);
 				if(scanMaxRange>cellSize)
 				{
+					cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
 					float n = cv::norm(v);
 					if(n > scanMaxRange+cellSize)
 					{
 						v = (v/n) * scanMaxRange;
+						pt[0] = pose.x()+viewpoint.x + v[0];
+						pt[1] = pose.y()+viewpoint.y + v[1];
 					}
 				}

-				cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
+				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -693,17 +699,20 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			{
 				const float * ptr = iter->second.second.ptr<float>(0, i);

-				cv::Vec2f v(ptr[0], ptr[1]);
+				cv::Vec2f pt(ptr[0], ptr[1]);
 				if(scanMaxRange>cellSize)
 				{
+					cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
 					float n = cv::norm(v);
 					if(n > scanMaxRange+cellSize)
 					{
 						v = (v/n) * scanMaxRange;
+						pt[0] = pose.x()+viewpoint.x + v[0];
+						pt[1] = pose.y()+viewpoint.y + v[1];
 					}
 				}

-				cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
+				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -748,10 +757,10 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 						cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
 						origin.at<float>(0) = pose.x()+viewpoint.x;
 						origin.at<float>(1) = pose.y()+viewpoint.y;
-						endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0]+origin.at<float>(0);
-						endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1]+origin.at<float>(1);
-						endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0]+origin.at<float>(0);
-						endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1]+origin.at<float>(1);
+						endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
+						endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
+						endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0];
+						endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1];
 						//UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
 						//UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
 						//UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));
@@ -37,6 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/UTimer.h"
+#include <opencv2/core/core_c.h>
+#include <opencv2/imgproc/types_c.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/surface/gp3.h>
 #include <pcl/features/normal_3d_omp.h>
@@ -5,6 +5,7 @@ IF(TARGET rtabmap_gui)
   ADD_SUBDIRECTORY( RGBDMapping )
   ADD_SUBDIRECTORY( WifiMapping )
   ADD_SUBDIRECTORY( NoEventsExample )
+   ADD_SUBDIRECTORY( Export )
 ELSE()
   MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
 ENDIF()
@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 2.8)
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(internal)
+	# inside rtabmap project (see below for external build)
+	SET(RTABMap_INCLUDE_DIRS 
+	    ${PROJECT_SOURCE_DIR}/utilite/include
+		${PROJECT_SOURCE_DIR}/corelib/include
+	)
+	SET(RTABMap_LIBRARIES 
+	    rtabmap_core
+		rtabmap_utilite
+	)  
+else()
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+	FIND_PACKAGE(OpenCV REQUIRED)
+	FIND_PACKAGE(PCL 1.7 REQUIRED)
+	
+endif()
+
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(export main.cpp)
+  
+TARGET_LINK_LIBRARIES(export ${LIBRARIES})
+
+if(internal)
+	SET_TARGET_PROPERTIES( export 
+	  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-export)
+endif(internal)
+
+
+
+
@@ -0,0 +1,295 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <pcl/filters/filter.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/common/common.h>
+#include <pcl/surface/poisson.h>
+#include <stdio.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-exportCloud [options] database.db\n"
+			"Options:\n"
+			"    --mesh          Create a mesh.\n"
+			"    --texture       Create a mesh with texture.\n"
+			"\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	bool mesh = false;
+	bool texture = false;
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(std::strcmp(argv[i], "--mesh") == 0)
+		{
+			mesh = true;
+		}
+		else if(std::strcmp(argv[i], "--texture") == 0)
+		{
+			texture = true;
+		}
+	}
+
+	std::string dbPath = argv[argc-1];
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		parameters = driver->getLastParameters();
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	// Get the global optimized map
+	Rtabmap rtabmap;
+	rtabmap.init(parameters, dbPath);
+
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	rtabmap.get3DMap(nodes, optimizedPoses, links, true, true);
+
+	// Construct the cloud
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	std::map<int, rtabmap::Transform> cameraPoses;
+	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
+	std::map<int, cv::Mat> cameraDepths;
+	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+	{
+		Signature node = nodes.find(iter->first)->second;
+
+		// uncompress data
+		node.sensorData().uncompressData();
+		std::vector<CameraModel> models = node.sensorData().cameraModels();
+		cv::Mat depth = node.sensorData().depthRaw();
+
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+				node.sensorData(),
+				4,           // image decimation before creating the clouds
+				4.0f,        // maximum depth of the cloud
+				0.0f,
+				indices.get());
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+		transformedCloud = rtabmap::util3d::voxelize(cloud, indices, 0.01);
+		transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
+
+		Eigen::Vector3f viewpoint( iter->second.x(),  iter->second.y(),  iter->second.z());
+		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, 10, 0.0f, viewpoint);
+
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals);
+
+		if(mergedClouds->size() == 0)
+		{
+			*mergedClouds = *cloudWithNormals;
+		}
+		else
+		{
+			*mergedClouds += *cloudWithNormals;
+		}
+
+		cameraPoses.insert(std::make_pair(iter->first, iter->second));
+		if(!models.empty())
+		{
+			cameraModels.insert(std::make_pair(iter->first, models));
+		}
+		if(!depth.empty())
+		{
+			cameraDepths.insert(std::make_pair(iter->first, depth));
+		}
+	}
+	if(mergedClouds->size())
+	{
+		if(!(mesh || texture))
+		{
+			printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)mergedClouds->size());
+			mergedClouds = util3d::voxelize(mergedClouds, 0.01f);
+
+			printf("Saving cloud.ply... (%d points)\n", (int)mergedClouds->size());
+			pcl::io::savePLYFile("cloud.ply", *mergedClouds);
+			printf("Saving cloud.ply... done!\n");
+		}
+		else
+		{
+			Eigen::Vector4f min,max;
+			pcl::getMinMax3D(*mergedClouds, min, max);
+			float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
+			int optimizedDepth = 12;
+			for(int i=6; i<12; ++i)
+			{
+				if(mapLength/float(1<<i) < 0.03f)
+				{
+					optimizedDepth = i;
+					break;
+				}
+			}
+
+			// Mesh reconstruction
+			printf("Mesh reconstruction...\n");
+			pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
+			pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
+			poisson.setDepth(optimizedDepth);
+			poisson.setInputCloud(mergedClouds);
+			UTimer timer;
+			poisson.reconstruct(*mesh);
+			printf("Mesh reconstruction... done! %fs (%d polygons)\n", timer.ticks(), (int)mesh->polygons.size());
+
+			if(mesh->polygons.size())
+			{
+				rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
+						mesh,
+						0.0f,
+						0,
+						mergedClouds,
+						0.05,
+						!texture);
+
+				if(!texture)
+				{
+					printf("Saving mesh.ply...\n");
+					pcl::io::savePLYFile("mesh.ply", *mesh);
+					printf("Saving mesh.ply... done!\n");
+				}
+				else
+				{
+					printf("Texturing... cameraPoses=%d, cameraDepths=%d\n", (int)cameraPoses.size(), (int)cameraDepths.size());
+					std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
+					pcl::TextureMeshPtr textureMesh = rtabmap::util3d::createTextureMesh(
+							mesh,
+							cameraPoses,
+							cameraModels,
+							cameraDepths,
+							3,
+							0.0f,
+							0.0f,
+							50,
+							std::vector<float>(),
+							0,
+							&vertexToPixels);
+					printf("Texturing... done! %fs\n", timer.ticks());
+
+					// Remove occluded polygons (polygons with no texture)
+					if(textureMesh->tex_coordinates.size())
+					{
+						printf("Cleanup mesh...\n");
+						rtabmap::util3d::cleanTextureMesh(*textureMesh, 0);
+						printf("Cleanup mesh... done! %fs\n", timer.ticks());
+					}
+
+					if(textureMesh->tex_materials.size())
+					{
+						printf("Merging %d textures...\n", (int)textureMesh->tex_materials.size());
+						cv::Mat textures = rtabmap::util3d::mergeTextures(
+								*textureMesh,
+								std::map<int, cv::Mat>(),
+								std::map<int, std::vector<rtabmap::CameraModel> >(),
+								rtabmap.getMemory(),
+								0,
+								4096,
+								1,
+								vertexToPixels,
+								true, 10.0f, true ,true, 0, 0, 0, false);
+
+
+						// TextureMesh OBJ
+						bool success = false;
+						UASSERT(!textures.empty());
+						UASSERT(textureMesh->tex_materials.size() == 1);
+
+						std::string filePath = "mesh.jpg";
+						textureMesh->tex_materials[0].tex_file = filePath;
+						printf("Saving texture to %s.\n", filePath.c_str());
+						success = cv::imwrite(filePath, textures);
+						if(!success)
+						{
+							UERROR("Failed saving %s!", filePath.c_str());
+						}
+						else
+						{
+							printf("Saved %s.\n", filePath.c_str());
+						}
+
+						if(success)
+						{
+
+							std::string filePath = "mesh.obj";
+							printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, filePath.c_str());
+							success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
+
+							if(success)
+							{
+								printf("Saved obj to %s!\n", filePath.c_str());
+							}
+							else
+							{
+								UERROR("Failed saving obj to %s!", filePath.c_str());
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+	else
+	{
+		printf("Export failed! The cloud is empty.\n");
+	}
+
+	return 0;
+}
@@ -224,7 +224,7 @@ public:
 		cloudViewer_->update();
 	}

-protected slots:
+protected Q_SLOTS:
 	void pauseDetection()
 	{
 		paused_ = !paused_;
@@ -47,7 +47,7 @@ void showUsage()
 			"\n"
 			"Example:\n"
 			"     (with images from \"https://github.com/introlab/rtabmap/wiki/Stereo-mapping#process-a-directory-of-stereo-images\") \n"
-			"     $ rtabmap-noEventsExample 20 2 10 stereo_20hz stereo_20Hz stereo_20hz/left stereo_20hz/right\n"
+			"     $ rtabmap-noEventsExample 20 2 10 stereo_20Hz stereo_20Hz stereo_20Hz/left stereo_20Hz/right\n"
 			"       Camera rate = 20 Hz\n"
 			"       Odometry update rate = 10 Hz\n"
 			"       Map update rate = 1 Hz\n");
@@ -86,7 +86,7 @@ public:
 		this->unregisterFromEventsManager();
 	}

-protected slots:
+protected Q_SLOTS:
 	virtual void pauseDetection()
 	{
 		UWARN("");
@@ -46,7 +46,7 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-rgbd_mapping driver\n"
-			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense\n\n");
+			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2\n\n");
 	exit(1);
 }

@@ -64,9 +64,9 @@ int main(int argc, char * argv[])
 	else
 	{
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 7)
+		if(driver < 0 || driver > 8)
 		{
-			UERROR("driver should be between 0 and 7.");
+			UERROR("driver should be between 0 and 8.");
 			showUsage();
 		}
 	}
@@ -141,6 +141,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
 	}
+	else if (driver == 8)
+	{
+		if (!CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 support...");
+			exit(-1);
+		}
+		camera = new CameraRealSense2("", 0, opticalRotation);
+	}
 	else
 	{
 		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
@@ -85,7 +85,7 @@ public:
 		this->unregisterFromEventsManager();
 	}

-protected slots:
+protected Q_SLOTS:
 	virtual void pauseDetection()
 	{
 		UWARN("");
@@ -65,7 +65,7 @@ public:
 		this->unregisterFromEventsManager();
 	}

-protected slots:
+protected Q_SLOTS:
 	virtual void processStatistics(const rtabmap::Statistics & stats)
 	{
 		processingStatistics_ = true;
@@ -70,7 +70,7 @@ public:

 	StereoCameraModel stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification) const;

-public slots:
+public Q_SLOTS:
 	void setBoardWidth(int width);
 	void setBoardHeight(int height);
 	void setSquareSize(double size);
@@ -81,7 +81,7 @@ public slots:
 	void restart();
 	bool save();

-private slots:
+private Q_SLOTS:
 	void unlock();

 protected:
@@ -53,7 +53,7 @@ public:
 		const ParametersMap & parameters = ParametersMap());
 	virtual ~CameraViewer();

-public slots:
+public Q_SLOTS:
 	void showImage(const rtabmap::SensorData & data);
 protected:
 	virtual bool handleEvent(UEvent * event);
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/core/Transform.h"
 #include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/gui/CloudViewerInteractorStyle.h"

 #include <QVTKWidget.h>
 #include <pcl/pcl_base.h>
@@ -48,8 +49,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/opencv.hpp>
 #include <set>

-#include <pcl/visualization/mouse_event.h>
-#include <pcl/visualization/point_picking_event.h>
 #include <pcl/PCLPointCloud2.h>

 namespace pcl {
@@ -72,7 +71,7 @@ class RTABMAPGUI_EXP CloudViewer : public QVTKWidget
 	Q_OBJECT

 public:
-	CloudViewer(QWidget * parent = 0);
+	CloudViewer(QWidget * parent = 0, CloudViewerInteractorStyle* style = CloudViewerInteractorStyle::New());
 	virtual ~CloudViewer();

 	void saveSettings(QSettings & settings, const QString & group = "") const;
@@ -235,6 +234,26 @@ public:
 	void removeAllCubes();
 	const std::set<std::string> & getAddedCubes() const {return _cubes;}

+	void addOrUpdateQuad(
+			const std::string & id,
+			const Transform & pose,
+			float width,
+			float height,
+			const QColor & color,
+			bool foreground = false);
+	void addOrUpdateQuad(
+			const std::string & id,
+			const Transform & pose,
+			float widthLeft,
+			float widthRight,
+			float heightBottom,
+			float heightTop,
+			const QColor & color,
+			bool foreground = false);
+	void removeQuad(const std::string & id);
+	void removeAllQuads();
+	const std::set<std::string> & getAddedQuads() const {return _quads;}
+
 	void addOrUpdateFrustum(
 			const std::string & id,
 			const Transform & transform,
@@ -289,6 +308,9 @@ public:
 	const QColor & getDefaultBackgroundColor() const;
 	const QColor & getBackgroundColor() const;
 	Transform getTargetPose() const;
+	std::string getIdByActor(vtkProp * actor) const;
+	QColor getColor(const std::string & id);
+	void setColor(const std::string & id, const QColor & color);

 	void setBackfaceCulling(bool enabled, bool frontfaceCulling);
 	void setPolygonPicking(bool enabled);
@@ -296,6 +318,7 @@ public:
 	void setLighting(bool on);
 	void setShading(bool on);
 	void setEdgeVisibility(bool visible);
+	void setInteractorLayer(int layer);
 	double getRenderingRate() const;

 	void getCameraPosition(
@@ -306,6 +329,7 @@ public:
 	bool isCameraTargetFollow() const;
 	bool isCameraFree() const;
 	bool isCameraLockZ() const;
+	bool isCameraOrtho() const;
 	bool isGridShown() const;
 	unsigned int getGridCellCount() const;
 	float getGridCellSize() const;
@@ -318,6 +342,7 @@ public:
 	void setCameraTargetFollow(bool enabled = true);
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
+	void setCameraOrtho(bool enabled = true);
 	void setGridShown(bool shown);
 	void setNormalsShown(bool shown);
 	void setGridCellCount(unsigned int count);
@@ -330,7 +355,7 @@ public:
 	void buildPickingLocator(bool enable);
 	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}

-public slots:
+public Q_SLOTS:
 	void setDefaultBackgroundColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
 	void setCloudVisibility(const std::string & id, bool isVisible);
@@ -339,7 +364,7 @@ public slots:
 	void setCloudPointSize(const std::string & id, int size);
 	virtual void clear();

-signals:
+Q_SIGNALS:
 	void configChanged();

 protected:
@@ -364,6 +389,7 @@ private:
    QAction * _aFollowCamera;
    QAction * _aResetCamera;
    QAction * _aLockViewZ;
+    QAction * _aCameraOrtho;
    QAction * _aShowTrajectory;
    QAction * _aSetTrajectorySize;
    QAction * _aClearTrajectory;
@@ -390,6 +416,7 @@ private:
    std::set<std::string> _lines;
    std::set<std::string> _spheres;
    std::set<std::string> _cubes;
+    std::set<std::string> _quads;
    QMap<std::string, Transform> _frustums;
    pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
    unsigned int _maxTrajectorySize;
@@ -0,0 +1,41 @@
+/*
+ * CloudViewerCellPicker.h
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#ifndef GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_
+#define GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <vtkCellPicker.h>
+
+namespace rtabmap {
+
+class RTABMAPGUI_EXP CloudViewerCellPicker : public vtkCellPicker {
+public:
+public:
+    static CloudViewerCellPicker *New ();
+    vtkTypeMacro(CloudViewerCellPicker, vtkCellPicker);
+	CloudViewerCellPicker();
+	virtual ~CloudViewerCellPicker();
+
+protected:
+	// overrided to ignore back faces
+    virtual double IntersectActorWithLine(const double p1[3],
+    		const double p2[3],
+			double t1, double t2,
+			double tol,
+			vtkProp3D *prop,
+			vtkMapper *mapper);
+
+private:
+	vtkGenericCell * cell_; //used to accelerate picking
+	vtkIdList * pointIds_; // used to accelerate picking
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_ */
@@ -0,0 +1,52 @@
+/*
+ * CloudViewerInteractorStyl.h
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#ifndef GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_
+#define GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <pcl/visualization/mouse_event.h>
+#include <pcl/visualization/point_picking_event.h>
+#include <pcl/visualization/interactor_style.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+class CloudViewer;
+
+class RTABMAPGUI_EXP CloudViewerInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
+{
+public:
+    static CloudViewerInteractorStyle *New ();
+    vtkTypeMacro(CloudViewerInteractorStyle, pcl::visualization::PCLVisualizerInteractorStyle);
+
+public:
+	CloudViewerInteractorStyle();
+	virtual void Rotate();
+	void setOrthoMode(bool enabled);
+protected:
+	virtual void OnMouseMove();
+	virtual void OnLeftButtonDown();
+
+protected:
+	friend class CloudViewer;
+	void setCloudViewer(CloudViewer * cloudViewer) {viewer_ = cloudViewer;}
+	CloudViewer * viewer_;
+
+private:
+	unsigned int NumberOfClicks;
+	int PreviousPosition[2];
+	int ResetPixelDistance;
+	float PreviousMeasure[3];
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
+	bool orthoMode_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_ */
@@ -50,13 +50,13 @@ public:
 	ConsoleWidget(QWidget * parent = 0);
 	virtual ~ConsoleWidget();

-public slots:
+public Q_SLOTS:
 	void appendMsg(const QString & msg, int level = 1);

-signals:
+Q_SIGNALS:
 	void msgReceived(const QString &, int);

-private slots:
+private Q_SLOTS:
 	void flushConsole();
 	void updateTextEditBufferSize();

@@ -52,7 +52,7 @@ public:
 	void setCameraInfoDir(const QString & folder) {savingFolder_ = folder;}
 	const QString & cameraName() const {return cameraName_;}

-private slots:
+private Q_SLOTS:
 	void updateStereoView();
 	void updateSaveStatus();
 	void saveCalibration();
@@ -56,7 +56,7 @@ public:

 	const QString & path() const {return path_;}

-public slots:
+public Q_SLOTS:
 	void addData(const rtabmap::SensorData & data, const Transform & pose = Transform(), const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1));
 	void showImage(const cv::Mat & image, const cv::Mat & depth);
 protected:
@@ -80,7 +80,7 @@ protected:
 	virtual void closeEvent(QCloseEvent* event);
 	virtual bool eventFilter(QObject *obj, QEvent *event);

-private slots:
+private Q_SLOTS:
 	void writeSettings();
 	void restoreDefaultSettings();
 	void configModified();
@@ -62,13 +62,13 @@ public:
 			const QString & workingDirectory,
 			const ParametersMap & parameters);

-signals:
+Q_SIGNALS:
 	void configChanged();

-public slots:
+public Q_SLOTS:
 	void restoreDefaults();

-private slots:
+private Q_SLOTS:
 	void saveModel();
 	void cancel();

@@ -55,7 +55,7 @@ public:
    void setPenWidth(int newWidth);
    int penWidth() const { return myPenWidth_; }

-public slots:
+public Q_SLOTS:
    void resetChanges();

 protected:
@@ -57,10 +57,10 @@ public:
 	double maxAngularSpeed() const;
 	double laplacianThreshold() const;

-signals:
+Q_SIGNALS:
 	void configChanged();

-private slots:
+private Q_SLOTS:
 	void getPath();
 	void restoreDefaults();

@@ -116,13 +116,13 @@ public:

 	static bool removeDirRecursively(const QString & dirName);

-signals:
+Q_SIGNALS:
 	void configChanged();

-public slots:
+public Q_SLOTS:
 	void restoreDefaults();

-private slots:
+private Q_SLOTS:
 	void loadSettings();
 	void saveSettings();
 	void updateReconstructionFlavor();
@@ -59,10 +59,10 @@ public:
 	bool isOdomExported() const;
 	bool isUserDataExported() const;

-signals:
+Q_SIGNALS:
 	void configChanged();

-private slots:
+private Q_SLOTS:
 	void getPath();
 	void restoreDefaults();

@@ -65,7 +65,7 @@ public:
 			const std::map<int, GPS> & gpsValues);
 	void updateReferentialPosition(const Transform & t);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
-	void updatePosterior(const std::map<int, float> & posterior);
+	void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f);
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());
@@ -145,11 +145,11 @@ public:
 	void setGPSGraphVisible(bool visible);
 	void setOrientationENU(bool enabled);

-signals:
+Q_SIGNALS:
 	void configChanged();
 	void mapShownRequested();

-public slots:
+public Q_SLOTS:
 	void restoreDefaults();

 protected:
@@ -64,6 +64,7 @@ public:
 	int getFeaturesSize() const {return _featuresSize;}
 	bool isGraphicsViewMode() const;
 	bool isGraphicsViewScaled() const;
+	bool isGraphicsViewScaledToHeight() const;
 	const QColor & getDefaultBackgroundColor() const;
 	const QColor & getBackgroundColor() const;

@@ -75,6 +76,7 @@ public:
 	void setLinesShown(bool shown);
 	void setGraphicsViewMode(bool on);
 	void setGraphicsViewScaled(bool scaled);
+	void setGraphicsViewScaledToHeight(bool scaled);
 	void setDefaultBackgroundColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);

@@ -83,6 +85,7 @@ public:
 	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
 	void addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text = QString());
 	void setImage(const QImage & image);
+	void setImageDepth(const cv::Mat & imageDepth);
 	void setImageDepth(const QImage & image);
 	void setFeatureColor(int id, QColor color);
 	void setFeaturesColor(QColor color);
@@ -98,7 +101,7 @@ public:

 	virtual QSize sizeHint() const;

-signals:
+Q_SIGNALS:
 	void configChanged();

 protected:
@@ -106,7 +109,7 @@ protected:
 	virtual void resizeEvent(QResizeEvent* event);
 	virtual void contextMenuEvent(QContextMenuEvent * e);

-private slots:
+private Q_SLOTS:
 	void sceneRectChanged(const QRectF &rect);

 private:
@@ -129,6 +132,13 @@ private:
 	QAction * _setFeaturesSize;
 	QAction * _graphicsViewMode;
 	QAction * _graphicsViewScaled;
+	QAction * _graphicsViewScaledToHeight;
+	QAction * _graphicsViewNoScaling;
+	QAction * _colorMapWhiteToBlack;
+	QAction * _colorMapBlackToWhite;
+	QAction * _colorMapRedToBlue;
+	QAction * _colorMapBlueToRed;
+	QMenu * _scaleMenu;

 	QGraphicsView * _graphicsView;
 	QMultiMap<int, rtabmap::KeypointItem *> _features;
@@ -137,6 +147,7 @@ private:
 	QGraphicsPixmapItem * _imageDepthItem;
 	QPixmap _image;
 	QPixmap _imageDepth;
+	cv::Mat _imageDepthCv;
 };

 }
@@ -53,7 +53,7 @@ public:
 	const Signature & sA() const {return sA_;}
 	const Signature & sB() const {return sB_;}

-public slots:
+public Q_SLOTS:
 	void setDecimation(int decimation) {decimation_ = decimation;}
 	void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;}
 	void setMinDepth(int minDepth) {minDepth_ = minDepth;}
@@ -109,10 +109,10 @@ public:

 	bool isDatabaseUpdated() const { return _databaseUpdated; }

-public slots:
+public Q_SLOTS:
 	virtual void processStats(const rtabmap::Statistics & stat);
 	void updateCacheFromDatabase(const QString & path);
-	void openDatabase(const QString & path);
+	void openDatabase(const QString & path, const rtabmap::ParametersMap & overridedParameters = rtabmap::ParametersMap());
 	void updateParameters(const rtabmap::ParametersMap & parameters);

 protected:
@@ -124,7 +124,7 @@ protected:
 	virtual void keyPressEvent(QKeyEvent *event);
 	virtual bool eventFilter(QObject *obj, QEvent *event);

-protected slots:
+protected Q_SLOTS:
 	virtual void changeState(MainWindow::State state);
 	virtual void newDatabase();
 	virtual void openDatabase();
@@ -144,7 +144,7 @@ protected slots:
 	virtual void triggerNewMap();
 	virtual void deleteMemory();

-protected slots:
+protected Q_SLOTS:
 	void beep();
 	void cancelProgress();
 	void configGUIModified();
@@ -172,6 +172,7 @@ protected slots:
 	void selectFreenect2();
 	void selectK4W2();
 	void selectRealSense();
+	void selectRealSense2();
 	void selectStereoDC1394();
 	void selectStereoFlyCapture2();
 	void selectStereoZed();
@@ -220,7 +221,7 @@ protected slots:
 	void updateNodeVisibility(int, bool);
 	void updateGraphView();

-signals:
+Q_SIGNALS:
 	void statsReceived(const rtabmap::Statistics &);
 	void statsProcessed();
 	void cameraInfoReceived(const rtabmap::CameraInfo &);
@@ -326,6 +327,7 @@ private:
 	bool _savedMaximized;
 	QStringList _waypoints;
 	int _waypointsIndex;
+	std::vector<CameraModel> _rectCameraModels;

 	QMap<int, Signature> _cachedSignatures;
 	long _cachedMemoryUsage;
@@ -338,6 +340,8 @@ private:
 	long _createdCloudsMemoryUsage;
 	std::set<int> _cachedEmptyClouds;
 	std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction
+	std::map<int, float> _cachedWordsCount;
+	std::map<int, float> _cachedLocalizationsCount;

 	std::map<int, LaserScan> _createdScans;

--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	db3b901063	Update package.xml Bump package version	2018-09-29 11:58:58 -04:00
matlabbe	8b055752aa	RegistrationIcp: Fixed invalid variance set when structural complexity is low and input doesn't have normals. DBViewer: added check on angular error after optimizing the graph when adding a new constraint. Memory: added warning if odom angular variance detected is very high.	2018-09-28 22:02:07 -04:00
matlabbe	dbb9cfa77a	Parameters: Updated description of Rtabmap/TimeThr and Rtabmap/MemoryThr (https://github.com/introlab/rtabmap_ros/issues/276 )	2018-09-28 15:37:24 -04:00
matlabbe	95e87fed14	MLS: added output voxel size parameter	2018-09-28 09:45:43 -04:00
matlabbe	02fdd677cf	💄	2018-09-27 16:08:55 -04:00
matlabbe	3b74534567	Parameters: Mem/LaserScanNormalRadius should be a float	2018-09-27 14:28:51 -04:00
matlabbe	790b0e5cf7	Memory: undistortion of features can be done also for RGB-only case	2018-09-27 10:19:28 -04:00
matlabbe	5e08da51aa	RealSense: fixed color rectification. Fixed ZR300 odometry frame.	2018-09-26 20:51:12 -04:00
matlabbe	0c2287df77	CameraModel fisheye model: fixed typo	2018-09-25 14:36:59 -04:00
matlabbe	b581a62c89	Fixed build with opencv < 2.4.10 for missing fisheye model	2018-09-24 15:09:12 -04:00
matlabbe	c341648a44	Added parameter "Rtabmap/RectifyOnlyFeatures". Added support of fisheye distortion model.	2018-09-24 14:36:54 -04:00
matlabbe	f903ffb927	Fixed regression errors for OpencV2 (#309 )	2018-09-18 23:03:40 -04:00
matlabbe	124543c57d	Fixed compilation errors with OpenCV 4.0.0 (#309 )	2018-09-18 22:43:18 -04:00
matlabbe	829f05e2fb	Implemented missing max angular optimization error check for RGBD/OptimizeMaxError	2018-09-18 22:34:15 -04:00
matlabbe	d936b2d35a	Added '*' to cmake info to show dependencies affecting default parameters	2018-09-18 17:00:37 -04:00
matlabbe	84a8e5830e	fixed https://github.com/introlab/rtabmap_ros/issues/274	2018-09-18 12:30:01 -04:00
matlabbe	eedc68c360	fixed #312	2018-09-18 12:20:15 -04:00
matlabbe	0cf37fbbf1	Added D415 icon	2018-09-15 11:37:02 -04:00
matlabbe	5f6dd0846d	NoEventsExample: updated usage example info	2018-09-07 13:11:39 -04:00
matlabbe	3c15563569	fixed sqlite3 cmake info not correctly shown	2018-09-05 15:03:50 -04:00
matlabbe	1f985ddef0	FindSqlite3.cmake: removed sqlite3 executable search	2018-09-05 12:05:16 -04:00
matlabbe	43e144e7b6	CMake: using external sqlite3 is available, otherwise use internal copy.	2018-09-05 14:22:34 -04:00
matlabbe	9c70b7116b	fixed a cmake warning	2018-09-04 19:01:43 -04:00
matlabbe	3c1095be65	Windows: BUILD_AS_BUNDLE option should now be set to ON to create package with all dependencies embedded, otherwise only rtabmap libraries and headers are installed (like default on linux and Mac).	2018-09-04 15:44:37 -04:00
matlabbe	89f27e84d0	💄	2018-08-29 15:36:53 -04:00
matlabbe	f64a5e75d5	fixed fovis.hpp not found when using kinetic libfovis branch of https://github.com/srv/libfovis	2018-08-29 14:48:30 -04:00
matlabbe	94178c8cde	LOAM: set back to ON by default as the pull request (https://github.com/laboshinl/loam_velodyne/pull/66 ) has been integrated	2018-08-29 14:38:50 -04:00
matlabbe	30290c36d7	Rtabmap: Added deleteLastLocation() and rejectLastLoopClosure() methods	2018-08-29 12:29:11 -04:00
matlabbe	956f07785b	Fixed build error (vtk version issue)	2018-08-28 17:29:20 -04:00
matlabbe	3e6f14f3bd	CloudViewer: added camera Ortho mode context menu option	2018-08-28 17:07:36 -04:00
matlabbe	7cb39f02f2	CloudViewer: Reduced interactor motion from 10 to 5	2018-08-28 11:49:32 -04:00
matlabbe	c105804572	Moved DBDriverSqlite3.h to installed headers (changed some members from private to protected for easier inheritance)	2018-08-27 16:54:23 -04:00
matlabbe	f498cf1b1a	CloudViewer: added getColor(id) and setColor(id) to change color of an actor	2018-08-25 11:41:28 -04:00
matlabbe	c0a2efe7e2	Fixed build with PCL <1.7.2	2018-08-24 16:17:30 -04:00
matlabbe	080d044c99	CloudViewer: added setInteractorLayer() function	2018-08-24 15:27:42 -04:00
matlabbe	cbf14bfa08	CloudViewer: set minimum ambiant light of 0.5 for all meshes	2018-08-23 16:18:04 -04:00
matlabbe	110f4a99ee	CloudViewer: added addOrUpdateQuad() function	2018-08-23 11:59:24 -04:00
matlabbe	63af05ef88	CloudViewer: refactoring + added CloudViewerCellPicker class to ignore picking backfaces (when backface culling is on)	2018-08-21 16:11:05 -04:00
matlabbe	0c790005b2	ImageView: colormap not updated if depth cv image is not set	2018-08-17 15:30:13 -04:00
matlabbe	3ce6de573d	ImageView: added depth colormap menu option	2018-08-17 15:28:23 -04:00
matlabbe	67aa4cd28e	DbViewer: added decimation parameter for cloud visualization. ImageView: adding scaling to height menu option.	2018-08-17 14:01:11 -04:00
matlabbe	c18f3cd539	Added rtabmap-export example	2018-08-16 17:01:02 -04:00
matlabbe	9e13d5600a	GUI: Fixed ProbMiss assert when resetting value to default	2018-08-16 16:51:29 -04:00
matlabbe	35d5200a3b	Parameters in group GridGlobal: renamed OctoMapOccupancyThr to OccupancyThr, added ProbHit, ProbMiss, ProbClampingMin, ProbClampingMax. Using logodds approach from OctoMap to evaluate occupancy of standard grid map. ref https://github.com/introlab/rtabmap_ros/issues/269	2018-08-16 14:42:05 -04:00
matlabbe	e0858a9c2a	GraphView: added orientation visualization on nodes	2018-08-16 11:21:25 -04:00
matlabbe	b8847fd006	0.17.5: added --version option on command line	2018-08-14 14:21:44 -04:00
matlabbe	714d95cc34	Removing redundant non null check before deleting a ptr in destructors #301	2018-08-09 15:53:47 -04:00
matlabbe	5e60a2596c	DbViewer: show full covariance in Constraints View	2018-08-09 15:20:19 -04:00
matlabbe	f281db8dd0	Windows: removed yaml_cpp required if pkgconfig is installed	2018-08-07 20:34:46 -04:00
matlabbe	7f09a9e0cb	fixed #293	2018-08-06 16:34:09 -04:00
matlabbe	fe52060de7	Update .travis.yml	2018-08-06 15:04:38 -04:00
matlabbe	d5128ddc18	Added parameter Mem/CovOffDiagIgnored (default true)	2018-08-05 12:51:50 -04:00
matlabbe	f938e8ce29	Calibration dialog: updated some tooltips	2018-08-03 14:49:39 -04:00
matlabbe	a0342671ac	OccupancyGrid: added getCache() method	2018-08-02 16:53:44 -04:00
matlabbe	e03da92a90	FAST: disabled grid parameters by default	2018-07-27 16:41:19 -04:00
matlabbe	4d5b42ab79	rtabmap: parameters passed by arguments override those in the database loaded from command line. Dictionary: set words saved to avoid saving them in database (causing db error when remapping from an old db created by fixed dictionary). G2O: ba supporting words with negative ids.	2018-07-27 16:01:17 -04:00
matlabbe	b63590bf1d	Memory: Fixed features with no id (-1) ignored for triangulation from motion. Dictionary: matching with negative word ids is now allowed	2018-07-26 20:41:54 -04:00
matlabbe	cfdee23d33	Statistics: added variance info of the loop closure/localization. GUI: Graph view option to show nodes on which loop closures/localizations happened	2018-07-26 14:40:16 -04:00
matlabbe	60499e895f	Updated About dialog	2018-07-24 17:11:27 -04:00
matlabbe	ddacee6d8b	rtabmap-report: ignoring --scale option if no ground truth is found	2018-07-24 14:38:14 -04:00
matlabbe	ae226cb1a2	0.17.4 (integrated MSCKF odom approach)	2018-07-23 15:56:53 -04:00
matlabbe	a7e70ab80b	Set LOAM default off as a special pull request should be used to use it as a library without ros	2018-07-20 19:41:50 -04:00
matlabbe	e95cabb1fc	fixed optimization error ratio bug where comparison was done using not highest ratio link but on highest abs error link instead.	2018-07-20 15:11:46 -04:00
matlabbe	173bd49a26	Handling intial odometry pose in all odometry approaches. For VIO approaches, gravity initialization is handled too. (#298 )	2018-07-19 14:10:36 -04:00
matlabbe	9ae47b79f9	Working MSCKF	2018-07-18 16:15:41 -04:00
matlabbe	15e09cd0a8	Devel: new feature msckf_vio	2018-07-14 20:40:44 -04:00
matlabbe	89a0eb506b	Fixed yaml-cpp not found on Ubuntu 14.04	2018-07-12 11:50:46 -04:00
matlabbe	4632c7650f	Reloading fixed dictionary when clearing memory	2018-07-12 10:06:02 -04:00
Nicolai Behmann	281452434c	Added support for OpenCV 3 CUDA ORB feature extractor and descriptor (#296 ) * added cuda support for orb with OpenCV 3 * added support for OpenCV 3 CUDA ORB feature detector and descriptor	2018-07-11 12:33:31 -04:00
matlabbe	ccdde45323	fixed build without RealSense2	2018-07-10 15:06:20 -04:00
matlabbe	a3e13b8e72	appveyor: updated gtsam/pcl binaries	2018-07-10 13:58:34 -04:00
matlabbe	35bc2d06a6	Fixed building with RealSense2 on Windows	2018-07-10 13:09:52 -04:00
matlabbe	ee00f81b5b	updated appveyor script	2018-07-10 12:30:23 -04:00
matlabbe	974db316ce	updated appveyor script	2018-07-10 12:21:55 -04:00
matlabbe	d41c15dbc7	Rtabmap::computePath() added tolerance parameter to overwrite default RGBD/LocalRadius	2018-07-09 15:11:19 -04:00
matlabbe	41d5e11511	Preferences: updated a description text	2018-07-09 12:01:31 -04:00
matlabbe	c8100e1464	Gui: show graph color based on visual features	2018-07-09 11:33:39 -04:00
matlabbe	b783df397a	fixed a cmake warning	2018-07-03 10:13:22 -04:00
matlabbe	62a64cd156	CameraRealSense2: increased wait time from 10 msec to 100 msec	2018-06-28 16:55:33 -04:00
matlabbe	675da6201a	Added OdomLOAM/LocalMapping parameter	2018-06-28 11:30:21 -04:00
matlabbe	d447329bf1	Added LOAM (loam_velodyne) odometry support	2018-06-28 11:10:46 -04:00
matlabbe	10b452197d	fixed build without realsense	2018-06-19 14:00:26 -04:00
matlabbe	973bf93c77	RealSense: added depthScaledToRGBSize option to be able to feed ORB_SLAM2 with depth and RGB images with same size #287 .	2018-06-19 13:53:03 -04:00
matlabbe	c91431410e	fixed g2o crash when failing computing marginals	2018-06-15 16:31:45 -04:00
matlabbe	a3bdb027e7	0.17.3: realsense2 and zed-m support	2018-06-15 14:37:22 -04:00
matlabbe	502d5e75e8	CameraRealSense2: added IR-Depth mode option	2018-06-15 14:35:54 -04:00
matlabbe	eb4de8e724	Added RealSense2 driver to UI	2018-06-15 13:29:08 -04:00
matlabbe	2060e0b1da	Added CameraRealsSense2 driver (tested only with D435)	2018-06-15 12:24:00 -04:00
matlabbe	dfcd7ae1a8	Added QT_NO_KEYWORDS definition to avoid conflicts between boost and qt macros	2018-06-15 12:17:11 -04:00
matlabbe	c6d893bc98	MainWindow: Odometry visualization not updated if msgs are received faster than they can be visualized. ZED: self-calibration set to true by default (should be true for ZED-M vio)	2018-06-14 17:25:35 -04:00
matlabbe	f638add755	fixed #286	2018-06-14 15:20:54 -04:00
matlabbe	9f22a2b1f8	Updated CameraStereoZed to latest zed sdk (2.4)	2018-06-03 02:40:19 -04:00
matlabbe	432b0dc6f6	fixed a build error	2018-06-01 16:33:11 -04:00
matlabbe	7424a1f463	When Mem/DepthAsMask is false, features without depth are triangulated using odometry. Added parameter RGBD/LocalBundleOnLoopClosure.	2018-06-01 16:26:09 -04:00
matlabbe	0bf83c0cd6	fixed frame trail on ros indigo	2018-05-30 17:24:21 -04:00
matlabbe	230e6a311d	fixed previous commit	2018-05-30 17:20:54 -04:00
matlabbe	26c004eee0	Fixed g2o build error on 14.04/Indigo	2018-05-30 16:21:06 -04:00
matlabbe	1914275fa8	0.17.2: compute marginals (covariance) on graph optimization	2018-05-30 15:35:08 -04:00
matlabbe	055cccd151	rtabmap: support two usb devices for stereo input	2018-05-29 15:43:02 -04:00
matlabbe	b6b0b9a984	Refactored and integrated pull request https://github.com/introlab/rtabmap/pull/285	2018-05-29 13:59:43 -04:00
matlabbe	0fc97c28c6	Merge pull request #284 from patrickelectric/corrections PreferencesDialog: Add forgotten include	2018-05-29 12:48:49 -04:00
Patrick José Pereira	11c34d383d	PreferencesDialog: Add forgotten include Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>	2018-05-26 21:40:24 -03:00
matlabbe	c68dde70ec	Memory: added getDatabaseUrl() function	2018-05-25 13:54:29 -04:00
matlabbe	124d78fefd	CloudViewer: custom interactor can be set for convenience	2018-05-25 12:32:22 -04:00
matlabbe	d34a529116	Added Rtabmap/StartNewMapOnGoodSignature parameter	2018-05-24 14:53:47 -04:00
matlabbe	4df9ac995a	Update .appveyor.yml	2018-05-22 15:42:45 -04:00
matlabbe	4149be47e0	appveyor: fixing gtsam and g2o download links	2018-05-22 12:28:55 -04:00
matlabbe	b7da3f7a97	appveyor: added g2o and gtsam dependencies	2018-05-22 12:04:15 -04:00
matlabbe	206c4fe09c	appveyor: using qt 5.10.1 instead of deprecated 5.8	2018-05-22 11:50:30 -04:00
matlabbe	df7539a48e	Update Parameters.h	2018-05-17 11:10:00 -04:00