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)
-  - ps: wget 'https://dl.dropboxusercontent.com/s/atf4r8kb1xyc1ls/PCL%201.8.1.exe?dl=0' -outfile PCL_1.8.1.exe
+  # VTK (including QVTK)
+  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
+  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/r9tvi9md54zlul2/PCL-1_8_1-July2018-msvc140.exe?dl=0' -outfile PCL_1.8.1.exe
   - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
   - ECHO "Installed PCL:"
-  - ps: "ls \"C:/Program Files/PCL 1.8.1\""
-  - set PATH=%PATH%;C:\Program Files\PCL 1.8.1\bin
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
   # zlib
   - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
   - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
   - ECHO "Installed zlib:"
   - ps: "ls \"C:/Program Files/zlib\""
   - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
   
 before_build:
   - cd c:\projects\rtabmap\build
   - ECHO %PROGRAMFILES%
   - ECHO %PATH%
-  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL 1.8.1\cmake" -DZLIB_ROOT="C:\Program Files\zlib" ..
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DZLIB_ROOT="C:\Program Files\zlib" -DBUILD_AS_BUNDLE=ON ..
   
 after_build :
   - cmake --build . --config Release --target package

.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

@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 16)
-SET(RTABMAP_PATCH_VERSION 3)
+SET(RTABMAP_MINOR_VERSION 17)
+SET(RTABMAP_PATCH_VERSION 6)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -113,15 +113,13 @@ SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
 SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
 
 ####### INSTALL DIR #######
-set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH
-  "Installation directory for header files")
+set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 if(WIN32 AND NOT CYGWIN)
   set(DEF_INSTALL_CMAKE_DIR CMake)
 else()
   set(DEF_INSTALL_CMAKE_DIR ${CMAKE_INSTALL_LIBDIR}/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 endif()
-set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
-  "Installation directory for CMake files")
+set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR})
   
 ####### BUILD OPTIONS #######
 
@@ -133,9 +131,9 @@ IF(ANDROID_PREBUILD)
    return()
 ENDIF(ANDROID_PREBUILD)
 
-IF(APPLE)
-    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
-ENDIF(APPLE)
+IF(APPLE OR WIN32)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle with all embedded dependencies (DragNDrop for Mac, installer for Windows)" OFF)
+ENDIF(APPLE OR WIN32)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
@@ -157,27 +155,43 @@ option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
+option(WITH_LOAM          "Include LOAM support"                 ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
+option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
+option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
 option(WITH_FOVIS         "Include FOVIS support"                ON)
 option(WITH_VISO2         "Include VISO2 support"                ON)
 option(WITH_DVO           "Include DVO support"                  ON)
 option(WITH_ORB_SLAM2     "Include ORB_SLAM2 support"            ON)
 option(WITH_OKVIS         "Include OKVIS support"                ON)
+option(WITH_MSCKF_VIO     "Include MSCKF_VIO support"            OFF)
 option(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
+
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
 ELSE()
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation )
 ENDIF()
+if("${PCL_DEFINITIONS}" MATCHES "-march=native")
+  MESSAGE(WARNING "PCL definitions contain \"-march=native\", make sure all libraries using Eigen are also compiled with that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+else()
+  MESSAGE(STATUS "PCL definitions don't contain \"-march=native\", make sure all libraries using Eigen are also compiled without that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+endif()
+
 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")
@@ -238,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)
 
@@ -267,12 +286,12 @@ IF(WITH_FREENECT2)
     ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)
 
-IF(WITH_K4W2)
+IF(WITH_K4W2 AND WIN32)
     FIND_PACKAGE(KinectSDK2 QUIET)
 	IF(KinectSDK2_FOUND)
        MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}")
     ENDIF(KinectSDK2_FOUND)
-ENDIF(WITH_K4W2)
+ENDIF(WITH_K4W2 AND WIN32)
 
 # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
 IF(WITH_OPENNI2 OR OpenNI2_FOUND)
@@ -321,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
@@ -365,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)
@@ -382,6 +419,13 @@ IF(WITH_CPUTSDF)
     ENDIF(CPUTSDF_FOUND)
 ENDIF(WITH_CPUTSDF)
 
+IF(WITH_OPENCHISEL)
+    find_package(open_chisel QUIET)
+    if(open_chisel_FOUND)
+        MESSAGE(STATUS "Found open_chisel: ${open_chisel_INCLUDE_DIRS}")
+    endif(open_chisel_FOUND)
+ENDIF(WITH_OPENCHISEL)
+
 IF(WITH_FOVIS)
     FIND_PACKAGE(libfovis QUIET)
     IF(libfovis_FOUND)
@@ -407,7 +451,6 @@ IF(WITH_OKVIS)
     FIND_PACKAGE(okvis 1.1 QUIET)
     IF(okvis_FOUND)
        MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
-       MESSAGE(WARNING "Don't forget to build okvis and its dependencies without \"-march=native\" to avoid crash when okvis starts.")
        find_package(brisk 2 REQUIRED)
        MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
        find_package(opengv REQUIRED)
@@ -417,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)
@@ -429,12 +480,22 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
           MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
           SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
           SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
-          MESSAGE(WARNING "Don't forget to build ORB_SLAM2 (and included g2o) without \"-march=native\" to avoid crash when ORB_SLAM2 starts.")
        ENDIF()
     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)
+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)
@@ -445,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)
+ENDIF()
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -514,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()
@@ -557,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()
@@ -567,6 +636,11 @@ IF(NOT CPUTSDF_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) 
 ENDIF()
+IF(NOT open_chisel_FOUND)
+   SET(OPENCHISEL "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) 
+ENDIF()
 IF(NOT libfovis_FOUND)
    SET(FOVIS "//")
 ELSE()
@@ -587,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()
@@ -602,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 )
@@ -724,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")
@@ -779,27 +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)
@@ -855,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)
@@ -889,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)
@@ -923,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)
@@ -939,6 +1046,14 @@ ELSE()
 MESSAGE(STATUS "  With CPUTSDF              = NO (CPUTSDF not found)")
 ENDIF()
 
+IF(open_chisel_FOUND)
+MESSAGE(STATUS "  With OpenChisel           = YES (License: ???)")
+ELSEIF(NOT WITH_OPENCHISEL)
+MESSAGE(STATUS "  With OpenChisel           = NO (WITH_OPENCHISEL=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
+ENDIF()
+
 IF(libfovis_FOUND)
 MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
 ELSEIF(NOT WITH_FOVIS)
@@ -965,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)

README.md

@@ -7,7 +7,7 @@ rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pix
 [![License][license-image]][license]
 Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
 
-[release-image]: https://img.shields.io/badge/release-0.14.0-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.16.3-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat

Version.h.in

@@ -50,17 +50,21 @@ 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
 @FOVIS@#define RTABMAP_FOVIS
 @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/android/jni/RTABMapApp.cpp

@@ -335,25 +335,18 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	std::map<int, rtabmap::Transform> optPoses;
 	if(!databaseSource.empty())
 	{
 		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
 		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
 		if(driver->openConnection(databaseSource))
 		{
-			cloudMat = driver->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+			cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
 			{
 				LOGI("Open: Found optimized mesh! Visualizing it.");
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
-				if(optPoses.size())
-				{
-					// just take the last as reference
-					optRefId_ = optPoses.rbegin()->first;
-					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
-				}
 				if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
@@ -592,6 +585,14 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 
 	rtabmap_->setOptimizedPoses(poses);
 
+	// for optimized mesh
+	if(poses.size())
+	{
+		// just take the last as reference
+		optRefId_ = poses.rbegin()->first;
+		optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
+	}
+
 	if(camera_)
 	{
 		camera_->resetOrigin();
@@ -2713,7 +2714,7 @@ bool RTABMapApp::exportMesh(
 						}
 						boost::mutex::scoped_lock  lock(rtabmapMutex_);
 
-						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons);
+						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons);
 						success = true;
 					}
 				}
@@ -2734,7 +2735,7 @@ bool RTABMapApp::exportMesh(
 						}
 					}
 					boost::mutex::scoped_lock  lock(rtabmapMutex_);
-					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons, textureMesh->tex_coordinates, globalTextures);
+					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons, textureMesh->tex_coordinates, globalTextures);
 					success = true;
 				}
 				else
@@ -2857,7 +2858,7 @@ bool RTABMapApp::exportMesh(
 				{
 					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds)); // for database
 					boost::mutex::scoped_lock  lock(rtabmapMutex_);
-					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses);
+					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat);
 					success = true;
 				}
 			}
@@ -2887,6 +2888,20 @@ bool RTABMapApp::exportMesh(
 	}
 	exporting_ = false;
 
+	optRefId_ = 0;
+	if(optRefPose_)
+	{
+		delete optRefPose_;
+		optRefPose_ = 0;
+	}
+	if(success && poses.size())
+	{
+		// for optimized mesh
+		// just take the last as reference
+		optRefId_ = poses.rbegin()->first;
+		optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
+	}
+
 	return success;
 }
 
@@ -2895,12 +2910,6 @@ bool RTABMapApp::postExportation(bool visualize)
 	LOGI("postExportation(visualize=%d)", visualize?1:0);
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
-	optRefId_ = 0;
-	if(optRefPose_)
-	{
-		delete optRefPose_;
-		optRefPose_ = 0;
-	}
 	exportedMeshUpdated_ = false;
 
 	if(visualize)
@@ -2914,23 +2923,15 @@ bool RTABMapApp::postExportation(bool visualize)
 		std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 		cv::Mat textures;
-		std::map<int, rtabmap::Transform> optPoses;
 		if(rtabmap_ && rtabmap_->getMemory())
 		{
-			cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+			cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
 			{
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
 
-				if(optPoses.size())
-				{
-					// just take the last as reference
-					optRefId_ = optPoses.rbegin()->first;
-					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
-				}
-
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
 				exportedMeshUpdated_ = true;
@@ -2975,10 +2976,9 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	std::map<int, rtabmap::Transform> optPoses;
 	if(rtabmap_ && rtabmap_->getMemory())
 	{
-		cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+		cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
 		if(!cloudMat.empty())
 		{
 			LOGI("writeExportedMesh: Found optimized mesh!");

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/FindKinectSDK2.cmake

@@ -180,4 +180,4 @@ if(KinectSDK2_FOUND)
   endif()
 endif()
 
-message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")
+message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")

cmake_modules/FindRealSense2.cmake

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

cmake_modules/FindSqlite3.cmake

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

corelib/include/rtabmap/core/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

@@ -102,9 +102,12 @@ public:
 	void addStatistics(const Statistics & statistics) const;
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
@@ -113,7 +116,6 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
 	cv::Mat loadOptimizedMesh(
-			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
@@ -153,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);
@@ -177,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;
@@ -230,9 +231,12 @@ private:
 	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const = 0;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(
 				const cv::Mat & cloud,
-				const std::map<int, Transform> & poses,
 				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -241,7 +245,6 @@ private:
 #endif
 				const cv::Mat & textures) const = 0;
 	virtual cv::Mat loadOptimizedMeshQuery(
-				std::map<int, Transform> * poses,
 				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
@@ -251,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;
@@ -101,9 +103,12 @@ private:
 	virtual void addStatisticsQuery(const Statistics & statistics) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
 	virtual void saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -112,7 +117,6 @@ private:
 #endif
 			const cv::Mat & textures) const;
 	virtual cv::Mat loadOptimizedMeshQuery(
-			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
@@ -122,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;
@@ -173,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

@@ -95,9 +95,12 @@ public:
 	void saveStatistics(const Statistics & statistics);
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
@@ -106,7 +109,6 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
 	cv::Mat loadOptimizedMesh(
-			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
@@ -169,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;
@@ -288,8 +291,9 @@ private:
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
-	int _laserScanNormalRadius;
+	float _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
+	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
@@ -298,6 +302,8 @@ private:
 	int _visMaxFeatures;
 	int _visCorType;
 	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
+	bool _covOffDiagonalIgnored;
 
 	int _idCount;
 	int _idMapCount;
@@ -307,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,9 +39,21 @@ namespace rtabmap {
 
 class RTABMAP_EXP OccupancyGrid
 {
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
+	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
 	void setCloudAssembling(bool enabled);
@@ -52,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(
@@ -86,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_;}
@@ -118,13 +132,17 @@ private:
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;
-	double scan2dMaxUnknownSpaceFilledRange_;
 	bool rayTracing_;
 	bool fullUpdate_;
 	float minMapSize_;
 	bool erode_;
 	float footprintRadius_;
 	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;
 
 	std::map<int, std::pair<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

@@ -52,11 +52,15 @@ private:
 
 private:
 	std::string configFilename_;
+#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,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				cv::Mat & outputCovariance,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
 			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,

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

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, MemoryThr,                    int, 0,      "Maximum signatures in the Working Memory (ms) (0 means infinity).");
+    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for map update (ms) (0 means infinity). When map update time exceeds this fixed time threshold, some nodes in Working Memory (WM) are transferred to Long-Term Memory to limit the size of the WM and decrease the update time.");
+    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      uFormat("Maximum nodes in the Working Memory (0 means infinity). Similar to \"%s\", when the number of nodes in Working Memory (WM) exceeds this treshold, some nodes are transferred to Long-Term Memory to keep WM size fixed.", kRtabmapTimeThr().c_str()));
     RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
     RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
     RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
@@ -186,7 +186,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
     RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
     RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
+    RTABMAP_PARAM(Rtabmap, StartNewMapOnGoodSignature,   bool, false, uFormat("Start a new map only if the first signature is not bad (i.e., has enough features, see %s).", kKpBadSignRatio().c_str()));
     RTABMAP_PARAM(Rtabmap, ImagesAlreadyRectified,       bool, true,  "Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.");
+    RTABMAP_PARAM(Rtabmap, RectifyOnlyFeatures,          bool, false,  uFormat("If \"%s\" is false and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.", kRtabmapImagesAlreadyRectified().c_str()));
 
     // Hypotheses selection
     RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
@@ -218,8 +220,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
     RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
     RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       float, 0.0,     "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
     RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features.");
+    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
 
     // KeypointMemory (Keypoint-based)
     RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -282,8 +285,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, "Used with FAST GPU.");
     RTABMAP_PARAM(FAST, MinThreshold,       int, 7,       "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
     RTABMAP_PARAM(FAST, MaxThreshold,       int, 200,     "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
-    RTABMAP_PARAM(FAST, GridRows,           int, 4,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
-    RTABMAP_PARAM(FAST, GridCols,           int, 4,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridRows,           int, 0,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridCols,           int, 0,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
 
     RTABMAP_PARAM(GFTT, QualityLevel,      double, 0.001, "");
     RTABMAP_PARAM(GFTT, MinDistance,       double, 3,    "");
@@ -336,6 +339,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, SavedLocalizationIgnored, bool, false, "Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.");
     RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
     RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
     RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
@@ -347,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
@@ -392,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).");
@@ -489,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");
@@ -550,11 +591,19 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
     RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.0,   "Uniform sampling voxel size (0=disabled).");
     RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1,       "Downsampling step size (1=no sampling). This is done before uniform sampling.");
+#ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1,   "Max distance for point correspondences.");
+#else
+    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
+#endif
     RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
     RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
     RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+#else
     RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
+#endif
     RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
     RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
     RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
@@ -621,8 +670,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
     RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
-    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  "Unknown space filled. Only used with 2D laser scans.");
-    RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,       float,   4.0,    "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
+    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
     RTABMAP_PARAM(Grid, RayTracing,                 bool,   false,   uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
 
     RTABMAP_PARAM(GridGlobal, FullUpdate,           bool,   true,    "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
@@ -631,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

@@ -129,6 +129,7 @@ public:
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 
+	void setInitialPose(const Transform & initialPose);
 	int triggerNewMap();
 	bool labelLocation(int id, const std::string & label);
 	/**
@@ -152,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,
@@ -170,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;
@@ -189,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;
@@ -197,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;
@@ -246,13 +250,15 @@ private:
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
-	float _optimizationMaxLinearError;
+	float _optimizationMaxError;
 	bool _startNewMapOnLoopClosure;
+	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters
 	bool _goalsSavedInUserData;
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
+	bool _savedLocalizationIgnored;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

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/include/rtabmap/core/util3d.h

@@ -228,6 +228,7 @@ cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXY
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 
+pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform());
 // For 2d laserScan, z is set to null.
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform());
 // For laserScan without normals, normals are set to null.

corelib/include/rtabmap/core/util3d_mapping.h

@@ -114,7 +114,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 		cv::Mat & grid,
 		bool stopOnObstacle);
 
-cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
+cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat = false);
+cv::Mat RTABMAP_EXP convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat = false);
 
 cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map);
 

corelib/include/rtabmap/core/util3d_surface.h

@@ -175,6 +175,15 @@ pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
 void RTABMAP_EXP concatenateTextureMaterials(
 		pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);
 
+std::vector<std::vector<unsigned int> > RTABMAP_EXP convertPolygonsFromPCL(
+		const std::vector<pcl::Vertices> & polygons);
+std::vector<std::vector<std::vector<unsigned int> > > RTABMAP_EXP convertPolygonsFromPCL(
+		const std::vector<std::vector<pcl::Vertices> > & polygons);
+std::vector<pcl::Vertices> RTABMAP_EXP convertPolygonsToPCL(
+		const std::vector<std::vector<unsigned int> > & polygons);
+std::vector<std::vector<pcl::Vertices> > RTABMAP_EXP convertPolygonsToPCL(
+		const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons);
+
 pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(
 		const cv::Mat & cloudMat,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
@@ -194,6 +203,24 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(
  * Merge all textures in the mesh into "textureCount" textures of size "textureSize".
  * @return merged textures corresponding to new materials set in TextureMesh (height=textureSize, width=textureSize*materials)
  */
+cv::Mat RTABMAP_EXP mergeTextures(
+		pcl::TextureMesh & mesh,
+		const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
+		const std::map<int, CameraModel> & calibrations, // Should match images
+		const Memory * memory = 0,             // Should be set if images are not set
+		const DBDriver * dbDriver = 0,         // Should be set if images and memory are not set
+		int textureSize = 4096,
+		int textureCount = 1,
+		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels = std::vector<std::map<int, pcl::PointXY> >(), // needed for parameters below
+		bool gainCompensation = true,
+		float gainBeta = 10.0f,
+		bool gainRGB = true,                 //Do gain compensation on each channel
+		bool blending = true,
+		int blendingDecimation = 0,          //0=auto depending on projected polygon size and texture size
+		int brightnessContrastRatioLow = 0,  //0=disabled, values between 0 and 100
+		int brightnessContrastRatioHigh = 0, //0=disabled, values between 0 and 100
+		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
+		const ProgressState * state = 0);
 cv::Mat RTABMAP_EXP mergeTextures(
 		pcl::TextureMesh & mesh,
 		const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
@@ -213,6 +240,8 @@ cv::Mat RTABMAP_EXP mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0);
 
+void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh);
+
 cv::Mat RTABMAP_EXP computeNormals(
 		const cv::Mat & laserScan,
 		int searchK,

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
-	
-	sqlite3/sqlite3.c	
+	FlannIndex.cpp	
 	
 	#clams stuff
 	clams/discrete_depth_distortion_model_helpers.cpp
@@ -96,9 +96,12 @@ IF(OpenCV_VERSION_MAJOR EQUAL 2)
 SET(SRC_FILES
     ${SRC_FILES}
     opencv/Orb.cpp
-    opencv/solvepnp.cpp
 )
 ENDIF(OpenCV_VERSION_MAJOR EQUAL 2)
+SET(SRC_FILES
+    ${SRC_FILES}
+    opencv/solvepnp.cpp
+)
 
 # to get includes in visual studio
 IF(MSVC)
@@ -124,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")
@@ -182,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}
@@ -238,25 +279,6 @@ IF(G2O_FOUND)
 	ENDIF(WITH_VERTIGO)
 ENDIF(G2O_FOUND)
 
-IF(GTSAM_FOUND)
-	IF(GTSAM_INCLUDE_DIR)
-		SET(INCLUDE_DIRS 
-			${GTSAM_INCLUDE_DIR} # place it in front to use Eigen installed by GTSAM
-			${INCLUDE_DIRS} 
-		)
-	ELSE()
-		SET(INCLUDE_DIRS 
-			${GTSAM_INCLUDE_DIRS} # cmake standard
-			${INCLUDE_DIRS} 
-		)
-	ENDIF()
-	add_definitions("-DGTSAM_IMPORT_STATIC")
-	SET(LIBRARIES
-		${LIBRARIES}
-		gtsam
-	)
-ENDIF(GTSAM_FOUND)
-
 IF(cvsba_FOUND)
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
@@ -279,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}
@@ -365,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
@@ -376,6 +420,29 @@ IF(ORB_SLAM2_FOUND)
 	)
 ENDIF(ORB_SLAM2_FOUND)
 
+IF(GTSAM_FOUND)
+	# Make sure GTSAM is built with system Eigen, not the included one in its package
+	IF(GTSAM_INCLUDE_DIR)
+		SET(INCLUDE_DIRS 
+			${INCLUDE_DIRS} 
+			${GTSAM_INCLUDE_DIR} 
+		)
+	ELSE()
+		SET(INCLUDE_DIRS 
+			${INCLUDE_DIRS} 
+			${GTSAM_INCLUDE_DIRS}
+		)
+	ENDIF()
+	IF(WIN32)
+		# GTSAM should be built in STATIC on Windows to avoid "error C2338: THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS" when building GTSAM
+		add_definitions("-DGTSAM_IMPORT_STATIC") 
+	ENDIF(WIN32)
+	SET(LIBRARIES
+		${LIBRARIES}
+		gtsam # Windows: Place static libs at the end
+	)
+ENDIF(GTSAM_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

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 << "cols" << D_.cols;
-			fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
+			fs << "rows" << D.rows;
+			fs << "cols" << D.cols;
+			fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
 			fs << "}";
 
 			// compaibility with ROS
-			if(D_.cols > 5)
+			if(D_.cols == 6)
 			{
-				fs << "distortion_model" << "rational_polynomial";
+				fs << "distortion_model" << "equidistant"; // equidistant, fisheye
+			}
+			else if(D.cols > 5)
+			{
+				fs << "distortion_model" << "rational_polynomial"; // rad tan
 			}
 			else
 			{
-				fs << "distortion_model" << "plumb_bob";
+				fs << "distortion_model" << "plumb_bob"; // rad tan
 			}
 		}
 

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;
-	}
-	if(_scanDir)
-	{
-		delete _scanDir;
-	}
+	delete _dir;
+	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);
-				int trackingConfidence = pose.pose_confidence;
-				// FIXME What does pose_confidence == -1 mean?
-				if (trackingConfidence>0)
+				sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
+				if (tracking_state == sl::TRACKING_STATE_OK)
 				{
-					info->odomPose = zedPoseToTransform(pose);
-					if (!info->odomPose.isNull())
+					int trackingConfidence = pose.pose_confidence;
+					// FIXME What does pose_confidence == -1 mean?
+					if (trackingConfidence>0)
 					{
-						//transform x->forward, y->left, z->up
-						Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
-						info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
-
-						if (lost_)
+						info->odomPose = zedPoseToTransform(pose);
+						if (!info->odomPose.isNull())
 						{
-							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
-							lost_ = false;
-							UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+							//transform x->forward, y->left, z->up
+							Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+							info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
+
+							if (lost_)
+							{
+								info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+								lost_ = false;
+								UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+							}
+							else
+							{
+								info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+								UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+							}
 						}
 						else
 						{
-							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
-							UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+							lost_ = true;
+							UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
 						}
 					}
 					else
@@ -1065,9 +1081,7 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 				}
 				else
 				{
-					info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
-					lost_ = true;
-					UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
+					UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
 				}
 			}
 		}
@@ -1135,10 +1149,7 @@ CameraStereoImages::CameraStereoImages(
 CameraStereoImages::~CameraStereoImages()
 {
 	UDEBUG("");
-	if(camera2_)
-	{
-		delete camera2_;
-	}
+	delete camera2_;
 	UDEBUG("");
 }
 
@@ -1279,7 +1290,8 @@ CameraStereoVideo::CameraStereoVideo(
 		path_(path),
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
-		usbDevice_(0)
+		usbDevice_(0),
+		usbDevice2_(-1)
 {
 }
 
@@ -1294,7 +1306,8 @@ CameraStereoVideo::CameraStereoVideo(
 		path2_(pathRight),
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
-		usbDevice_(0)
+		usbDevice_(0),
+		usbDevice2_(-1)
 {
 }
 
@@ -1306,7 +1319,22 @@ CameraStereoVideo::CameraStereoVideo(
 	Camera(imageRate, localTransform),
 	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
-	usbDevice_(device)
+	usbDevice_(device),
+	usbDevice2_(-1)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int deviceLeft,
+	int deviceRight,
+	bool rectifyImages,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	rectifyImages_(rectifyImages),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(deviceLeft),
+	usbDevice2_(deviceRight)
 {
 }
 
@@ -1330,20 +1358,27 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 
 	if (src_ == CameraVideo::kUsbDevice)
 	{
-		ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
 		capture_.open(usbDevice_);
+		if(usbDevice2_ < 0)
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		}
+		else
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on devices %d and %d", usbDevice_, usbDevice2_);
+			capture2_.open(usbDevice2_);
+		}
 	}
 	else if (src_ == CameraVideo::kVideoFile)
 	{
+		capture_.open(path_.c_str());
 		if(path2_.empty())
 		{
 			ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
-			capture_.open(path_.c_str());
 		}
 		else
 		{
 			ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
-			capture_.open(path_.c_str());
 			capture2_.open(path2_.c_str());
 		}
 	}
@@ -1352,7 +1387,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
 	}
 
-	if(!capture_.isOpened() || (!path2_.empty() && !capture2_.isOpened()))
+	if(!capture_.isOpened() || ((!path2_.empty() || usbDevice2_>=0) && !capture2_.isOpened()))
 	{
 		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
@@ -1411,11 +1446,11 @@ SensorData CameraStereoVideo::captureImage(CameraInfo * info)
 	SensorData data;
 
 	cv::Mat img;
-	if(capture_.isOpened() && (path2_.empty() || capture2_.isOpened()))
+	if(capture_.isOpened() && ((path2_.empty() && usbDevice2_ < 0) || capture2_.isOpened()))
 	{
 		cv::Mat leftImage;
 		cv::Mat rightImage;
-		if(path2_.empty())
+		if(path2_.empty() && usbDevice2_ < 0)
 		{
 			if(!capture_.read(img))
 			{

corelib/src/CameraThread.cpp

@@ -73,14 +73,8 @@ CameraThread::~CameraThread()
 {
 	UDEBUG("");
 	join(true);
-	if(_camera)
-	{
-		delete _camera;
-	}
-	if(_distortionModel)
-	{
-		delete _distortionModel;
-	}
+	delete _camera;
+	delete _distortionModel;
 	delete _stereoDense;
 }
 
@@ -325,7 +319,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		!data.depthRaw().empty())
 	{
 		UDEBUG("");
-		if(data.laserScanRaw().size())
+		if(data.laserScanRaw().isEmpty())
 		{
 			UASSERT(_scanDecimation >= 1);
 			UTimer timer;

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();
 }
 
@@ -1061,9 +1061,37 @@ cv::Mat DBDriver::loadPreviewImage() const
 	return image;
 }
 
+void DBDriver::saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const
+{
+	_dbSafeAccessMutex.lock();
+	saveOptimizedPosesQuery(optimizedPoses, lastlocalizationPose);
+	_dbSafeAccessMutex.unlock();
+}
+std::map<int, Transform> DBDriver::loadOptimizedPoses(Transform * lastlocalizationPose) const
+{
+	_dbSafeAccessMutex.lock();
+	std::map<int, Transform> poses = loadOptimizedPosesQuery(lastlocalizationPose);
+	_dbSafeAccessMutex.unlock();
+	return poses;
+}
+
+void DBDriver::save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const
+{
+	_dbSafeAccessMutex.lock();
+	save2DMapQuery(map, xMin, yMin, cellSize);
+	_dbSafeAccessMutex.unlock();
+}
+
+cv::Mat DBDriver::load2DMap(float & xMin, float & yMin, float & cellSize) const
+{
+	_dbSafeAccessMutex.lock();
+	cv::Mat map = load2DMapQuery(xMin, yMin, cellSize);
+	_dbSafeAccessMutex.unlock();
+	return map;
+}
+
 void DBDriver::saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -1073,12 +1101,11 @@ void DBDriver::saveOptimizedMesh(
 			const cv::Mat & textures) const
 {
 	_dbSafeAccessMutex.lock();
-	saveOptimizedMeshQuery(cloud, poses, polygons, texCoords, textures);
+	saveOptimizedMeshQuery(cloud, polygons, texCoords, textures);
 	_dbSafeAccessMutex.unlock();
 }
 
 cv::Mat DBDriver::loadOptimizedMesh(
-				std::map<int, Transform> * poses,
 				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
@@ -1088,7 +1115,7 @@ cv::Mat DBDriver::loadOptimizedMesh(
 				cv::Mat * textures) const
 {
 	_dbSafeAccessMutex.lock();
-	cv::Mat cloud = loadOptimizedMeshQuery(poses, polygons, texCoords, textures);
+	cv::Mat cloud = loadOptimizedMeshQuery(polygons, texCoords, textures);
 	_dbSafeAccessMutex.unlock();
 	return cloud;
 }

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 "
-					"FROM Word "
-					"WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) "
-					"ORDER BY id;";
-		}
-		else
-		{
-			query = "SELECT id, descriptor_size, descriptor "
-					"FROM Word "
-					"WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
-					"ORDER BY id;";
+			if(uStrNumCmp(_version, "0.11.11") >= 0)
+			{
+				query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) ";
+			}
+			else
+			{
+				query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) ";
+			}
 		}
+		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());
@@ -4147,16 +4147,19 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 				}
 
 				cv::Mat compressedWmState;
-				if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0 && !statistics.wmState().empty())
+				if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
 				{
-					compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
-					rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
-					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				}
-				else
-				{
-					rc = sqlite3_bind_null(ppStmt, index++);
-					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					if(!statistics.wmState().empty())
+					{
+						compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
+						rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					}
+					else
+					{
+						rc = sqlite3_bind_null(ppStmt, index++);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					}
 				}
 
 				//step
@@ -4277,9 +4280,274 @@ cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
 	return image;
 }
 
+void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
+{
+	UDEBUG("");
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::string query;
+
+		// Update table Admin
+		query = uFormat("UPDATE Admin SET opt_ids=?, opt_poses=?, opt_last_localization=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		int index = 1;
+
+		// opt ids and poses
+		cv::Mat compressedIds;
+		cv::Mat compressedPoses;
+		if(poses.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			std::vector<int> serializedIds(poses.size());
+			std::vector<float> serializedPoses(poses.size()*12);
+			int i=0;
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				serializedIds[i] = iter->first;
+				memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
+				++i;
+			}
+
+			compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
+			compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
+
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		if(lastlocalizationPose.isNull())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			UDEBUG("lastlocalizationPose=%s", lastlocalizationPose.prettyPrint().c_str());
+			rc = sqlite3_bind_blob(ppStmt, index++, lastlocalizationPose.data(), lastlocalizationPose.size()*sizeof(float), SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		//execute query
+		rc=sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
+std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * lastlocalizationPose) const
+{
+	UDEBUG("");
+	std::map<int, Transform> poses;
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT opt_ids, opt_poses, opt_last_localization "
+			  << "FROM Admin "
+			  << "WHERE version='" << _version.c_str()
+			  <<"';";
+
+		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
+		rc = sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
+		if(rc == SQLITE_ROW)
+		{
+			const void * data = 0;
+			int dataSize = 0;
+			int index = 0;
+
+			//opt_poses
+			cv::Mat serializedIds;
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("serializedIds=%d", serializedIds.cols);
+			}
+
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("serializedPoses=%d", serializedPoses.cols);
+
+				UASSERT(serializedIds.cols == serializedPoses.cols/12);
+				UASSERT(serializedPoses.type() == CV_32FC1);
+				UASSERT(serializedIds.type() == CV_32SC1);
+				for(int i=0; i<serializedIds.cols; ++i)
+				{
+					Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
+							serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
+							serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
+					poses.insert(std::make_pair(serializedIds.at<int>(i), t));
+					UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
+				}
+			}
+
+			data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(lastlocalizationPose)
+			{
+				if((unsigned int)dataSize == lastlocalizationPose->size()*sizeof(float) && data)
+				{
+					memcpy(lastlocalizationPose->data(), data, dataSize);
+				}
+				UDEBUG("lastlocalizationPose=%s", lastlocalizationPose->prettyPrint().c_str());
+			}
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+
+	}
+	return poses;
+}
+
+void DBDriverSqlite3::save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const
+{
+	UDEBUG("");
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::string query;
+
+		// Update table Admin
+		query = uFormat("UPDATE Admin SET opt_map=?, opt_map_x_min=?, opt_map_y_min=?, opt_map_resolution=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		int index = 1;
+
+		// opt ids and poses
+		cv::Mat compressedMap;
+		if(map.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			compressedMap = compressData2(map);
+
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedMap.data, compressedMap.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		rc = sqlite3_bind_double(ppStmt, index++, xMin);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, yMin);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, cellSize);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		//execute query
+		rc=sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
+cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cellSize) const
+{
+	UDEBUG("");
+	cv::Mat map;
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT  opt_map, opt_map_x_min, opt_map_y_min, opt_map_resolution "
+			  << "FROM Admin "
+			  << "WHERE version='" << _version.c_str()
+			  <<"';";
+
+		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
+		rc = sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
+		if(rc == SQLITE_ROW)
+		{
+			const void * data = 0;
+			int dataSize = 0;
+			int index = 0;
+
+			//opt_map
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				map = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("map=%d/%d", map.cols, map.rows);
+			}
+
+			xMin = sqlite3_column_double(ppStmt, index++);
+			UDEBUG("xMin=%f", xMin);
+			yMin = sqlite3_column_double(ppStmt, index++);
+			UDEBUG("yMin=%f", yMin);
+			cellSize = sqlite3_column_double(ppStmt, index++);
+			UDEBUG("cellSize=%f", cellSize);
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+
+	}
+	return map;
+}
+
 void DBDriverSqlite3::saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -4298,14 +4566,14 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 		std::string query;
 
 		// Update table Admin
-		query = uFormat("UPDATE Admin SET opt_cloud=?, opt_ids=?, opt_poses=?, opt_polygons_size=?, opt_polygons=?, opt_tex_coords=?, opt_tex_materials=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
+		query = uFormat("UPDATE Admin SET opt_cloud=?, opt_polygons_size=?, opt_polygons=?, opt_tex_coords=?, opt_tex_materials=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
 		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 		if(cloud.empty())
 		{
 			// set all fields to null
-			for(int i=1; i<=7; ++i)
+			for(int i=1; i<=5; ++i)
 			{
 				rc = sqlite3_bind_null(ppStmt, i);
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4336,39 +4604,9 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 			// opt ids and poses
-			cv::Mat compressedIds;
-			cv::Mat compressedPoses;
 			cv::Mat compressedPolygons;
 			cv::Mat compressedTexCoords;
 			cv::Mat compressedTextures;
-			if(poses.empty())
-			{
-				rc = sqlite3_bind_null(ppStmt, index++);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				rc = sqlite3_bind_null(ppStmt, index++);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-			}
-			else
-			{
-				std::vector<int> serializedIds(poses.size());
-				std::vector<float> serializedPoses(poses.size()*12);
-				int i=0;
-				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					serializedIds[i] = iter->first;
-					memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
-					++i;
-				}
-
-				compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
-				compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
-
-				rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-			}
-
 			// polygons
 			if(polygons.empty())
 			{
@@ -4505,7 +4743,6 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 }
 
 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
-			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
@@ -4524,7 +4761,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		query << "SELECT opt_cloud, opt_ids, opt_poses, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
+		query << "SELECT opt_cloud, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
 			  << "FROM Admin "
 			  << "WHERE version='" << _version.c_str()
 			  <<"';";
@@ -4550,39 +4787,6 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 			}
 			UDEBUG("Cloud=%d points", cloud.cols);
 
-			//opt_poses
-			cv::Mat serializedIds;
-			data = sqlite3_column_blob(ppStmt, index);
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-			if(dataSize>0 && data)
-			{
-				serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
-				UDEBUG("serializedIds=%d", serializedIds.cols);
-			}
-
-			data = sqlite3_column_blob(ppStmt, index);
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-			if(dataSize>0 && data)
-			{
-				cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
-				UDEBUG("serializedPoses=%d", serializedPoses.cols);
-
-				if(poses)
-				{
-					UASSERT(serializedIds.cols == serializedPoses.cols/12);
-					UASSERT(serializedPoses.type() == CV_32FC1);
-					UASSERT(serializedIds.type() == CV_32SC1);
-					for(int i=0; i<serializedIds.cols; ++i)
-					{
-						Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
-								serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
-								serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
-						poses->insert(std::make_pair(serializedIds.at<int>(i), t));
-						UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
-					}
-				}
-			}
-
 			//opt_polygons_size
 			int polygonSize = sqlite3_column_int(ppStmt, index++);
 			UDEBUG("polygonSize=%d", polygonSize);

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/OccupancyGrid.cpp

@@ -68,13 +68,17 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
-	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
 	rayTracing_(Parameters::defaultGridRayTracing()),
 	fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
 	minMapSize_(Parameters::defaultGridGlobalMinSize()),
 	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),
@@ -125,7 +129,6 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
-	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
 	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
@@ -133,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
@@ -197,6 +221,40 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	}
 }
 
+void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses)
+{
+	UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d",
+			map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size());
+	this->clear();
+	if(!poses.empty() && !map.empty())
+	{
+		UASSERT(cellSize > 0.0f);
+		UASSERT(map.type() == CV_8SC1);
+		map_ = map.clone();
+		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;
+		addedNodes_ = poses;
+	}
+}
+
 void OccupancyGrid::setCellSize(float cellSize)
 {
 	UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str());
@@ -241,11 +299,20 @@ void OccupancyGrid::createLocalMap(
 				node.sensorData().laserScanRaw().localTransform().z());
 
 		LaserScan scan = node.sensorData().laserScanRaw();
-		if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
+		if(cloudMinDepth_ > 0.0f)
 		{
-			scan = util3d::rangeFiltering(scan, cloudMinDepth_, cloudMaxDepth_);
+			scan = util3d::rangeFiltering(scan, cloudMinDepth_, 0.0f);
 		}
 
+		float maxRange = cloudMaxDepth_;
+		if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().maxRange()>0.0f)
+		{
+			maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().maxRange()?cloudMaxDepth_:node.sensorData().laserScanRaw().maxRange();
+		}
+		else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().maxRange()>0.0f)
+		{
+			maxRange = node.sensorData().laserScanRaw().maxRange();
+		}
 		util3d::occupancy2DFromLaserScan(
 				util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
 				cv::Mat(),
@@ -254,7 +321,7 @@ void OccupancyGrid::createLocalMap(
 				obstacleCells,
 				cellSize_,
 				scan2dUnknownSpaceFilled_,
-				node.sensorData().laserScanRaw().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanRaw().maxRange());
+				maxRange);
 
 		UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
 	}
@@ -515,7 +582,7 @@ void OccupancyGrid::createLocalMap(
 					obstacleCells,
 					cellSize_,
 					false, // don't fill unknown space
-					0);
+					cloudMaxDepth_);
 		}
 	}
 	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
@@ -538,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(
@@ -761,6 +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 will be added to map", iter->first);
 			poses.push_back(*iter);
 		}
 	}
@@ -812,6 +940,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 			{
 				const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);
 
+				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
+
 				//ground
 				if(pair.first.first.cols)
 				{
@@ -968,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
 			{
@@ -1082,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_;
+								}
+							}
 						}
 					}
 				}
@@ -1192,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();
-	Parameters::parse(parameters, Parameters::kGridGlobalOctoMapOccupancyThr(), occupancyThr);
+	float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	float probHit = Parameters::defaultGridGlobalProbHit();
+	float probMiss = Parameters::defaultGridGlobalProbMiss();
+	float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
+	float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);
 
 	octree_ = new RtabmapColorOcTree(cellSize);
+	if(occupancyThr <= 0.0f)
+	{
+		UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
+				Parameters::kGridGlobalOccupancyThr().c_str(),
+				Parameters::defaultGridGlobalOccupancyThr());
+		occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	}
 	octree_->setOccupancyThres(occupancyThr);
+	octree_->setProbHit(probHit);
+	octree_->setProbMiss(probMiss);
+	octree_->setClampingThresMin(clampingMin);
+	octree_->setClampingThresMax(clampingMax);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);

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

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 = this->getPose().inverse() * t;
+			t = previousPoseInv*p;
 		}
+		previousPose_ = p;
 
 		if(firstFrame_)
 		{
@@ -1003,10 +1010,12 @@ Transform OdometryORBSLAM2::computeTransform(
 			info->reg.matches = oi;
 
 			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
+			Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)
 			{
-				cv::Mat pt = mapPoints[i]->GetWorldPos();
-				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_)));
+				cv::Point3f pt(mapPoints[i]->GetWorldPos());
+				pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt.x, pt.y, pt.z), fixRot);
+				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
 			}
 		}
 	}

corelib/src/OdometryOkvis.cpp

@@ -130,12 +130,12 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
 	Odometry(parameters),
 #ifdef RTABMAP_OKVIS
 	okvisCallbackHandler_(new OkvisCallbackHandler),
-#else
-	okvisCallbackHandler_(0),
-#endif
 	okvisEstimator_(0),
+	imagesProcessed_(0),
+	initGravity_(false),
+#endif
 	okvisParameters_(parameters),
-	imagesProcessed_(0)
+	previousPose_(Transform::getIdentity())
 {
 #ifdef RTABMAP_OKVIS
 	Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
@@ -150,10 +150,7 @@ OdometryOkvis::~OdometryOkvis()
 {
 	UDEBUG("");
 #ifdef RTABMAP_OKVIS
-	if(okvisEstimator_)
-	{
-		delete okvisEstimator_;
-	}
+	delete okvisEstimator_;
 	delete okvisCallbackHandler_;
 #endif
 }
@@ -162,17 +159,22 @@ void OdometryOkvis::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
 #ifdef RTABMAP_OKVIS
-	if(okvisEstimator_)
+	if(!initGravity_)
 	{
-		delete okvisEstimator_;
-		okvisEstimator_ = 0;
-	}
-	lastImu_ = IMU();
+		if(okvisEstimator_)
+		{
+			delete okvisEstimator_;
+			okvisEstimator_ = 0;
+		}
+		lastImu_ = IMU();
+		imagesProcessed_ = 0;
+		previousPose_.setIdentity();
 
-	delete okvisCallbackHandler_;
-	okvisCallbackHandler_ = new OkvisCallbackHandler();
+		delete okvisCallbackHandler_;
+		okvisCallbackHandler_ = new OkvisCallbackHandler();
+	}
+	initGravity_ = false;
 #endif
-	imagesProcessed_ = 0;
 }
 
 // return not null transform if odometry is correctly computed
@@ -306,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;
@@ -453,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)
 			{
@@ -470,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())
+		if(!data.imageRaw().empty() || (pose.isNull() && data.imu().empty()))
 		{
+			UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
 			// a null pose notify that odometry could not be computed
 			this->post(new OdometryEvent(data, pose, info));
 		}
@@ -196,7 +193,7 @@ bool OdometryThread::getData(SensorData & data)
 		if(!_dataBuffer.empty() || !_imuBuffer.empty())
 		{
 			if(_dataBuffer.empty() ||
-				(!_dataBuffer.empty() && !_imuBuffer.empty() && _imuBuffer.front().stamp() <= _dataBuffer.front().stamp()))
+				(!_dataBuffer.empty() && !_imuBuffer.empty() && _imuBuffer.front().stamp() < _dataBuffer.front().stamp()))
 			{
 				data = _imuBuffer.front();
 				_imuBuffer.pop_front();

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

@@ -279,11 +279,14 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
 	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 	{
+		incGraph.insert(*iter);
 		bool hasLoopClosure = false;
 		for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
 		{
+			UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
 			if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
 			{
+				UASSERT(uContains(incGraph, iter->first));
 				incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
 				incGraphLinks.insert(*jter);
 			}
@@ -316,6 +319,8 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	if(!incGraph.empty() && incGraph.size() == poses.size())
 	{
 		UASSERT(incGraphLinks.size() == constraints.size());
+		UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
+		incGraph.at(rootId) = poses.at(rootId);
 		return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
 	}
 
@@ -323,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,12 @@ 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())));
+
 		// 0.16.0
 		removedParameters_.insert(std::make_pair("Grid/ProjRayTracing",           std::make_pair(true,  Parameters::kGridRayTracing())));
 		removedParameters_.insert(std::make_pair("Grid/DepthMin",                 std::make_pair(true,  Parameters::kGridRangeMin())));
@@ -510,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"
@@ -522,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"
 			;
 }
 
@@ -541,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);
 			}
@@ -675,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)
 					{
@@ -792,17 +994,15 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 				key = uReplaceChar(key, '\\', '/'); // Ini files use \ by default for separators, so replace them
 
 				// look for old parameter name
-				bool addParameter = true;
 				std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
 				if(oldIter!=Parameters::getRemovedParameters().end())
 				{
-					addParameter = oldIter->second.first;
-					if(addParameter)
+					if(oldIter->second.first)
 					{
 						if(parameters.find(oldIter->second.second) == parameters.end())
 						{
 							key = oldIter->second.second;
-							UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
+							UINFO("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
 									oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second, Parameters::getDefaultParameters().at(oldIter->second.second).c_str());
 						}
 					}

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
 }
 
@@ -423,7 +420,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 #ifndef RTABMAP_POINTMATCHER
 	if(_libpointmatcher)
 	{
-		UWARN("Parameter %s is set to true but RTAB-MAp has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
+		UWARN("Parameter %s is set to true but RTAB-Map has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
 		_libpointmatcher = false;
 	}
 #else
@@ -1049,17 +1046,29 @@ Transform RegistrationIcp::computeTransformationImpl(
 							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
 							icpT = guess * t.inverse() * guessInv;
 
-							// we were using normals, so compute correspondences using normals
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
-							pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+							if(fromScan.hasNormals() && toScan.hasNormals())
+							{
+								// we were using normals, so compute correspondences using normals
+								pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
+								pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
 
-							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
-									toCloudNormals,
-									_maxCorrespondenceDistance,
-									_maxRotation,
-									variance,
-									correspondences);
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudNormalsRegistered,
+										toCloudNormals,
+										_maxCorrespondenceDistance,
+										_maxRotation,
+										variance,
+										correspondences);
+							}
+							else
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudRegistered,
+										toCloudFiltered,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
+							}
 						}
 						else
 						{
@@ -1100,6 +1109,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					// verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
 					int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
 					UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
+
 					if(maxLaserScans)
 					{
 						correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -1128,10 +1138,17 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);
 
-					info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					if(correspondences == 0)
+					{
+						UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
+					}
+					else
+					{
+						info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					}
 					info.icpInliersRatio = correspondencesRatio;
 
-					if(correspondencesRatio < _correspondenceRatio)
+					if(correspondencesRatio <= _correspondenceRatio)
 					{
 						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
 								correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);

corelib/src/Rtabmap.cpp

@@ -114,13 +114,15 @@ 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()),
 	_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
 	_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
+	_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -309,6 +311,19 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	// Parse all parameters
 	this->parseParameters(parameters);
 
+	Transform lastPose;
+	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
+	if(_optimizedPoses.size())
+	{
+		if(!_savedLocalizationIgnored)
+		{
+			_lastLocalizationPose = lastPose;
+		}
+		std::map<int, Transform> tmp;
+		// Get just the links
+		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
+	}
+
 	if(_databasePath.empty())
 	{
 		_statisticLogged = false;
@@ -337,11 +352,10 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_loopClosureHypothesis = std::make_pair(0,0.0f);
 	_lastProcessTime = 0.0;
 	_someNodesHaveBeenTransferred = false;
-	_optimizedPoses.clear();
 	_constraints.clear();
 	_mapCorrection.setIdentity();
 	_mapCorrectionBackup.setNull();
-	_lastLocalizationPose.setNull();
+
 	_lastLocalizationNodeId = 0;
 	_distanceTravelled = 0.0f;
 	this->clearPath(0);
@@ -365,10 +379,14 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	}
 	if(_memory)
 	{
+		_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
 		_memory->close(databaseSaved, true, ouputDatabasePath);
 		delete _memory;
 		_memory = 0;
 	}
+	_optimizedPoses.clear();
+	_lastLocalizationPose.setNull();
+
 	if(_bayesFilter)
 	{
 		delete _bayesFilter;
@@ -437,13 +455,15 @@ 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);
 	Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
 	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
+	Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -644,6 +664,24 @@ Transform Rtabmap::getPose(int locationId) const
 	return Transform();
 }
 
+void Rtabmap::setInitialPose(const Transform & initialPose)
+{
+	if(_memory)
+	{
+		if(!_memory->isIncremental())
+		{
+			_lastLocalizationPose = initialPose;
+			_lastLocalizationNodeId = 0;
+			_mapCorrection.setIdentity();
+			_mapCorrectionBackup.setNull();
+		}
+		else
+		{
+			UWARN("Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().c_str());
+		}
+	}
+}
+
 int Rtabmap::triggerNewMap()
 {
 	int mapId = -1;
@@ -655,8 +693,6 @@ int Rtabmap::triggerNewMap()
 		_optimizedPoses.clear();
 		_constraints.clear();
 		_lastLocalizationNodeId = 0;
-		_mapCorrection.setIdentity();
-		_mapCorrectionBackup.setNull();
 
 		if(_bayesFilter)
 		{
@@ -761,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
 		{
@@ -810,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)
 		{
@@ -937,10 +975,24 @@ bool Rtabmap::process(
 	bool fakeOdom = false;
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->isIncremental() && !odomPose.isNull() && !_mapCorrectionBackup.isNull())
+		if(!_memory->isIncremental() && !odomPose.isNull())
 		{
-			_mapCorrection = _mapCorrectionBackup;
-			_mapCorrectionBackup.setNull();
+			if(!_mapCorrectionBackup.isNull())
+			{
+				_mapCorrection = _mapCorrectionBackup;
+				_mapCorrectionBackup.setNull();
+			}
+			else if(_optimizedPoses.size() && _mapCorrection.isIdentity() && !_lastLocalizationPose.isNull() && _lastLocalizationNodeId == 0)
+			{
+				// Localization mode, set map->odom so that odom is moved back to last saved localization
+				_mapCorrection = _lastLocalizationPose * odomPose.inverse();
+				_lastLocalizationNodeId = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
+				UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
+						_mapCorrection.prettyPrint().c_str(),
+						_lastLocalizationNodeId,
+						_lastLocalizationPose.prettyPrint().c_str(),
+						odomPose.prettyPrint().c_str());
+			}
 		}
 
 		if(odomPose.isNull())
@@ -1042,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
@@ -1057,12 +1109,15 @@ bool Rtabmap::process(
 					{
 						float x,y,z, roll,pitch,yaw;
 						links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-						bool isMoving = fabs(x) > _rgbdLinearUpdate ||
-										fabs(y) > _rgbdLinearUpdate ||
-										fabs(z) > _rgbdLinearUpdate ||
-										fabs(roll) > _rgbdAngularUpdate ||
-										fabs(pitch) > _rgbdAngularUpdate ||
-										fabs(yaw) > _rgbdAngularUpdate;
+						bool isMoving = (_rgbdLinearUpdate>0.0f && (
+											fabs(x) > _rgbdLinearUpdate ||
+											fabs(y) > _rgbdLinearUpdate ||
+											fabs(z) > _rgbdLinearUpdate))
+										||
+									    (_rgbdAngularUpdate>0.0f && (
+											fabs(roll) > _rgbdAngularUpdate ||
+											fabs(pitch) > _rgbdAngularUpdate ||
+											fabs(yaw) > _rgbdAngularUpdate));
 						if(!isMoving)
 						{
 							// This will disable global loop closure detection, only retrieval will be done.
@@ -1148,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()));
 						}
 					}
@@ -1847,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
@@ -1858,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)
 			{
@@ -1983,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
@@ -2066,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();
@@ -2182,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
@@ -2232,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
 		{
@@ -2241,14 +2308,23 @@ bool Rtabmap::process(
 			
 			// if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
 			float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection
-			if((normMapCorrection > 0.001f && _optimizeFromGraphEnd) ||
-				(normMapCorrection < 0.001f && !_optimizeFromGraphEnd))
+			if((normMapCorrection > 0.000001f && _optimizeFromGraphEnd) ||
+				(normMapCorrection < 0.000001f && !_optimizeFromGraphEnd))
 			{
-				poses.clear();
+				for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
+				{
+					if(iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kVirtualClosure)
+					{
+						UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection);
+						poses.clear();
+						break;
+					}
+				}
 			}
 
 			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).
@@ -2267,11 +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)
+			  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
@@ -2284,35 +2362,80 @@ 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)
+					{
+						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 m, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
+							  (int)loopClosureLinksAdded.size(),
+							  maxLinearErrorRatio,
+							  maxLinearLink->from(),
+							  maxLinearLink->to(),
+							  maxLinearLink->type(),
+							  maxLinearError,
+							  sqrt(maxLinearLink->transVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
+					}
+				}
+				if(maxAngularLink)
+				{
+					UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+					if(maxAngularErrorRatio > _optimizationMaxError)
+					{
+						UWARN("Rejecting all added loop closures (%d) in this "
+							  "iteration because a wrong loop closure has been "
+							  "detected after graph optimization, resulting in "
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
+							  (int)loopClosureLinksAdded.size(),
+							  maxAngularErrorRatio,
+							  maxAngularLink->from(),
+							  maxAngularLink->to(),
+							  maxAngularLink->type(),
+							  maxAngularError*180.0f/CV_PI,
+							  sqrt(maxAngularLink->rotVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
+					}
 				}
 
-				float stddev = sqrt(maxLinearLink->transVariance());
-				maxLinearErrorRatio = maxLinearError/stddev;
-				if(maxLinearErrorRatio > _optimizationMaxLinearError)
+				if(reject)
 				{
-					UWARN("Rejecting all added loop closures (%d) in this "
-						  "iteration because a wrong loop closure has been "
-						  "detected after graph optimization, resulting in "
-						  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f, stddev=%f). The "
-						  "maximum error ratio parameter is %f of std deviation.",
-						  (int)loopClosureLinksAdded.size(),
-						  maxLinearErrorRatio,
-						  maxLinearLink->from(),
-						  maxLinearLink->to(),
-						  maxLinearLink->type(),
-						  maxLinearError,
-						  stddev,
-						  _optimizationMaxLinearError);
 					for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
 					{
 						_memory->removeLink(iter->first, iter->second);
@@ -2330,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;
 			}
 		}
 
@@ -2398,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);
@@ -2420,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);
@@ -2521,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
@@ -2929,21 +3065,111 @@ void Rtabmap::setWorkingDirectory(std::string path)
 	}
 }
 
-void Rtabmap::rejectLoopClosure(int oldId, int newId)
+void Rtabmap::rejectLastLoopClosure()
 {
-	UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
-	if(_loopClosureHypothesis.first)
+	if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
 	{
-		_loopClosureHypothesis.first = 0;
-		if(_memory)
+		std::map<int, Link> links = _memory->getLinks(getLastLocationId(), false);
+		bool linksRemoved = false;
+		for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
-			_memory->removeLink(oldId, newId);
+			if(iter->second.type() == Link::kGlobalClosure ||
+				iter->second.type() == Link::kLocalSpaceClosure ||
+				iter->second.type() == Link::kLocalTimeClosure ||
+				iter->second.type() == Link::kUserClosure)
+			{
+				_memory->removeLink(iter->second.from(), iter->second.to());
+				std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
+				if(jter!=_constraints.end())
+				{
+					_constraints.erase(jter);
+					// second time if link is also inverted
+					jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
+					if(jter!=_constraints.end())
+					{
+						_constraints.erase(jter);
+					}
+				}
+				linksRemoved = true;
+			}
 		}
-		if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
+
+		if(linksRemoved)
 		{
-			statistics_.addStatistic(rtabmap::Statistics::kLoopRejectedHypothesis(), 1.0f);
+			_loopClosureHypothesis.first = 0;
+
+			// we have to re-optimize the graph without the rejected links
+			if(_memory->isIncremental() && _optimizedPoses.size())
+			{
+				UINFO("Update graph");
+				std::map<int, Transform> poses = _optimizedPoses;
+				std::multimap<int, Link> constraints;
+				cv::Mat covariance;
+				optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints);
+
+				if(poses.empty())
+				{
+					UWARN("Graph optimization failed after removing loop closure links from last location!");
+				}
+				else
+				{
+					UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
+					_optimizedPoses = poses;
+					_constraints = constraints;
+					_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
+				}
+			}
+		}
+	}
+}
+
+void Rtabmap::deleteLastLocation()
+{
+	if(_memory && _memory->getStMem().size())
+	{
+		int lastId = *_memory->getStMem().rbegin();
+		_memory->deleteLocation(lastId);
+		// we have to re-optimize the graph without the deleted location
+		if(_memory->isIncremental() && _optimizedPoses.size())
+		{
+			UINFO("Update graph");
+			_optimizedPoses.erase(lastId);
+			std::map<int, Transform> poses = _optimizedPoses;
+			//remove all constraints with last localization id
+			for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
+			{
+				if(iter->second.from() == lastId || iter->second.to() == lastId)
+				{
+					_constraints.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+
+			if(poses.empty())
+			{
+				_mapCorrection.setIdentity();
+			}
+			else
+			{
+				std::multimap<int, Link> constraints;
+				cv::Mat covariance;
+				optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints);
+
+				if(poses.empty())
+				{
+					UWARN("Graph optimization failed after deleting the last location!");
+				}
+				else
+				{
+					_optimizedPoses = poses;
+					_constraints = constraints;
+					_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
+				}
+			}
 		}
-		statistics_.setLoopClosureId(0);
 	}
 }
 
@@ -3153,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
@@ -3169,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())
@@ -3199,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
@@ -3280,28 +3508,28 @@ 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())
 		{
-			UERROR("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
+			UWARN("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
 			optimizedPoses = _graphOptimizer->optimizeIncremental(fromId, poses, edgeConstraints, 0, error, iterationsDone);
 
 			if(optimizedPoses.empty())
 			{
 				if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO)
 				{
-					UERROR("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
+					UWARN("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
 							Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str());
 				}
 				else
 				{
-					UERROR("Incremental optimization also failed.");
+					UWARN("Incremental optimization also failed.");
 				}
 			}
 			else
 			{
-				UERROR("Incremental optimization succeeded!");
+				UWARN("Incremental optimization succeeded!");
 			}
 		}
 	}
@@ -3454,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
 			{
@@ -3541,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
 			{
@@ -3695,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
 
@@ -3769,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). "
@@ -3783,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
@@ -4041,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);
@@ -4095,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;
@@ -4107,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);
-	data_ = transformationMatrix;
+			(transformationMatrix.type() == CV_32FC1 || transformationMatrix.type() == CV_64FC1));
+	if(transformationMatrix.type() == CV_32FC1)
+	{
+		data_ = transformationMatrix;
+	}
+	else
+	{
+		transformationMatrix.convertTo(data_, CV_32F);
+	}
 }
 
 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)

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;
-			file.open(dictionaryPath.c_str(), std::ifstream::in);
-			if(file.good())
+			UDEBUG("incremental=%d, oldPath=%s newPath=%s, visual words=%d",
+					_incrementalDictionary?1:0, _dictionaryPath.c_str(), dictionaryPath.c_str(), (int)_visualWords.size());
+
+			if(UFile::getExtension(dictionaryPath).compare("db") == 0)
 			{
-				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);
-				unsigned int dimension  = 0;
-				for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
+				UDEBUG("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(dictionaryPath, false))
 				{
-					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());
-						break;
+						iter->second->setSaved(true);
 					}
-				}
-
-				if(dimension == 0 || dimension > 1000)
-				{
-					UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
+					_incrementalDictionary = _visualWords.size()==0;
+					driver->closeConnection(false);
 				}
 				else
 				{
-					// Process all words
-					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;
+					UERROR("Could not load dictionary from database %s", dictionaryPath.c_str());
 				}
-
-
-				UDEBUG("Time changing dictionary = %fs", timer.ticks());
+				delete driver;
 			}
 			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)
-	{
-		_incrementalDictionary = false;
-	}
-	else if(_incrementalDictionary)
+	else if(_incrementalDictionary && _visualWords.size())
 	{
 		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 = uValue(_visualWords, wordId, vw);
-		if(vw)
-		{
-			vw->addRef(signatureId);
-			_totalActiveReferences += 1;
+		vw->addRef(signatureId);
+		_totalActiveReferences += 1;
 
-			_unusedWords.erase(vw->id());
-		}
-		else
-		{
-			UERROR("Not found word %d", wordId);
-		}
+		_unusedWords.erase(vw->id());
+	}
+	else
+	{
+		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));
-				if(d >= 0.0f && id > 0)
+				int index;
+				if (sizeof(size_t) == 8)
+				{
+					index = *((size_t*)&results.at<double>(i, j));
+				}
+				else
+				{
+					index = *((size_t*)&results.at<int>(i, j));
+				}
+				int id = uValue(_mapIndexId, index);
+				if(d >= 0.0f && id != 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
 				}
@@ -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));
-					if(d >= 0.0f && id > 0)
+					int index;
+
+					if (sizeof(size_t) == 8)
+					{
+						index = *((size_t*)&results.at<double>(i, j));
+					}
+					else
+					{
+						index = *((size_t*)&results.at<int>(i, j));
+					}
+					int id = uValue(_mapIndexId, index);
+					if(d >= 0.0f && id != 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));
 					}
@@ -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/resources/DatabaseSchema.sql.in

@@ -112,6 +112,7 @@ CREATE TABLE Admin (
 	opt_cloud BLOB,          -- compressed data
 	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
 	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
 	opt_polygons_size INTEGER, -- e.g., 3
 	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
 	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
@@ -119,6 +120,7 @@ CREATE TABLE Admin (
 	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
 	opt_map_x_min FLOAT,
 	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT, 
 
 	time_enter DATE
 );

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
 {
@@ -2285,6 +2286,45 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
+pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)
+{
+	pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
+	if(laserScan.isEmpty())
+	{
+		return cloud;
+	}
+
+	if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYZ)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYINormal || laserScan.format() == LaserScan::kXYZINormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudINormal(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYZRGB)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudRGB(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYZRGBNormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudRGBNormal(laserScan, transform), *cloud);
+	}
+	else
+	{
+		UERROR("Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.format());
+	}
+	return cloud;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);

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

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>
 
 #include <pcl/common/common.h>
 #include <pcl/common/centroid.h>
@@ -133,7 +134,14 @@ void occupancy2DFromLaserScan(
 	}
 
 	// copy directly obstacles precise positions
-	occupied = scanHit.clone();
+	if(scanMaxRange > cellSize)
+	{
+		occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data().clone();
+	}
+	else
+	{
+		occupied = scanHit.clone();
+	}
 }
 
 /**
@@ -566,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;
@@ -621,13 +631,16 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
 		float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
 
-		//UWARN("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
-		//		xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
+		UDEBUG("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
+				xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
 
 		UTimer timer;
 
 		map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
+		UDEBUG("map size = %dx%d", map.cols, map.rows);
+
 		int j=0;
+		float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
 		for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
 		{
 			const Transform & pose = poses.at(iter->first);
@@ -643,10 +656,14 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			for(int i=0; i<iter->second.first.cols; ++i)
 			{
 				const float * ptr = iter->second.first.ptr<float>(0, i);
-				cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
-				if(end!=start)
+				bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
+				if(!ignore)
 				{
-					map.at<char>(end.y, end.x) = 100; // obstacle
+					cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
+					if(end!=start)
+					{
+						map.at<char>(end.y, end.x) = 100; // obstacle
+					}
 				}
 			}
 
@@ -654,7 +671,21 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			for(int i=0; i<iter->second.first.cols; ++i)
 			{
 				const float * ptr = iter->second.first.ptr<float>(0, i);
-				cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
+
+				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((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -667,7 +698,21 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			for(int i=0; i<iter->second.second.cols; ++i)
 			{
 				const float * ptr = iter->second.second.ptr<float>(0, i);
-				cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
+
+				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((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -849,7 +894,7 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
 }
 
 //convert to gray scaled map
-cv::Mat convertMap2Image8U(const cv::Mat & map8S)
+cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
 {
 	UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
 	cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
@@ -857,11 +902,11 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 	{
 		for (int j = 0; j < map8S.cols; ++j)
 		{
-			char v = map8S.at<char>(i, j);
+			char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
 			unsigned char gray;
 			if(v == 0)
 			{
-				gray = 178;
+				gray = pgmFormat?254:178;
 			}
 			else if(v == 100)
 			{
@@ -869,11 +914,11 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 			}
 			else if(v == -2)
 			{
-				gray = 200;
+				gray = pgmFormat?254:200;
 			}
 			else // -1
 			{
-				gray = 89;
+				gray = pgmFormat?205:89;
 			}
 			map8U.at<unsigned char>(i, j) = gray;
 		}
@@ -881,6 +926,58 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 	return map8U;
 }
 
+//convert gray scaled image to map
+cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
+{
+	UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
+	cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
+	for (int i = 0; i < map8U.rows; ++i)
+	{
+		for (int j = 0; j < map8U.cols; ++j)
+		{
+			unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
+			char occupancy;
+			if(pgmFormat)
+			{
+				if(v >= 254)
+				{
+					occupancy = 0;
+				}
+				else if(v == 0)
+				{
+					occupancy = 100;
+				}
+				else // 205
+				{
+					occupancy = -1;
+				}
+			}
+			else
+			{
+				if(v == 178)
+				{
+					occupancy = 0;
+				}
+				else if(v == 0)
+				{
+					occupancy = 100;
+				}
+				else if(v == 200)
+				{
+					occupancy = -2;
+				}
+				else // 89
+				{
+					occupancy = -1;
+				}
+			}
+
+			map8S.at<char>(i, j) = occupancy;
+		}
+	}
+	return map8S;
+}
+
 cv::Mat erodeMap(const cv::Mat & map)
 {
 	UASSERT(map.type() == CV_8SC1);

corelib/src/util3d_motion_estimation.cpp

@@ -37,9 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/common/common.h>
 
-#if CV_MAJOR_VERSION < 3
 #include "opencv/solvepnp.h"
-#endif
 
 namespace rtabmap
 {
@@ -344,11 +342,10 @@ void solvePnPRansac(
 	{
 		minInliersCount = 4;
 	}
-#if CV_MAJOR_VERSION < 3
-	cv3::solvePnPRansac( //use OpenCV3 version of solvePnPRansac in OpenCV2
-#else
-	cv::solvePnPRansac( // use directly version from OpenCV 3
-#endif
+
+	// Use OpenCV3 version of solvePnPRansac in OpenCV2.
+	// FIXME: we should use this version of solvePnPRansac in newer 3.3.1 too, which seems a lot less stable!?!? Why!?
+	cv3::solvePnPRansac(
 			objectPoints,
 			imagePoints,
 			cameraMatrix,

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>
@@ -640,6 +642,11 @@ pcl::texture_mapping::CameraVector createTextureCameras(
 				cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
 			}
 
+			UDEBUG("%f", cam.focal_length);
+			UDEBUG("%f", cam.height);
+			UDEBUG("%f", cam.width);
+			UDEBUG("cam.pose=%s", t.prettyPrint().c_str());
+
 			cameras.push_back(cam);
 		}
 	}
@@ -1191,6 +1198,51 @@ void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & image
 	}
 }
 
+std::vector<std::vector<unsigned int> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
+{
+	std::vector<std::vector<unsigned int> > polygonsOut(polygons.size());
+	for(unsigned int p=0; p<polygons.size(); ++p)
+	{
+		polygonsOut[p] = polygons[p].vertices;
+	}
+	return polygonsOut;
+}
+std::vector<std::vector<std::vector<unsigned int> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
+{
+	std::vector<std::vector<std::vector<unsigned int> > > polygonsOut(tex_polygons.size());
+	for(unsigned int t=0; t<tex_polygons.size(); ++t)
+	{
+		polygonsOut[t].resize(tex_polygons[t].size());
+		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
+		{
+			polygonsOut[t][p] = tex_polygons[t][p].vertices;
+		}
+	}
+	return polygonsOut;
+}
+std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<unsigned int> > & polygons)
+{
+	std::vector<pcl::Vertices> polygonsOut(polygons.size());
+	for(unsigned int p=0; p<polygons.size(); ++p)
+	{
+		polygonsOut[p].vertices = polygons[p];
+	}
+	return polygonsOut;
+}
+std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons)
+{
+	std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
+	for(unsigned int t=0; t<tex_polygons.size(); ++t)
+	{
+		polygonsOut[t].resize(tex_polygons[t].size());
+		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
+		{
+			polygonsOut[t][p].vertices = tex_polygons[t][p];
+		}
+	}
+	return polygonsOut;
+}
+
 pcl::TextureMesh::Ptr assembleTextureMesh(
 		const cv::Mat & cloudMat,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
@@ -1357,6 +1409,50 @@ double sqr(uchar v)
 	return double(v)*double(v);
 }
 
+cv::Mat mergeTextures(
+		pcl::TextureMesh & mesh,
+		const std::map<int, cv::Mat> & images,
+		const std::map<int, CameraModel> & calibrations,
+		const Memory * memory,
+		const DBDriver * dbDriver,
+		int textureSize,
+		int textureCount,
+		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
+		bool gainCompensation,
+		float gainBeta,
+		bool gainRGB,
+		bool blending,
+		int blendingDecimation,
+		int brightnessContrastRatioLow,
+		int brightnessContrastRatioHigh,
+		bool exposureFusion,
+		const ProgressState * state)
+{
+	std::map<int, std::vector<CameraModel> > calibVectors;
+	for(std::map<int, CameraModel>::const_iterator iter=calibrations.begin(); iter!=calibrations.end(); ++iter)
+	{
+		std::vector<CameraModel> m;
+		m.push_back(iter->second);
+		calibVectors.insert(std::make_pair(iter->first, m));
+	}
+	return mergeTextures(mesh,
+			images,
+			calibVectors,
+			memory,
+			dbDriver,
+			textureSize,
+			textureCount,
+			vertexToPixels,
+			gainCompensation,
+			gainBeta,
+			gainRGB,
+			blending,
+			blendingDecimation,
+			brightnessContrastRatioLow,
+			brightnessContrastRatioHigh,
+			exposureFusion,
+			state);
+}
 cv::Mat mergeTextures(
 		pcl::TextureMesh & mesh,
 		const std::map<int, cv::Mat> & images,
@@ -1533,8 +1629,8 @@ cv::Mat mergeTextures(
 						newCamIndex[t] = oi;
 						int u = oi%cols * emptyImage.cols;
 						int v = ((oi/cols) % rows ) * emptyImage.rows;
-						UASSERT(u < textureSize-emptyImage.cols);
-						UASSERT(v < textureSize-emptyImage.rows);
+						UASSERT_MSG(u < textureSize-emptyImage.cols, uFormat("u=%d textureSize=%d emptyImage.cols=%d", u, textureSize, emptyImage.cols).c_str());
+						UASSERT_MSG(v < textureSize-emptyImage.rows, uFormat("v=%d textureSize=%d emptyImage.rows=%d", v, textureSize, emptyImage.rows).c_str());
 						imageOrigin[t].x = u;
 						imageOrigin[t].y = v;
 						if(textures[t].first>=0)
@@ -1990,10 +2086,49 @@ cv::Mat mergeTextures(
 			}
 		}
 	}
-	UDEBUG("globalTextures=%d", globalTextures.cols / globalTextures.rows);
+	UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
 	return globalTextures;
 }
 
+void fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh)
+{
+	// VTK issue:
+	//  tex_coordinates should be linked to points, not
+	//  polygon vertices. Points linked to multiple different TCoords (different textures) should
+	//  be duplicated.
+	for (unsigned int t = 0; t < textureMesh.tex_coordinates.size(); ++t)
+	{
+		if(textureMesh.tex_polygons[t].size())
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromPCLPointCloud2(textureMesh.cloud, *originalCloud);
+
+			// make a cloud with as many points than polygon vertices
+			unsigned int nPoints = textureMesh.tex_coordinates[t].size();
+			UASSERT(nPoints == textureMesh.tex_polygons[t].size()*textureMesh.tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			newCloud->resize(nPoints);
+
+			unsigned int oi = 0;
+			for (unsigned int i = 0; i < textureMesh.tex_polygons[t].size(); ++i)
+			{
+				pcl::Vertices & vertices = textureMesh.tex_polygons[t][i];
+
+				for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+				{
+					UASSERT(oi < newCloud->size());
+					UASSERT_MSG(vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+					newCloud->at(oi) = originalCloud->at(vertices.vertices[j]);
+					vertices.vertices[j] = oi; // new vertex index
+					++oi;
+				}
+			}
+			pcl::toPCLPointCloud2(*newCloud, textureMesh.cloud);
+		}
+	}
+}
+
 LaserScan computeNormals(
 		const LaserScan & laserScan,
 		int searchK,
@@ -2828,11 +2963,12 @@ void adjustNormalsToViewPoints(
 		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
 {
+	UASSERT(rawCloud.get() && cloud.get());
+	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)poses.size(), (int)rawCloud->size(), (int)rawCameraIndices.size(), (int)cloud->size());
 	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
 		rawTree->setInputCloud (rawCloud);
-
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
 			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
@@ -2841,9 +2977,10 @@ void adjustNormalsToViewPoints(
 				std::vector<int> indices;
 				std::vector<float> dist;
 				rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
-				UASSERT(indices.size() == 1);
 				if(indices.size() && indices[0]>=0)
 				{
+					UASSERT_MSG(indices[0]<(int)rawCameraIndices.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)rawCameraIndices.size()).c_str());
+					UASSERT(uContains(poses, rawCameraIndices[indices[0]]));
 					Transform p = poses.at(rawCameraIndices[indices[0]]);
 					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
 					Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
@@ -2861,7 +2998,7 @@ void adjustNormalsToViewPoints(
 				}
 				else
 				{
-					UWARN("Not found camera viewpoint for point %d", i);
+					UWARN("Not found camera viewpoint for point %d!?", i);
 				}
 			}
 		}

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

@@ -0,0 +1,84 @@
+# Image: introlab3it/rtabmap:bionic
+
+FROM ubuntu:18.04
+
+# Install build dependencies
+RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
+      libsqlite3-dev \
+      libpcl-dev \
+      git \
+      cmake \
+      libopencv-dev \
+      libproj-dev \
+      libqt5svg5-dev \
+      libfreenect-dev \
+      libopenni2-dev \
+      ffmpeg \
+      software-properties-common
+
+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
+RUN cd rtabmap/build && \
+    cmake .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd ../.. && \
+    rm -rf rtabmap && \
+    ldconfig
+
+WORKDIR /root
+    

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
-rm -r gtsam
+cd $pwd
+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
-rm -r g2o
+cd $pwd
+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
-rm -r VTK cmake-3.7.2-Linux-x86_64
+cd $pwd
+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
-rm -r pcl
+cd $pwd
+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
-rm -r opencv opencv_contrib
+cd $pwd
+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
-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 update
+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
-rm -r gtsam
+cd $pwd
+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
-rm -r g2o
+cd $pwd
+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
-rm -r VTK
+cd $pwd
+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
-rm -r pcl
+cd $pwd
+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
-rm -r opencv opencv_contrib
+cd $pwd
+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_;