Update package.xml

Bump package version

.appveyor.yml

@@ -18,9 +18,11 @@ init:
 
 install:
   # Qt
-  - set QTDIR=C:\Qt\5.8\msvc2015_64
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
   # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
   - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
   # Openni2
   - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
   - cmd: OpenNI2.exe -o"C:\Program Files" -y
@@ -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)
+  # VTK (including QVTK)
-  - ps: wget 'https://dl.dropboxusercontent.com/s/atf4r8kb1xyc1ls/PCL%201.8.1.exe?dl=0' -outfile PCL_1.8.1.exe
+  - 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\""
+  - ps: "ls \"C:/Program Files/PCL\""
-  - set PATH=%PATH%;C:\Program Files\PCL 1.8.1\bin
+  - 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

.gitignore

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

.travis.yml

@@ -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: 

CMakeLists.txt

@@ -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 0)
+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)
+IF(APPLE OR WIN32)
-    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle with all embedded dependencies (DragNDrop for Mac, installer for Windows)" OFF)
-ENDIF(APPLE)
+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)

Version.h.in

@@ -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_ */

app/src/CMakeLists.txt

@@ -104,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
         WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
        
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
   SET(plugin_dest_dir bin)
   SET(qtconf_dest_dir bin)
@@ -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))
   

app/src/ObjDeletionHandler.h

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

app/src/main.cpp

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

cmake_modules/FindRealSense2.cmake

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

cmake_modules/FindSqlite3.cmake

@@ -2,46 +2,29 @@
 # This module finds an installed Sqlite3 package.
 #
 # It sets the following variables:
-#  SQLITE3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
+#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  SQLITE3_INCLUDE_DIR - The Sqlite3 include directory.
+#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
-#  SQLITE3_LIBRARY     - The Sqlite3 library to link against.
+#  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_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
-	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_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_FOUND)
-
-IF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-   SET(SQLITE3_FOUND TRUE)
-ENDIF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-
-IF (SQLITE3_FOUND)
    # show which Sqlite3 was found only if not quiet
    IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3")
+      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
    ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (SQLITE3_FOUND)
+ELSE (Sqlite3_FOUND)
    # fatal error if Sqlite3 is required but not found
    IF (Sqlite3_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Sqlite3")
    ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (SQLITE3_FOUND)
+ENDIF (Sqlite3_FOUND)
 

corelib/include/rtabmap/core/CameraModel.h

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

corelib/include/rtabmap/core/CameraRGBD.h

@@ -39,9 +39,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_config.h>
 
 #ifdef HAVE_OPENNI
+#if __linux__ && __i386__ && __cplusplus >= 201103L
+#warning "Openni driver is not available on i386 when building with c++11 support"
+#else
+#define RTABMAP_OPENNI
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
 #endif
+#endif
 
 #include <boost/signals2/connection.hpp>
 
@@ -74,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;
 
@@ -102,7 +116,7 @@ public:
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenni();
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
     void image_cb (
     		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
@@ -365,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));
@@ -377,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;
@@ -393,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_;
@@ -404,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
+};
 
 
 /////////////////////////

corelib/include/rtabmap/core/CameraStereo.h

@@ -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

corelib/include/rtabmap/core/DBDriver.h

@@ -155,7 +155,7 @@ public:
 	void executeNoResult(const std::string & sql) const;
 
 	// Load objects
-	void load(VWDictionary * dictionary) const;
+	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	void loadLastNodes(std::list<Signature *> & signatures) const;
 	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
@@ -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;
@@ -255,7 +254,7 @@ private:
 				cv::Mat * textures) const = 0;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const = 0;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
-#include "sqlite3/sqlite3.h"
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;
 
 namespace rtabmap {
 
@@ -48,7 +50,7 @@ public:
 	void setSynchronous(int synchronous);
 	void setTempStore(int tempStore);
 
-private:
+protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
@@ -124,7 +126,7 @@ private:
 			cv::Mat * textures) const;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
@@ -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;

corelib/include/rtabmap/core/IMU.h

@@ -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();
 	}
 
 

corelib/include/rtabmap/core/Memory.h

@@ -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

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -39,6 +39,17 @@ namespace rtabmap {
 
 class RTABMAP_EXP OccupancyGrid
 {
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
@@ -53,6 +64,7 @@ public:
 	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
+	const std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > & getCache() const {return cache_;}
 
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -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_;

corelib/include/rtabmap/core/Odometry.h

@@ -50,7 +50,9 @@ public:
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
 		kTypeORBSLAM2 = 5,
-		kTypeOkvis = 6
+		kTypeOkvis = 6,
+		kTypeLOAM = 7,
+		kTypeMSCKF = 8
 	};
 
 public:

corelib/include/rtabmap/core/OdometryLOAM.h

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

corelib/include/rtabmap/core/OdometryMSCKF.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_ */

corelib/include/rtabmap/core/OdometryORBSLAM2.h

@@ -54,8 +54,9 @@ private:
 #ifdef RTABMAP_ORB_SLAM2
 	ORBSLAM2System * orbslam2_;
 	bool firstFrame_;
-#endif
 	Transform originLocalTransform_;
+	Transform previousPose_;
+#endif
 
 };
 

corelib/include/rtabmap/core/OdometryOkvis.h

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

corelib/include/rtabmap/core/Optimizer.h

@@ -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,
+				int rootId,
-			const std::map<int, Transform> & poses,
+				const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
+				const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				cv::Mat & outputCovariance,
-			double * finalError = 0,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			int * iterationsDone = 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,

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -64,12 +64,13 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 
 	virtual std::map<int, Transform> optimize(
-			int rootId,
+				int rootId,
-			const std::map<int, Transform> & poses,
+				const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
+				const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				cv::Mat & outputCovariance,
-			double * finalError = 0,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			int * iterationsDone = 0);
+				double * finalError = 0,
+				int * iterationsDone = 0);
 
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId,

corelib/include/rtabmap/core/OptimizerGTSAM.h

@@ -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);

corelib/include/rtabmap/core/OptimizerTORO.h

@@ -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);

corelib/include/rtabmap/core/Parameters.h

@@ -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, 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,      "Maximum signatures in the Working Memory (ms) (0 means infinity).");
+    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, 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, 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, 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();

corelib/include/rtabmap/core/Rtabmap.h

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

corelib/include/rtabmap/core/Statistics.h

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

corelib/include/rtabmap/core/StereoCameraModel.h

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

corelib/src/CMakeLists.txt

@@ -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
+	FlannIndex.cpp	
-	
-	sqlite3/sqlite3.c	
 	
 	#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

corelib/src/CameraModel.cpp

@@ -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 << "rows" << D.rows;
-			fs << "cols" << D_.cols;
+			fs << "cols" << D.cols;
-			fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*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
 			}
 		}
 

corelib/src/CameraRGB.cpp

@@ -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;
-	{
+	delete _scanDir;
-		delete _dir;
-	}
-	if(_scanDir)
-	{
-		delete _scanDir;
-	}
 }
 
 bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)

corelib/src/CameraStereo.cpp

@@ -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);
+				sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
-				int trackingConfidence = pose.pose_confidence;
+				if (tracking_state == sl::TRACKING_STATE_OK)
-				// FIXME What does pose_confidence == -1 mean?
-				if (trackingConfidence>0)
 				{
-					info->odomPose = zedPoseToTransform(pose);
+					int trackingConfidence = pose.pose_confidence;
-					if (!info->odomPose.isNull())
+					// FIXME What does pose_confidence == -1 mean?
+					if (trackingConfidence>0)
 					{
-						//transform x->forward, y->left, z->up
+						info->odomPose = zedPoseToTransform(pose);
-						Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+						if (!info->odomPose.isNull())
-						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
+							//transform x->forward, y->left, z->up
-							lost_ = false;
+							Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
-							UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+							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);
+							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
-							UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+							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
+					UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
-					lost_ = true;
-					UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
 				}
 			}
 		}
@@ -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))
 			{

corelib/src/CameraThread.cpp

@@ -73,14 +73,8 @@ CameraThread::~CameraThread()
 {
 	UDEBUG("");
 	join(true);
-	if(_camera)
+	delete _camera;
-	{
+	delete _distortionModel;
-		delete _camera;
-	}
-	if(_distortionModel)
-	{
-		delete _distortionModel;
-	}
 	delete _stereoDense;
 }
 

corelib/src/DBDriver.cpp

@@ -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 {
 
@@ -436,7 +436,7 @@ void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures)
 
 void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
 {
-	ULOGGER_DEBUG("");
+	ULOGGER_DEBUG("words.size=%d", (int)words.size());
 	std::list<VisualWord *> toSave;
 	std::list<VisualWord *> toUpdate;
 	if(this->isConnected() && words.size())
@@ -511,10 +511,10 @@ void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::load(VWDictionary * dictionary) const
+void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
 {
 	_dbSafeAccessMutex.lock();
-	this->loadQuery(dictionary);
+	this->loadQuery(dictionary, lastStateOnly);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -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()),
@@ -3077,7 +3078,7 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
 	}
 }
 
-void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
+void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
 {
 	ULOGGER_DEBUG("");
 	if(_ppDb && dictionary)
@@ -3087,26 +3088,25 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 		timer.start();
 		int rc = SQLITE_OK;
 		sqlite3_stmt * ppStmt = 0;
-		std::string query;
+		std::stringstream query;
 		std::list<VisualWord *> visualWords;
 
 		// Get the visual words
-		if(uStrNumCmp(_version, "0.11.11") >= 0)
+		query << "SELECT id, descriptor_size, descriptor FROM Word ";
+		if(lastStateOnly)
 		{
-			query = "SELECT id, descriptor_size, descriptor "
+			if(uStrNumCmp(_version, "0.11.11") >= 0)
-					"FROM Word "
+			{
-					"WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) "
+				query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) ";
-					"ORDER BY id;";
+			}
-		}
+			else
-		else
+			{
-		{
+				query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) ";
-			query = "SELECT id, descriptor_size, descriptor "
+			}
-					"FROM Word "
-					"WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
-					"ORDER BY id;";
 		}
+		query << "ORDER BY id;";
 
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 		// Process the result if one
@@ -3958,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());

corelib/src/DBReader.cpp

@@ -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.");

corelib/src/Features2d.cpp

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

corelib/src/IMUThread.cpp

@@ -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())

corelib/src/Memory.cpp

@@ -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),
@@ -279,25 +284,32 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			}
 
 			UDEBUG("load words %d", (int)wordIds.size());
-			if(wordIds.size())
+			if(_vwd->isIncremental())
 			{
-				std::list<VisualWord*> words;
+				if(wordIds.size())
-				_dbDriver->loadWords(wordIds, words);
-				for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
 				{
-					_vwd->addWord(*iter);
+					std::list<VisualWord*> words;
+					_dbDriver->loadWords(wordIds, words);
+					for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
+					{
+						_vwd->addWord(*iter);
+					}
+					// Get Last word id
+					int id = 0;
+					_dbDriver->getLastWordId(id);
+					_vwd->setLastWordId(id);
 				}
-				// Get Last word id
+			}
-				int id = 0;
+			else
-				_dbDriver->getLastWordId(id);
+			{
-				_vwd->setLastWordId(id);
+				_dbDriver->load(_vwd, false);
 			}
 		}
 		else
 		{
 			UDEBUG("load words");
 			// load the last dictionary
-			_dbDriver->load(_vwd);
+			_dbDriver->load(_vwd, _vwd->isIncremental());
 		}
 		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
 		_vwd->update();
@@ -327,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());
 		}
@@ -420,26 +432,11 @@ Memory::~Memory()
 	{
 		UWARN("Please call Memory::close() before");
 	}
-	if(_feature2D)
+	delete _feature2D;
-	{
+	delete _vwd;
-		delete _feature2D;
+	delete _registrationPipeline;
-	}
+	delete _registrationIcpMulti;
-	if(_vwd)
+	delete _occupancy;
-	{
-		delete _vwd;
-	}
-	if(_registrationPipeline)
-	{
-		delete _registrationPipeline;
-	}
-	if(_registrationIcpMulti)
-	{
-		delete _registrationIcpMulti;
-	}
-	if(_occupancy)
-	{
-		delete _occupancy;
-	}
 }
 
 void Memory::parseParameters(const ParametersMap & parameters)
@@ -473,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);
@@ -489,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());
@@ -649,7 +649,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
 void Memory::preUpdate()
 {
 	_signaturesAdded = 0;
-	this->cleanUnusedWords();
+	if(_vwd->isIncremental())
+	{
+		this->cleanUnusedWords();
+	}
 	if(_vwd && !_parallelized)
 	{
 		//When parallelized, it is done in CreateSignature
@@ -798,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))
 					{
@@ -823,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
+				if(_mapLabelsAdded && isIncremental())
-				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());
+					//Tag the first node of the map
-					signature->setLabel(tag);
+					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());
@@ -1363,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;
@@ -1478,6 +1517,8 @@ void Memory::clear()
 	_memoryChanged = false;
 	_linksChanged = false;
 	_gpsOrigin = GPS();
+	_rectCameraModels.clear();
+	_rectStereoCameraModel = StereoCameraModel();
 
 	if(_dbDriver)
 	{
@@ -1584,7 +1625,7 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
 				{
 					// "Inverted index" - Pour chaque endroit contenu dans chaque mot
 					vw = _vwd->getWord(*i);
-					UASSERT(vw!=0);
+					UASSERT_MSG(vw!=0, uFormat("Word %d not found in dictionary!?", *i).c_str());
 
 					const std::map<int, int> & refs = vw->getReferences();
 					nw = refs.size();
@@ -2055,7 +2096,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		}
 
 		this->disableWordsRef(s->id());
-		if(!keepLinkedToGraph)
+		if(!keepLinkedToGraph && _vwd->isIncremental())
 		{
 			std::list<int> keys = uUniqueKeys(s->getWords());
 			for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
@@ -2102,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()))
@@ -2375,6 +2416,7 @@ Transform Memory::computeTransform(
 		RegistrationInfo * info,
 		bool useKnownCorrespondencesIfPossible) const
 {
+	UDEBUG("");
 	Transform transform;
 
 	// make sure we have all data needed
@@ -2449,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);
@@ -3489,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())
+		// we assume that once rtabmap is receiving data, the calibration won't change over time
-		{
-			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;
-		}
 		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
 				{
@@ -3522,8 +3747,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 		else if(data.stereoCameraModel().isValidForRectification())
 		{
-			data.setImageRaw(data.stereoCameraModel().left().rectifyImage(data.imageRaw()));
+			if(!_rectStereoCameraModel.isValidForRectification())
-			data.setDepthOrRightRaw(data.stereoCameraModel().right().rectifyImage(data.rightRaw()));
+			{
+				_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
 		{
@@ -3538,10 +3776,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}
 
 	int treeSize= int(_workingMem.size() + _stMem.size());
-	int meanWordsPerLocation = 0;
+	int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
-	if(treeSize > 0)
+	if(treeSize > 1)
 	{
-		meanWordsPerLocation = _vwd->getTotalActiveReferences() / treeSize;
+		meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
 	}
 
 	if(_parallelized && !isIntermediateNode)
@@ -3552,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;
@@ -3592,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 &&
@@ -3636,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()) ||
+			else
-					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 			{
-				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
+				if(!imagesRectified && decimatedData.cameraModels().size())
-				t = timer.ticks();
+				{
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+					std::vector<cv::KeyPoint> keypointsValid;
-				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+					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())
@@ -3694,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();
@@ -3814,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);
 		}
 
@@ -3834,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)
 		{
@@ -3860,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();
@@ -3909,19 +4238,29 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// apply decimation?
 	if(_imagePostDecimation > 1 && !isIntermediateNode)
 	{
-		if(!data.rightRaw().empty() ||
+		if(_imagePostDecimation == preDecimation && decimatedData.isValid())
-			(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
 		{
-			depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
+			image = decimatedData.imageRaw();
+			depthOrRightImage = decimatedData.depthOrRightRaw();
+			cameraModels = decimatedData.cameraModels();
+			stereoCameraModel = decimatedData.stereoCameraModel();
 		}
-		image = util2d::decimate(image, _imagePostDecimation);
+		else
-		for(unsigned int i=0; i<cameraModels.size(); ++i)
 		{
-			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
+			if(!data.rightRaw().empty() ||
-		}
+				(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
-		if(stereoCameraModel.isValidForProjection())
+			{
-		{
+				depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
-			stereoCameraModel.scale(1.0/double(_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();
@@ -3929,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())
@@ -4217,25 +4641,22 @@ void Memory::disableWordsRef(int signatureId)
 
 void Memory::cleanUnusedWords()
 {
-	if(_vwd->isIncremental())
+	std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
+	UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
+	if(removedWords.size())
 	{
-		std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
+		// remove them from the dictionary
-		UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
+		_vwd->removeWords(removedWords);
-		if(removedWords.size())
-		{
-			// remove them from the dictionary
-			_vwd->removeWords(removedWords);
 
-			for(unsigned int i=0; i<removedWords.size(); ++i)
+		for(unsigned int i=0; i<removedWords.size(); ++i)
+		{
+			if(_dbDriver)
 			{
-				if(_dbDriver)
+				_dbDriver->asyncSave(removedWords[i]);
-				{
+			}
-					_dbDriver->asyncSave(removedWords[i]);
+			else
-				}
+			{
-				else
+				delete removedWords[i];
-				{
-					delete removedWords[i];
-				}
 			}
 		}
 	}
@@ -4270,6 +4691,11 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 		}
 	}
 
+	if(!_vwd->isIncremental() && oldWordIds.size())
+	{
+		UWARN("Dictionary is fixed, but some words retrieved have not been found!?");
+	}
+
 	UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
 
 	// the words were deleted, so try to math it with an active word

corelib/src/OccupancyGrid.cpp

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

corelib/src/OctoMap.cpp

@@ -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();
+	float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
-	Parameters::parse(parameters, Parameters::kGridGlobalOctoMapOccupancyThr(), occupancyThr);
+	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_);

corelib/src/Odometry.cpp

@@ -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());
 		}

corelib/src/OdometryDVO.cpp

@@ -55,18 +55,9 @@ OdometryDVO::OdometryDVO(const ParametersMap & parameters) :
 OdometryDVO::~OdometryDVO()
 {
 #ifdef RTABMAP_DVO
-	if(dvo_)
+	delete dvo_;
-	{
+	delete reference_;
-		delete dvo_;
+	delete camera_;
-	}
-	if(reference_)
-	{
-		delete reference_;
-	}
-	if(camera_)
-	{
-		delete camera_;
-	}
 #endif
 }
 

corelib/src/OdometryF2M.cpp

@@ -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();

corelib/src/OdometryFovis.cpp

@@ -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_;
-	{
+	delete rect_;
-		delete fovis_;
+	delete stereoCalib_;
-	}
+	delete depthImage_;
-	if(rect_)
+	delete stereoDepth_;
-	{
-		delete rect_;
-	}
-	if(stereoCalib_)
-	{
-		delete stereoCalib_;
-	}
-	if(depthImage_)
-	{
-		delete depthImage_;
-	}
-	if(stereoDepth_)
-	{
-		delete stereoDepth_;
-	}
 #endif
 }
 

corelib/src/OdometryLOAM.cpp

@@ -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

corelib/src/OdometryMSCKF.cpp

@@ -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

corelib/src/OdometryORBSLAM2.cpp

@@ -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 = previousPoseInv*p;
-			t = this->getPose().inverse() * t;
 		}
+		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();
+				cv::Point3f pt(mapPoints[i]->GetWorldPos());
-				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_)));
+				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)));
 			}
 		}
 	}

corelib/src/OdometryOkvis.cpp

@@ -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_;
+		if(okvisEstimator_)
-		okvisEstimator_ = 0;
+		{
-	}
+			delete okvisEstimator_;
-	lastImu_ = IMU();
+			okvisEstimator_ = 0;
+		}
+		lastImu_ = IMU();
+		imagesProcessed_ = 0;
+		previousPose_.setIdentity();
 
-	delete okvisCallbackHandler_;
+		delete okvisCallbackHandler_;
-	okvisCallbackHandler_ = new 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.");

corelib/src/OdometryThread.cpp

@@ -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() && data.imu().empty()))
-		if(!data.imageRaw().empty() || pose.isNull())
 		{
+			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();

corelib/src/OdometryViso2.cpp

@@ -71,10 +71,7 @@ OdometryViso2::OdometryViso2(const ParametersMap & parameters) :
 OdometryViso2::~OdometryViso2()
 {
 #ifdef RTABMAP_VISO2
-	if(viso2_)
+	delete viso2_;
-	{
-		delete viso2_;
-	}
 #endif
 }
 

corelib/src/Optimizer.cpp

@@ -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)

corelib/src/OptimizerG2O.cpp

@@ -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]);

corelib/src/OptimizerGTSAM.cpp

@@ -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)

corelib/src/OptimizerTORO.cpp

@@ -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)
 	{

corelib/src/Parameters.cpp

@@ -136,7 +136,8 @@ ParametersMap Parameters::deserialize(const std::string & parameters)
 
 			}
 
-			if(Parameters::getDefaultParameters().find(key) == Parameters::getDefaultParameters().end())
+			if(oldIter==Parameters::getRemovedParameters().end() &&
+			   Parameters::getDefaultParameters().find(key) == Parameters::getDefaultParameters().end())
 			{
 				UWARN("Unknown parameter \"%s\"=\"%s\"! The parameter is still added to output map.", key.c_str(), value.c_str());
 			}
@@ -226,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())));
 
@@ -513,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"
@@ -525,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"
 			;
 }
 
@@ -544,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);
 			}
@@ -678,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)
 					{
@@ -795,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(oldIter->second.first)
-					if(addParameter)
 					{
 						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());
 						}
 					}

corelib/src/Registration.cpp

@@ -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)
 {

corelib/src/RegistrationIcp.cpp

@@ -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
+							if(fromScan.hasNormals() && toScan.hasNormals())
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
+							{
-							pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+								// 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(
+								util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
+										fromCloudNormalsRegistered,
-									toCloudNormals,
+										toCloudNormals,
-									_maxCorrespondenceDistance,
+										_maxCorrespondenceDistance,
-									_maxRotation,
+										_maxRotation,
-									variance,
+										variance,
-									correspondences);
+										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);

corelib/src/Rtabmap.cpp

@@ -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 ||
+						bool isMoving = (_rgbdLinearUpdate>0.0f && (
-										fabs(y) > _rgbdLinearUpdate ||
+											fabs(x) > _rgbdLinearUpdate ||
-										fabs(z) > _rgbdLinearUpdate ||
+											fabs(y) > _rgbdLinearUpdate ||
-										fabs(roll) > _rgbdAngularUpdate ||
+											fabs(z) > _rgbdLinearUpdate))
-										fabs(pitch) > _rgbdAngularUpdate ||
+										||
-										fabs(yaw) > _rgbdAngularUpdate;
+									    (_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()));
+					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)
-					float stddev = sqrt(maxLinearLink->transVariance());
-					maxLinearErrorRatio = maxLinearError/stddev;
-					if(maxLinearErrorRatio > _optimizationMaxLinearError)
 					{
 						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 "
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
-							  "maximum error ratio parameter is %f of std deviation.",
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
 							  (int)loopClosureLinksAdded.size(),
 							  maxLinearErrorRatio,
 							  maxLinearLink->from(),
 							  maxLinearLink->to(),
 							  maxLinearLink->type(),
 							  maxLinearError,
-							  stddev,
+							  sqrt(maxLinearLink->transVariance()),
-							  _optimizationMaxLinearError);
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
-						for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
+							  _optimizationMaxError);
-						{
+						reject = true;
-							_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(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(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
-	if(_loopClosureHypothesis.first)
 	{
-		_loopClosureHypothesis.first = 0;
+		std::map<int, Link> links = _memory->getLinks(getLastLocationId(), false);
-		if(_memory)
+		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
 		{

corelib/src/StereoDense.cpp

@@ -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 {
 

corelib/src/Transform.cpp

@@ -58,8 +58,15 @@ Transform::Transform(const cv::Mat & transformationMatrix)
 {
 	UASSERT(transformationMatrix.cols == 4 &&
 			transformationMatrix.rows == 3 &&
-			transformationMatrix.type() == CV_32FC1);
+			(transformationMatrix.type() == CV_32FC1 || transformationMatrix.type() == CV_64FC1));
-	data_ = transformationMatrix;
+	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)

corelib/src/VWDictionary.cpp

@@ -142,79 +142,120 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 		if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
 		   _visualWords.size() == 0)
 		{
-			std::ifstream file;
+			UDEBUG("incremental=%d, oldPath=%s newPath=%s, visual words=%d",
-			file.open(dictionaryPath.c_str(), std::ifstream::in);
+					_incrementalDictionary?1:0, _dictionaryPath.c_str(), dictionaryPath.c_str(), (int)_visualWords.size());
-			if(file.good())
+
+			if(UFile::getExtension(dictionaryPath).compare("db") == 0)
 			{
-				UDEBUG("Deleting old dictionary and loading the new one from \"%s\"", dictionaryPath.c_str());
+				UDEBUG("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
-				UTimer timer;
+				DBDriver * driver = DBDriver::create();
-
+				if(driver->openConnection(dictionaryPath, false))
-				// first line is the header
-				std::string str;
-				std::list<std::string> strList;
-				std::getline(file, str);
-				strList = uSplitNumChar(str);
-				unsigned int dimension  = 0;
-				for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
 				{
-					if(uIsDigit(iter->at(0)))
+					driver->load(this, false);
+					for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
 					{
-						dimension = std::atoi(iter->c_str());
+						iter->second->setSaved(true);
-						break;
 					}
-				}
+					_incrementalDictionary = _visualWords.size()==0;
-
+					driver->closeConnection(false);
-				if(dimension == 0 || dimension > 1000)
-				{
-					UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
 				}
 				else
 				{
-					// Process all words
+					UERROR("Could not load dictionary from database %s", dictionaryPath.c_str());
-					while(file.good())
-					{
-						std::getline(file, str);
-						strList = uSplit(str);
-						if(strList.size() == dimension+1)
-						{
-							//first one is the visual word id
-							std::list<std::string>::iterator iter = strList.begin();
-							int id = std::atoi(iter->c_str());
-							cv::Mat descriptor(1, dimension, CV_32F);
-							++iter;
-							unsigned int i=0;
-
-							//get descriptor
-							for(;i<dimension && iter != strList.end(); ++i, ++iter)
-							{
-								descriptor.at<float>(i) = uStr2Float(*iter);
-							}
-							if(i != dimension)
-							{
-								UERROR("");
-							}
-
-							VisualWord * vw = new VisualWord(id, descriptor, 0);
-							_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
-							_notIndexedWords.insert(_notIndexedWords.end(), id);
-						}
-						else
-						{
-							UWARN("Cannot parse line \"%s\"", str.c_str());
-						}
-					}
-					this->update();
-					_incrementalDictionary = false;
 				}
-
+				delete driver;
-
-				UDEBUG("Time changing dictionary = %fs", timer.ticks());
 			}
 			else
 			{
-				UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
+				UWARN("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
+				std::ifstream file;
+				file.open(dictionaryPath.c_str(), std::ifstream::in);
+				if(file.good())
+				{
+					UDEBUG("Deleting old dictionary and loading the new one from \"%s\"", dictionaryPath.c_str());
+					UTimer timer;
+
+					// first line is the header
+					std::string str;
+					std::list<std::string> strList;
+					std::getline(file, str);
+					strList = uSplitNumChar(str);
+					int dimension  = 0;
+					for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
+					{
+						if(uIsDigit(iter->at(0)))
+						{
+							dimension = std::atoi(iter->c_str());
+							break;
+						}
+					}
+					UDEBUG("descriptor dimension = %d", dimension);
+
+					if(dimension <= 0 || dimension > 1000)
+					{
+						UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
+					}
+					else
+					{
+						// Process all words
+						while(file.good())
+						{
+							std::getline(file, str);
+							strList = uSplit(str);
+							if((int)strList.size() == dimension+1)
+							{
+								//first one is the visual word id
+								std::list<std::string>::iterator iter = strList.begin();
+								int id = std::atoi(iter->c_str());
+								cv::Mat descriptor(1, dimension, CV_32F);
+								++iter;
+								int i=0;
+
+								//get descriptor
+								for(;i<dimension && iter != strList.end(); ++i, ++iter)
+								{
+									descriptor.at<float>(i) = uStr2Float(*iter);
+								}
+								if(i != dimension)
+								{
+									UERROR("Loaded word has not the same size (%d) than descriptor size previously detected (%d).", i, dimension);
+								}
+
+								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));
+							}
+							else if(!str.empty())
+							{
+								UWARN("Cannot parse line \"%s\"", str.c_str());
+							}
+						}
+						if(_visualWords.size())
+						{
+							UWARN("Loaded %d words!", (int)_visualWords.size());
+						}
+					}
+				}
+				else
+				{
+					UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
+				}
+				file.close();
+			}
+
+			if(_visualWords.size() == 0)
+			{
+				_incrementalDictionary = _visualWords.size()==0;
+				UWARN("No words loaded, cannot set a fixed dictionary.", (int)_visualWords.size());
+			}
+			else
+			{
+				this->update();
+				_incrementalDictionary = false;
+				UDEBUG("Loaded %d words!", (int)_visualWords.size());
 			}
-			file.close();
 		}
 		else if(!_incrementalDictionary)
 		{
@@ -225,14 +266,14 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 			UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
 		}
 	}
-	else if(_visualWords.size() == 0)
+	else if(_incrementalDictionary && _visualWords.size())
-	{
-		_incrementalDictionary = false;
-	}
-	else if(_incrementalDictionary)
 	{
 		UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size());
 	}
+	else
+	{
+		_incrementalDictionary = false;
+	}
 	_dictionaryPath = dictionaryPath;
 }
 
@@ -546,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()
@@ -555,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->addRef(signatureId);
-		vw = uValue(_visualWords, wordId, vw);
+		_totalActiveReferences += 1;
-		if(vw)
-		{
-			vw->addRef(signatureId);
-			_totalActiveReferences += 1;
 
-			_unusedWords.erase(vw->id());
+		_unusedWords.erase(vw->id());
-		}
+	}
-		else
+	else
-		{
+	{
-			UERROR("Not found word %d", wordId);
+		UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
-		}
 	}
 }
 
@@ -590,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;
@@ -762,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));
+				int index;
-				if(d >= 0.0f && id > 0)
+				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));
 				}
@@ -779,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));
 				}
@@ -802,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));
 				}
@@ -1110,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));
+					int index;
-					if(d >= 0.0f && id > 0)
+
+					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));
 					}
@@ -1123,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));
 					}
@@ -1137,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));
 					}
@@ -1220,26 +1281,17 @@ VisualWord * VWDictionary::getUnusedWord(int id) const
 
 std::vector<VisualWord*> VWDictionary::getUnusedWords() const
 {
-	if(!_incrementalDictionary)
-	{
-		ULOGGER_WARN("This method does nothing on a fixed dictionary");
-		return std::vector<VisualWord*>();
-	}
 	return uValues(_unusedWords);
 }
 
 std::vector<int> VWDictionary::getUnusedWordIds() const
 {
-	if(!_incrementalDictionary)
-	{
-		ULOGGER_WARN("This method does nothing on a fixed dictionary");
-		return std::vector<int>();
-	}
 	return uKeys(_unusedWords);
 }
 
 void VWDictionary::removeWords(const std::vector<VisualWord*> & words)
 {
+	UDEBUG("Removing %d words from dictionary (current size=%d)", (int)words.size(), (int)_visualWords.size());
 	for(unsigned int i=0; i<words.size(); ++i)
 	{
 		_visualWords.erase(words[i]->id());

corelib/src/clams/discrete_depth_distortion_model_helpers.cpp

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

corelib/src/opencv/solvepnp.h

@@ -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 {
 

corelib/src/util2d.cpp

@@ -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>
 

corelib/src/util3d.cpp

@@ -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
 {

corelib/src/util3d_features.cpp

@@ -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(),

corelib/src/util3d_mapping.cpp

@@ -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])",
+		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);
+				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>(0) = iter->second.first.ptr<float>(0,0)[0];
-						endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1]+origin.at<float>(1);
+						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]+origin.at<float>(0);
+						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]+origin.at<float>(1);
+						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));

corelib/src/util3d_surface.cpp

@@ -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>

docker/artful/Dockerfile

@@ -18,7 +18,57 @@ RUN apt-get update && apt-get install -y \
 
 WORKDIR /root/
 
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+
 # Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/artful/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:17.10 .

docker/artful/hooks/post_push

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:17.10

docker/bionic/Dockerfile

@@ -18,7 +18,57 @@ RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
 
 WORKDIR /root/
 
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+
 # Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/bionic/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:18.04 .

docker/bionic/hooks/post_push

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:18.04

docker/xenial/Dockerfile

@@ -15,8 +15,58 @@ RUN apt-get update && apt-get install -y \
       libopenni2-dev \
       software-properties-common
 
-# Clone source code
 WORKDIR /root/
+
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+# Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/xenial/android/install_deps.bash

@@ -29,7 +29,7 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
 rm -r boost_1_59_0.tar.gz boost_1_59_0
 
 # eigen
@@ -46,7 +46,7 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
 rm -r 3.2.7.tar.gz eigen-eigen-b30b87236a1b
 
 # FLANN
@@ -63,7 +63,7 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
 rm -r flann-1.8.4-src.zip flann-1.8.4-src
 
 # GTSAM
@@ -82,12 +82,13 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r gtsam
+rm -rf gtsam
 
 # g2o
 git clone https://github.com/RainerKuemmerle/g2o.git
 cd g2o
+git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
 mkdir build
 cd build
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
@@ -97,8 +98,8 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r g2o
+rm -rf g2o
 
 
 #CMake 3.7 for VTK
@@ -113,15 +114,15 @@ cd VTK
 git checkout tags/v7.1.1
 mkdir build
 cd build
-~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
+$pwd/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 cp -r CMakeExternals/Install/vtk-android/* $prefix/armeabi-v7a/.
 rm -r *
-~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
+$pwd/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 cp -r CMakeExternals/Install/vtk-android/* $prefix/arm64-v8a/.
-cd
+cd $pwd
-rm -r VTK cmake-3.7.2-Linux-x86_64
+rm -rf VTK cmake-3.7.2-Linux-x86_64
 
 # PCL
 git clone https://github.com/PointCloudLibrary/pcl.git
@@ -143,25 +144,25 @@ cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r pcl
+rm -rf pcl
 
 # OpenCV
 git clone https://github.com/opencv/opencv_contrib.git
 cd opencv_contrib
 git checkout tags/3.2.0
-cd
+cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.2.0
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r opencv opencv_contrib
+rm -rf opencv opencv_contrib

docker/xenial/android/install_deps_host.bash

@@ -5,8 +5,8 @@ if [ $# -ne 1 ]; then
     exit 1
 fi
 
-apt-get update
+sudo apt-get update
-apt-get install -y git cmake g++ software-properties-common wget unzip zlib1g-dev qtbase5-dev libqt5x11extras5-dev libqt5svg5-dev qttools5-dev libxt-dev 
+sudo apt-get install -y git cmake g++ software-properties-common wget unzip zlib1g-dev qtbase5-dev libqt5x11extras5-dev libqt5svg5-dev qttools5-dev libxt-dev 
 
 prefix=$1
 pwd=$(pwd)
@@ -25,7 +25,7 @@ cd build
 cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
 rm -r boost_1_59_0.tar.gz boost_1_59_0
 
 # eigen
@@ -38,7 +38,7 @@ cd build
 cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
 rm -r 3.2.7.tar.gz eigen-eigen-b30b87236a1b
 
 # FLANN
@@ -51,7 +51,7 @@ cd build
 cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make 
 make install
-cd
+cd $pwd
 rm -r flann-1.8.4-src.zip flann-1.8.4-src
 
 # GTSAM
@@ -66,19 +66,20 @@ cd build
 cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make 
 make install
-cd
+cd $pwd
-rm -r gtsam
+rm -rf gtsam
 
 # g2o
 git clone https://github.com/RainerKuemmerle/g2o.git
 cd g2o
+git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
 mkdir build
 cd build
 cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r g2o
+rm -rf g2o
 
 # VTK
 git clone https://github.com/Kitware/VTK.git
@@ -89,8 +90,8 @@ cd build
 cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DVTK_Group_Qt=ON -DVTK_QT_VERSION=5 -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r VTK
+rm -rf VTK
 
 # PCL
 git clone https://github.com/PointCloudLibrary/pcl.git
@@ -104,14 +105,14 @@ cd build
 cmake -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=ON -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=ON -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r pcl
+rm -rf pcl
 
 # OpenCV
 git clone https://github.com/opencv/opencv_contrib.git
 cd opencv_contrib
 git checkout tags/3.2.0
-cd
+cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.2.0
@@ -120,5 +121,5 @@ cd build
 cmake -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=ON -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r opencv opencv_contrib
+rm -rf opencv opencv_contrib

docker/xenial/android/tango/build.bash

@@ -35,7 +35,7 @@ mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 cd rtabmap-tango/build
 cmake -DANDROID_PREBUILD=ON ..
 make
-cd
+cd ../..
 
 # rtabmap
 mkdir rtabmap-tango/build/armeabi-v7a
@@ -43,7 +43,7 @@ cd rtabmap-tango/build/armeabi-v7a
 cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/armeabi-v7a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ../..
 make
 
-cd
+cd ../../..
 mkdir rtabmap-tango/build/arm64-v8a
 cd rtabmap-tango/build/arm64-v8a
 cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ../..

docker/xenial/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:16.04 -t $DOCKER_REPO:latest  .

docker/xenial/hooks/post_push

@@ -0,0 +1,3 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:16.04
+docker push $DOCKER_REPO:latest

docker/zesty/Dockerfile

@@ -16,8 +16,59 @@ RUN apt-get update && apt-get install -y \
 # Issue: http://www.pcl-users.org/Build-failure-on-Ubuntu-17-04-td4044552.html
 RUN sed -i 's|/usr/lib/libmpi.so;||g' /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake
 
-# Clone source code
 WORKDIR /root/
+
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+
+# Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/zesty/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:17.04 .

docker/zesty/hooks/post_push

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:17.04

examples/CMakeLists.txt

@@ -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()

examples/Export/CMakeLists.txt

@@ -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)
+
+
+
+

examples/Export/main.cpp

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

examples/NoEventsExample/MapBuilder.h

@@ -224,7 +224,7 @@ public:
 		cloudViewer_->update();
 	}
 
-protected slots:
+protected Q_SLOTS:
 	void pauseDetection()
 	{
 		paused_ = !paused_;

examples/NoEventsExample/main.cpp

@@ -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");

examples/RGBDMapping/MapBuilder.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/OccupancyGrid.h"
 #endif
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UConversion.h"
@@ -85,7 +86,7 @@ public:
 		this->unregisterFromEventsManager();
 	}
 
-protected slots:
+protected Q_SLOTS:
 	virtual void pauseDetection()
 	{
 		UWARN("");
@@ -242,6 +243,44 @@ protected slots:
 			cloudViewer_->setCloudPointSize("graph_nodes", 5);
 		}
 
+		//============================
+		// Update/add occupancy grid (when RGBD/CreateOccupancyGrid is true)
+		//============================
+		for(std::map<int, Transform>::const_reverse_iterator iter = stats.poses().rbegin(); iter!=stats.poses().rend(); ++iter)
+		{
+			int id = iter->first;
+			if(grid_.addedNodes().find(id) == grid_.addedNodes().end())
+			{
+				std::map<int, Signature>::const_iterator jter = stats.getSignatures().find(id);
+				if(jter != stats.getSignatures().end() && jter->second.sensorData().gridCellSize() > 0.0f)
+				{
+					cv::Mat groundCells, obstacleCells, emptyCells;
+					jter->second.sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells);
+					grid_.addToCache(id, groundCells, obstacleCells, emptyCells);
+				}
+			}
+			else
+			{
+				// Assume that older nodes are already added to map
+				break;
+			}
+		}
+		if(grid_.addedNodes().size() || grid_.cacheSize())
+		{
+			grid_.update(stats.poses());
+		}
+		if(grid_.addedNodes().size())
+		{
+			float xMin, yMin;
+			cv::Mat map8S = grid_.getMap(xMin, yMin);
+			if(!map8S.empty())
+			{
+				//convert to gray scaled map
+				cv::Mat map8U = util3d::convertMap2Image8U(map8S);
+				cloudViewer_->addOccupancyGridMap(map8U, grid_.getCellSize(), xMin, yMin, 0.75);
+			}
+		}
+
 		odometryCorrection_ = stats.mapCorrection();
 
 		cloudViewer_->update();
@@ -283,6 +322,7 @@ protected:
 	Transform odometryCorrection_;
 	bool processingStatistics_;
 	bool lastOdometryProcessed_;
+	OccupancyGrid grid_;
 };
 
 

examples/RGBDMapping/main.cpp

@@ -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);
@@ -163,9 +172,12 @@ int main(int argc, char * argv[])
 	OdometryThread odomThread(new OdometryF2M());
 
 
+	ParametersMap params;
+	//param.insert(ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // uncomment to create local occupancy grids
+
 	// Create RTAB-Map to process OdometryEvent
 	Rtabmap * rtabmap = new Rtabmap();
-	rtabmap->init();
+	rtabmap->init(params);
 	RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
 
 	// Setup handlers

examples/WifiMapping/MapBuilder.h

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

examples/WifiMapping/MapBuilderWifi.h

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

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -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:

guilib/include/rtabmap/gui/CameraViewer.h

@@ -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);

guilib/include/rtabmap/gui/CloudViewer.h

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