Fixed 2 cmake minor warnings on standard build

Fix laserscan from depth

.appveyor.yml

@@ -0,0 +1,70 @@
+
+branches:
+  only:
+    - master
+    - devel
+
+os: Visual Studio 2015
+
+clone_folder: c:\projects\rtabmap
+
+platform: x64
+configuration: Release
+
+init:
+  - cmake --version
+  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
+  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
+
+install:
+  # Qt
+  - set QTDIR=C:\Qt\5.8\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%
+  # Openni2
+  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
+  - cmd: OpenNI2.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenNI2:"
+  - ps: "ls \"C:/Program Files/OpenNI2\""
+  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
+  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
+  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
+  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
+  # OpenCV
+  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
+  - cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
+  - 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
+  - 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
+  # 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
+  
+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" ..
+  
+after_build :
+  - cmake --build . --config Release --target package
+
+artifacts:
+  - path: build\RTABMap-*
+  
+notifications:
+  - provider: Email
+    to:
+      - matlabbe@email.com
+    on_build_success: false
+    on_build_failure: false
+    on_build_status_changed: true

.travis.yml

@@ -14,6 +14,7 @@ addons:
       - libopencv-dev
       - libqt4-dev
       - libsqlite3-dev
+      - libyaml-cpp-dev
 
 install:
     - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'

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 13)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_MINOR_VERSION 17)
+SET(RTABMAP_PATCH_VERSION 0)
 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 #######
 
@@ -148,6 +146,7 @@ option(WITH_QT            "Include Qt support"                   ON)
 ENDIF()
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
+option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
 option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
@@ -155,27 +154,39 @@ option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
 option(WITH_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_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(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
-FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
+
+IF(WITH_QT)
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
+ELSE()
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation )
+ENDIF()
+if("${PCL_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)
 
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
    # fix libproj.so not found on Xenial
    list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
-   # fix libmpi.so not found on Zesty
-   list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so")
 endif()
 
 # OpenMP ("-fopenmp" should be added for flann included in PCL)
@@ -262,6 +273,13 @@ IF(WITH_FREENECT2)
     ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)
 
+IF(WITH_K4W2 AND WIN32)
+    FIND_PACKAGE(KinectSDK2 QUIET)
+	IF(KinectSDK2_FOUND)
+       MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}")
+    ENDIF(KinectSDK2_FOUND)
+ENDIF(WITH_K4W2 AND WIN32)
+
 # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
 IF(WITH_OPENNI2 OR OpenNI2_FOUND)
     FIND_PACKAGE(OpenNI2 QUIET)
@@ -302,6 +320,14 @@ IF(WITH_CVSBA)
     ENDIF(cvsba_FOUND)
 ENDIF(WITH_CVSBA)
 
+IF(WITH_POINTMATCHER)
+    find_package(libpointmatcher QUIET)
+    IF(libpointmatcher_FOUND)
+       MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
+    ENDIF(libpointmatcher_FOUND)
+ENDIF(WITH_POINTMATCHER)
+
+SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
     IF(WIN32) # Windows
         SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
@@ -316,7 +342,7 @@ IF(WITH_ZED)
             LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
         ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
     ELSE() # Linux
-        find_package(ZED 1 QUIET)
+        find_package(ZED 2 QUIET)
     ENDIF(WIN32)
     
     IF(ZED_FOUND)
@@ -348,7 +374,10 @@ ENDIF(WITH_REALSENSE)
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(OCTOMAP QUIET)
     IF(OCTOMAP_FOUND)
-       MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
+       MESSAGE(STATUS "Found octomap ${OCTOMAP_VERSION}: ${OCTOMAP_INCLUDE_DIRS}")
+       IF(OCTOMAP_VERSION VERSION_LESS 1.8)
+          ADD_DEFINITIONS("-DOCTOMAP_PRE_18")
+       ENDIF(OCTOMAP_VERSION VERSION_LESS 1.8)
     ENDIF(OCTOMAP_FOUND)
 ENDIF(WITH_OCTOMAP)
 
@@ -359,6 +388,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)
@@ -380,6 +416,19 @@ IF(WITH_DVO)
     ENDIF(dvo_core_FOUND)
 ENDIF(WITH_DVO)
 
+IF(WITH_OKVIS)
+    FIND_PACKAGE(okvis 1.1 QUIET)
+    IF(okvis_FOUND)
+       MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
+       find_package(brisk 2 REQUIRED)
+       MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
+       find_package(opengv REQUIRED)
+       MESSAGE(STATUS "Found opengv: ${OPENGV_INCLUDE_DIRS}")
+       find_package(Ceres REQUIRED CONFIG PATHS ${OKVIS_CERES_CONFIG} NO_DEFAULT_PATH)
+       MESSAGE(STATUS "Found ceres: ${CERES_INCLUDE_DIRS}")
+    ENDIF(okvis_FOUND)
+ENDIF(WITH_OKVIS)
+
 IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     FIND_PACKAGE(ORB_SLAM2 QUIET)
     IF(ORB_SLAM2_FOUND)
@@ -392,12 +441,11 @@ 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)
+IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
    #Newest versions require std11
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -411,7 +459,7 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SL
         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).")
       ENDIF()
    ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND)
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -454,6 +502,9 @@ IF(NOT G2O_FOUND)
    SET(G2O "//")
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
+   IF(NOT G2O_CPP11)
+   SET(G2O_CPP_CONF "//")
+   ENDIF(NOT G2O_CPP11)
 ENDIF()
 IF(NOT GTSAM_FOUND)
    SET(GTSAM "//")
@@ -471,6 +522,9 @@ IF(NOT cvsba_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES})
 ENDIF()
+IF(NOT libpointmatcher_FOUND)
+   SET(POINTMATCHER "//")
+ENDIF(NOT libpointmatcher_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
 ELSE()
@@ -481,6 +535,11 @@ IF(NOT freenect2_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
 ENDIF()
+IF(NOT KinectSDK2_FOUND)
+   SET(K4W2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${KinectSDK2_LIBRARIES})
+ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
 ELSE()
@@ -519,6 +578,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()
@@ -534,6 +598,11 @@ IF(NOT dvo_core_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) 
 ENDIF()
+IF(NOT okvis_FOUND)
+   SET(OKVIS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OKVIS_LIBRARIES}) 
+ENDIF()
 IF(NOT ORB_SLAM2_FOUND)
    SET(ORB_SLAM2 "//")
 ELSE()
@@ -545,6 +614,10 @@ IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
 ELSE()
    SET(CONF_VTK_QT false)
 ENDIF()
+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))
    SET(OPENCV3 "//")
 ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
@@ -726,6 +799,7 @@ IF(APPLE)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
 ENDIF(APPLE)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
+MESSAGE(STATUS "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")
 
 IF(OpenCV_FOUND)
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -767,6 +841,14 @@ ELSE()
 MESSAGE(STATUS "  With Freenect2            = NO (libfreenect2 not found)")
 ENDIF()
 
+IF(KinectSDK2_FOUND)
+MESSAGE(STATUS "  With Kinect for Windows 2 = YES (License: Apache v2 and/or GPLv2)")
+ELSEIF(NOT WITH_K4W2)
+MESSAGE(STATUS "  With Kinect for Windows 2 = NO (WITH_K4W2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)")
+ENDIF()
+
 IF(DC1394_FOUND)
 MESSAGE(STATUS "  With dc1394               = YES (License: LGPL)")
 ELSEIF(NOT WITH_DC1394)
@@ -823,6 +905,14 @@ ELSE()
 MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()
 
+IF(libpointmatcher_FOUND)
+MESSAGE(STATUS "  With libpointmatcher      = YES (License: BSD)")
+ELSEIF(NOT WITH_POINTMATCHER)
+MESSAGE(STATUS "  With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libpointmatcher      = NO (libpointmatcher not found)")
+ENDIF()
+
 IF(ZED_FOUND)
 IF(CUDA_FOUND)
 MESSAGE(STATUS "  With ZED                  = YES (With CUDA)")
@@ -866,6 +956,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)
@@ -890,6 +988,14 @@ ELSE()
 MESSAGE(STATUS "  With dvo_core             = NO (dvo_core not found)")
 ENDIF()
 
+IF(okvis_FOUND)
+MESSAGE(STATUS "  With okvis                = YES (License: BSD)")
+ELSEIF(NOT WITH_DVO)
+MESSAGE(STATUS "  With okvis                = NO (WITH_OKVIS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With okvis                = NO (okvis not found)")
+ENDIF()
+
 IF(ORB_SLAM2_FOUND)
 MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
 ELSEIF(NOT WITH_ORB_SLAM2)

README.md

@@ -1,6 +1,18 @@
-rtabmap [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap)
+rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pixel) 
 =======
 
+[![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
+
+[![Release][release-image]][releases]
+[![License][license-image]][license]
+Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
+
+[release-image]: https://img.shields.io/badge/release-0.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
+[license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
+
 RTAB-Map library and standalone application.
 
 For more information, visit the [RTAB-Map's home page](http://introlab.github.io/rtabmap) or the [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).

Version.h.in

@@ -40,13 +40,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @NONFREE@#define RTABMAP_NONFREE
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
 @GTSAM@#define RTABMAP_GTSAM
 @VERTIGO@#define RTABMAP_VERTIGO
 @OPENCV3@#define RTABMAP_OPENCV3
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
+@K4W2@#define RTABMAP_K4W2
 @CVSBA@#define RTABMAP_CVSBA
+@POINTMATCHER@#define RTABMAP_POINTMATCHER
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
@@ -54,9 +57,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
 @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
 @ORB_SLAM2@#define RTABMAP_ORB_SLAM2
 
 #endif /* VERSION_H_ */

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="55"
+        android:versionCode="70"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -12,6 +12,8 @@
     <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
+    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-feature android:name="android.hardware.location.gps" />
     <uses-feature android:glEsVersion="0x00020000" />
 
     <!-- This is the platform API where NativeActivity was introduced. -->

app/android/jni/CameraTango.cpp

@@ -46,7 +46,7 @@ const int scanDownsampling = 1;
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(point_cloud->num_points>0)
+	if(app->isRunning() && point_cloud->num_points>0)
 	{
 		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 	}
@@ -55,31 +55,34 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	cv::Mat tangoImage;
-	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+	if(app->isRunning())
 	{
-		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
-	}
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
-	{
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-	}
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-	{
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-	}
-	else if(color->format == 35)
-	{
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-	}
-	else
-	{
-		LOGE("Not supported color format : %d.", color->format);
-	}
+		cv::Mat tangoImage;
+		if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+		{
+			tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+		}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+		{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+		{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
+		else if(color->format == 35)
+		{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
+		else
+		{
+			LOGE("Not supported color format : %d.", color->format);
+		}
 
-	if(!tangoImage.empty())
-	{
-		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+		if(!tangoImage.empty())
+		{
+			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+		}
 	}
 }
 
@@ -116,7 +119,8 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan,
 		cloudStamp_(0),
 		tangoColorType_(0),
 		tangoColorStamp_(0),
-		colorCameraToDisplayRotation_(ROTATION_0)
+		colorCameraToDisplayRotation_(ROTATION_0),
+		originUpdate_(false)
 {
 	UASSERT(decimation >= 1);
 }
@@ -425,12 +429,22 @@ void CameraTango::close()
 	{
 		TangoConfig_free(tango_config_);
 		tango_config_ = nullptr;
+		LOGI("TangoService_disconnect()");
 		TangoService_disconnect();
+		LOGI("TangoService_disconnect() done.");
 	}
 	previousPose_.setNull();
 	previousStamp_ = 0.0;
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
+	lastKnownGPS_ = GPS();
+	originOffset_ = Transform();
+	originUpdate_ = false;
+}
+
+void CameraTango::resetOrigin()
+{
+	originUpdate_ = true;
 }
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -491,10 +505,23 @@ static rtabmap::Transform opticalRotation(
 								0.0f,  0.0f, -1.0f, 0.0f);
 void CameraTango::poseReceived(const Transform & pose)
 {
-	if(!pose.isNull() && pose.getNormSquared() < 100000)
+	if(!pose.isNull())
 	{
 		// send pose of the camera (without optical rotation), not the device
-		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotation));
+		Transform p = pose*deviceTColorCamera_*opticalRotation;
+		if(originUpdate_)
+		{
+			originOffset_ = p.translation().inverse();
+			originUpdate_ = false;
+		}
+		if(!originOffset_.isNull())
+		{
+			this->post(new PoseEvent(originOffset_*p));
+		}
+		else
+		{
+			this->post(new PoseEvent(p));
+		}
 	}
 }
 
@@ -513,6 +540,11 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }
 
+void CameraTango::setGPS(const GPS & gps)
+{
+	lastKnownGPS_ = gps;
+}
+
 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
@@ -699,6 +731,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
 			std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
 			int oi=0;
+			int closePoints = 0;
+			float closeROI[4];
+			closeROI[0] = depth.cols/4;
+			closeROI[1] = 3*(depth.cols/4);
+			closeROI[2] = depth.rows/4;
+			closeROI[3] = 3*(depth.rows/4);
+			unsigned short minDepthValue=10000;
 			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
 				float * p = cloud.ptr<float>(0,i);
@@ -717,6 +756,17 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
 				unsigned short depth_value(pt.z * 1000.0f);
 
+				if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
+				   pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
+				   depth_value < 600)
+				{
+					++closePoints;
+					if(depth_value < minDepthValue)
+					{
+						minDepthValue = depth_value;
+					}
+				}
+
 				bool pixelSet = false;
 				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
 				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
@@ -746,6 +796,11 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				}
 			}
 
+			if(closePoints > 100)
+			{
+				this->post(new CameraTangoEvent(0, "TooClose", ""));
+			}
+
 			if(oi)
 			{
 				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
@@ -763,6 +818,12 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
 
+			// adjust origin
+			if(!originOffset_.isNull())
+			{
+				poseDevice = originOffset_ * poseDevice;
+			}
+
 			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
 			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
 			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
@@ -830,13 +891,22 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 			if(rawScanPublished_)
 			{
-				data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
 			else
 			{
 				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
 			data.setGroundTruth(odom);
+
+			if(lastKnownGPS_.stamp() > 0.0 && rgbStamp-lastKnownGPS_.stamp()<1.0)
+			{
+				data.setGPS(lastKnownGPS_);
+			}
+			else if(lastKnownGPS_.stamp()>0.0)
+			{
+				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
+			}
 		}
 		else
 		{
@@ -866,6 +936,7 @@ void CameraTango::mainLoop()
 		{
 			rtabmap::Transform pose = data.groundTruth();
 			data.setGroundTruth(Transform());
+
 			// convert stamp to epoch
 			bool firstFrame = previousPose_.isNull();
 			if(firstFrame)
@@ -879,8 +950,8 @@ void CameraTango::mainLoop()
 				info.interval = data.stamp()-previousStamp_;
 				info.transform = previousPose_.inverse() * pose;
 			}
-			info.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.000001);
-			LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.000001);
+			info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
+			LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
 			this->post(new OdometryEvent(data, pose, info));
 			previousPose_ = pose;
 			previousStamp_ = data.stamp();

app/android/jni/CameraTango.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define CAMERATANGO_H_
 
 #include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
 #include <rtabmap/utilite/USemaphore.h>
 #include <rtabmap/utilite/UEventsSender.h>
@@ -80,6 +81,7 @@ public:
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	void close(); // close Tango connection
+	void resetOrigin();
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	const CameraModel & getCameraModel() const {return model_;}
@@ -89,6 +91,7 @@ public:
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setGPS(const GPS & gps);
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
@@ -126,6 +129,9 @@ private:
 	TangoSupportRotation colorCameraToDisplayRotation_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
+	GPS lastKnownGPS_;
+	Transform originOffset_;
+	bool originUpdate_;
 };
 
 } /* namespace rtabmap */

app/android/jni/RTABMapApp.cpp

@@ -93,12 +93,13 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpParallelized(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.1")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("1")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), std::string("0.05")));
 
 	if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end())
 	{
@@ -183,11 +184,12 @@ RTABMapApp::RTABMapApp() :
 		lastDrawnCloudsCount_(0),
 		renderingTime_(0.0f),
 		lastPostRenderEventTime_(0.0),
-		processMemoryUsedBytes(0),
-		processGPUMemoryUsedBytes(0),
+		lastPoseEventTime_(0.0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
 		optMesh_(new pcl::TextureMesh),
+		optRefId_(0),
+		optRefPose_(0),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
@@ -195,28 +197,45 @@ RTABMapApp::RTABMapApp() :
 }
 
 RTABMapApp::~RTABMapApp() {
-  if(camera_)
-  {
-	  delete camera_;
-  }
-  if(rtabmapThread_)
-  {
-	  rtabmapThread_->close(false);
-	  delete rtabmapThread_;
-  }
-  if(logHandler_)
-  {
-	  delete logHandler_;
-  }
-  boost::mutex::scoped_lock  lock(rtabmapMutex_);
-  if(rtabmapEvents_.size())
-  {
-	  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
-	  {
-		  delete *iter;
-	  }
-  }
-  rtabmapEvents_.clear();
+	if(camera_)
+	{
+		delete camera_;
+	}
+	if(rtabmapThread_)
+	{
+		rtabmapThread_->close(false);
+		delete rtabmapThread_;
+	}
+	if(logHandler_)
+	{
+		delete logHandler_;
+	}
+	if(optRefPose_)
+	{
+		delete optRefPose_;
+	}
+	{
+		boost::mutex::scoped_lock  lock(rtabmapMutex_);
+		if(rtabmapEvents_.size())
+		{
+			for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
+			{
+				delete *iter;
+			}
+		}
+		rtabmapEvents_.clear();
+	}
+	{
+		boost::mutex::scoped_lock  lock(visLocalizationMutex_);
+		if(visLocalizationEvents_.size())
+		{
+			for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents_.begin(); iter!=visLocalizationEvents_.end(); ++iter)
+			{
+				delete *iter;
+			}
+		}
+		visLocalizationEvents_.clear();
+	}
 }
 
 void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
@@ -236,8 +255,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 	lastDrawnCloudsCount_ = 0;
 	renderingTime_ = 0.0f;
 	lastPostRenderEventTime_ = 0.0;
-	processMemoryUsedBytes = 0;
-	processGPUMemoryUsedBytes = 0;
+	lastPoseEventTime_ = 0.0;
 	bufferedStatsData_.clear();
 	progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
 	main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
@@ -279,6 +297,13 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
 	rtabmapMutex_.lock();
+	if(rtabmapEvents_.size())
+	{
+		for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
+		{
+			delete *iter;
+		}
+	}
 	rtabmapEvents_.clear();
 	openingDatabase_ = true;
 	if(rtabmapThread_)
@@ -296,6 +321,12 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	// Open visualization while we load (if there is an optimized mesh saved in database)
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
+	optRefId_ = 0;
+	if(optRefPose_)
+	{
+		delete optRefPose_;
+		optRefPose_ = 0;
+	}
 	cv::Mat cloudMat;
 	std::vector<std::vector<std::vector<unsigned int> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -304,14 +335,13 @@ 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.");
@@ -403,7 +433,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	}
 
 	{
-		LOGI("Creating the meshes (%d)....", poses.size());
+		LOGI("Creating the meshes (%d)....", (int)poses.size());
 		boost::mutex::scoped_lock  lock(meshesMutex_);
 		createdMeshes_.clear();
 		int i=0;
@@ -480,16 +510,6 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							UERROR("Failed to uncompress data!");
 							status=-2;
 						}
-						const rtabmap::Signature & s = signatures.at(id);
-						processMemoryUsedBytes += data.imageCompressed().total();
-						processMemoryUsedBytes += data.depthOrRightCompressed().total();
-						processMemoryUsedBytes += data.laserScanCompressed().total();
-						processMemoryUsedBytes += s.getWords().size()*4*8;
-						processMemoryUsedBytes += s.getWords3().size()*4*4;
-						if(!s.getWordsDescriptors().empty())
-						{
-							processMemoryUsedBytes +=s.getWordsDescriptors().size()*(4+s.getWordsDescriptors().begin()->second.total());
-						}
 					}
 					else
 					{
@@ -565,6 +585,19 @@ 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();
+	}
+
 	// Start threads
 	LOGI("Start rtabmap thread");
 	rtabmapThread_->registerToEventsManager();
@@ -735,7 +768,7 @@ std::vector<pcl::Vertices> RTABMapApp::filterOrganizedPolygons(
 	unsigned int biggestClusterSize = 0;
 	for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
 	{
-		LOGD("cluster %d = %d", iter->first, iter->second.size());
+		LOGD("cluster %d = %d", iter->first, (int)iter->second.size());
 
 		if(iter->second.size() > biggestClusterSize)
 		{
@@ -995,14 +1028,12 @@ int RTABMapApp::Render()
 				poseEvents_.clear();
 			}
 		}
-		rtabmap::Transform mapOdom = rtabmap::Transform::getIdentity();
 		if(!pose.isNull())
 		{
 			// update camera pose?
-			if(graphOptimization_ && !visualizingMesh_ && !mapToOdom_.isIdentity())
+			if(graphOptimization_ && !mapToOdom_.isIdentity())
 			{
-				mapOdom = mapToOdom_;
-				main_scene_.SetCameraPose(opengl_world_T_rtabmap_world*mapOdom*rtabmap_world_T_tango_world*pose);
+				main_scene_.SetCameraPose(opengl_world_T_rtabmap_world*mapToOdom_*rtabmap_world_T_tango_world*pose);
 			}
 			else
 			{
@@ -1013,6 +1044,7 @@ int RTABMapApp::Render()
 				notifyCameraStarted = true;
 				cameraJustInitialized_ = false;
 			}
+			lastPoseEventTime_ = UTimer::now();
 		}
 
 		rtabmap::OdometryEvent odomEvent;
@@ -1061,7 +1093,6 @@ int RTABMapApp::Render()
 						mesh.texCoords = optMesh_->tex_coordinates[0];
 						mesh.texture = optTexture_;
 					}
-
 					main_scene_.addMesh(g_optMeshId, mesh, opengl_world_T_rtabmap_world, true);
 				}
 				else
@@ -1071,6 +1102,58 @@ int RTABMapApp::Render()
 					pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud);
 					main_scene_.addCloud(g_optMeshId, cloud, indices, opengl_world_T_rtabmap_world);
 				}
+
+				// clean up old messages if there are ones
+				boost::mutex::scoped_lock  lock(visLocalizationMutex_);
+				if(visLocalizationEvents_.size())
+				{
+					for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents_.begin(); iter!=visLocalizationEvents_.end(); ++iter)
+					{
+						delete *iter;
+					}
+				}
+				visLocalizationEvents_.clear();
+			}
+
+			std::list<rtabmap::RtabmapEvent*> visLocalizationEvents;
+			visLocalizationMutex_.lock();
+			visLocalizationEvents = visLocalizationEvents_;
+			visLocalizationEvents_.clear();
+			visLocalizationMutex_.unlock();
+
+			if(visLocalizationEvents.size())
+			{
+				const rtabmap::Statistics & stats = visLocalizationEvents.back()->getStats();
+				if(!stats.mapCorrection().isNull())
+				{
+					mapToOdom_ = stats.mapCorrection();
+				}
+
+				std::map<int, rtabmap::Transform>::const_iterator iter = stats.poses().find(optRefId_);
+				if(iter != stats.poses().end() && !iter->second.isNull() && optRefPose_)
+				{
+					// adjust opt mesh pose
+					main_scene_.setCloudPose(g_optMeshId, opengl_world_T_rtabmap_world * iter->second * (*optRefPose_).inverse());
+				}
+				int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
+				int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
+				int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
+				if(!paused_ && loopClosure>0)
+				{
+					main_scene_.setBackgroundColor(0, 0.5f, 0); // green
+				}
+				else if(!paused_ && rejected>0)
+				{
+					main_scene_.setBackgroundColor(0, 0.2f, 0); // dark green
+				}
+				else if(!paused_ && fastMovement)
+				{
+					main_scene_.setBackgroundColor(0.2f, 0, 0.2f); // dark magenta
+				}
+				else
+				{
+					main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
+				}
 			}
 
 			//backup state
@@ -1088,6 +1171,20 @@ int RTABMapApp::Render()
 
 			// revert state
 			main_scene_.setMeshRendering(isMeshRendering, isTextureRendering);
+
+			if(visLocalizationEvents.size())
+			{
+				// send statistics to GUI
+				UEventsManager::post(new PostRenderEvent(visLocalizationEvents.back()));
+				visLocalizationEvents.pop_back();
+
+				for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents.begin(); iter!=visLocalizationEvents.end(); ++iter)
+				{
+					delete *iter;
+				}
+				visLocalizationEvents.clear();
+				lastPostRenderEventTime_ = UTimer::now();
+			}
 		}
 		else
 		{
@@ -1155,8 +1252,7 @@ int RTABMapApp::Render()
 				lastDrawnCloudsCount_ = 0;
 				renderingTime_ = 0.0f;
 				lastPostRenderEventTime_ = 0.0;
-				processMemoryUsedBytes = 0;
-				processGPUMemoryUsedBytes = 0;
+				lastPoseEventTime_ = 0.0;
 				bufferedStatsData_.clear();
 			}
 
@@ -1170,7 +1266,6 @@ int RTABMapApp::Render()
 				if(added.size() != meshes)
 				{
 					LOGI("added (%d) != meshes (%d)", (int)added.size(), meshes);
-					processGPUMemoryUsedBytes = 0;
 					boost::mutex::scoped_lock  lockRtabmap(rtabmapMutex_);
 					UASSERT(rtabmap_!=0);
 					for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
@@ -1205,14 +1300,6 @@ int RTABMapApp::Render()
 							main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
 							main_scene_.setCloudVisible(iter->first, iter->second.visible);
 
-							long estimateGPUMem = 0;
-							estimateGPUMem += iter->second.cloud->size()*16; // 3*float + 1 float rgb
-							estimateGPUMem += iter->second.indices->size()*4; // int
-							estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon
-							estimateGPUMem += iter->second.polygonsLowRes.size()*4*3; // 3 indices per polygon
-
-							processGPUMemoryUsedBytes += estimateGPUMem + (iter->second.texture.empty()?0:iter->second.polygons.size()*3*8+iter->second.texture.total());
-
 							iter->second.texture = cv::Mat(); // don't keep textures in memory
 						}
 					}
@@ -1237,7 +1324,7 @@ int RTABMapApp::Render()
 
 				// update buffered signatures
 				std::map<int, rtabmap::SensorData> bufferedSensorData;
-				if(!trajectoryMode_ && !dataRecorderMode_)
+				if(!dataRecorderMode_)
 				{
 					for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
 					{
@@ -1245,42 +1332,29 @@ int RTABMapApp::Render()
 
 						// Don't create mesh for the last node added if rehearsal happened or if discarded (small movement)
 						int smallMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
+						int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
 						int rehearsalMerged = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f);
-						if(smallMovement == 0 && rehearsalMerged == 0)
+						if(!localizationMode_ && stats.getSignatures().size() &&
+							smallMovement == 0 && rehearsalMerged == 0 && fastMovement == 0)
 						{
-							for(std::map<int, rtabmap::Signature>::const_iterator jter=stats.getSignatures().begin(); jter!=stats.getSignatures().end(); ++jter)
+							int id = stats.getSignatures().rbegin()->first;
+							const rtabmap::Signature & s = stats.getSignatures().rbegin()->second;
+
+							if(!trajectoryMode_ &&
+							   !s.sensorData().imageRaw().empty() &&
+							   !s.sensorData().depthRaw().empty())
 							{
-								bool dataDetected = false;
-								if(!jter->second.sensorData().imageRaw().empty() &&
-								   !jter->second.sensorData().depthRaw().empty())
-								{
-									if(!localizationMode_)
-									{
-										uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData()));
-										uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
-										dataDetected = true;
-									}
-								}
-								if(dataDetected)
-								{
-									processMemoryUsedBytes += jter->second.sensorData().imageCompressed().total();
-									processMemoryUsedBytes += jter->second.sensorData().depthOrRightCompressed().total();
-									processMemoryUsedBytes += jter->second.sensorData().laserScanCompressed().total();
-									processMemoryUsedBytes += jter->second.getWords().size()*4*8;
-									processMemoryUsedBytes += jter->second.getWords3().size()*4*4;
-									if(!jter->second.getWordsDescriptors().empty())
-									{
-										processMemoryUsedBytes += jter->second.getWordsDescriptors().size()*(4+jter->second.getWordsDescriptors().begin()->second.total());
-									}
-								}
+								uInsert(bufferedSensorData, std::make_pair(id, s.sensorData()));
 							}
+
+							uInsert(rawPoses_, std::make_pair(id, s.getPose()));
 						}
 
 						int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
 						int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
 						if(!paused_ && loopClosure>0)
 						{
-							main_scene_.setBackgroundColor(0, 0.7f, 0); // green
+							main_scene_.setBackgroundColor(0, 0.5f, 0); // green
 						}
 						else if(!paused_ && rejected>0)
 						{
@@ -1290,12 +1364,17 @@ int RTABMapApp::Render()
 						{
 							main_scene_.setBackgroundColor(0, 0, 0.2f); // blue
 						}
+						else if(!paused_ && fastMovement)
+						{
+							main_scene_.setBackgroundColor(0.2f, 0, 0.2f); // dark magenta
+						}
 						else
 						{
 							main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
 						}
 					}
 				}
+
 #ifdef DEBUG_RENDERING_PERFORMANCE
 				LOGW("Looking fo data to load (%d) %fs", bufferedSensorData.size(), time.ticks());
 #endif
@@ -1431,13 +1510,6 @@ int RTABMapApp::Render()
 #ifdef DEBUG_RENDERING_PERFORMANCE
 									LOGW("Adding mesh to scene: %fs", time.ticks());
 #endif
-									long estimateCPUMem = 0;
-									estimateCPUMem += mesh.cloud->size()*16; // 3*float + 1 float rgb
-									estimateCPUMem += mesh.indices->size()*4; // int
-									estimateCPUMem += mesh.polygons.size()*4*3; // 3 indices per polygon
-
-									processMemoryUsedBytes += estimateCPUMem;
-									processGPUMemoryUsedBytes += estimateCPUMem + (mesh.texture.empty()?0:mesh.polygons.size()*3*8+mesh.texture.total());
 									mesh.texture = cv::Mat(); // don't keep textures in memory
 								}
 							}
@@ -1500,7 +1572,7 @@ int RTABMapApp::Render()
 										odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
 										odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
 									   (int)cloud->width, (int)cloud->height);
-								main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*mapOdom*odomEvent.pose());
+								main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
 								main_scene_.setCloudVisible(-1, true);
 							}
 							else
@@ -1584,6 +1656,12 @@ int RTABMapApp::Render()
 				rtabmapEvents.clear();
 
 				lastPostRenderEventTime_ = UTimer::now();
+
+				if(lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
+				{
+					UERROR("TangoPoseEventNotReceived");
+					UEventsManager::post(new rtabmap::CameraTangoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_)));
+				}
 			}
 		}
 
@@ -1697,7 +1775,10 @@ void RTABMapApp::setPausedMapping(bool paused)
 {
 	{
 		boost::mutex::scoped_lock  lock(renderingMutex_);
-		visualizingMesh_ = false;
+		if(!localizationMode_)
+		{
+			visualizingMesh_ = false;
+		}
 		main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
 	}
 	paused_ = paused;
@@ -1975,6 +2056,14 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string &
 	}
 }
 
+void RTABMapApp::setGPS(const rtabmap::GPS & gps)
+{
+	if(camera_)
+	{
+		camera_->setGPS(gps);
+	}
+}
+
 void RTABMapApp::resetMapping()
 {
 	LOGW("Reset!");
@@ -1984,6 +2073,11 @@ void RTABMapApp::resetMapping()
 	mapToOdom_.setIdentity();
 	clearSceneOnNextRender_ = true;
 
+	if(camera_)
+	{
+		camera_->resetOrigin();
+	}
+
 	UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdResetMemory));
 }
 
@@ -2012,12 +2106,19 @@ void RTABMapApp::save(const std::string & databasePath)
 		dataRecorderMode_ = false;
 	}
 
-	if(appendModeBackup || dataRecorderModeBackup)
+	bool localizationModeBackup = localizationMode_;
+	if(localizationMode_)
+	{
+		localizationMode_ = false;
+	}
+
+	if(appendModeBackup || dataRecorderModeBackup || localizationModeBackup)
 	{
 		rtabmap::ParametersMap parameters = getRtabmapParameters();
 		rtabmap_->parseParameters(parameters);
 		appendMode_ = appendModeBackup;
 		dataRecorderMode_ = dataRecorderModeBackup;
+		localizationMode_ = localizationModeBackup;
 	}
 
 	std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
@@ -2149,11 +2250,12 @@ bool RTABMapApp::exportMesh(
 						++iter)
 					{
 						std::map<int, Mesh>::iterator jter = createdMeshes_.find(iter->first);
-						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 						pcl::IndicesPtr indices(new std::vector<int>);
 						rtabmap::CameraModel model;
 						cv::Mat depth;
-						float gains[3] = {1.0f};
+						float gains[3];
+						gains[0] = gains[1] = gains[2] = 1.0f;
 						if(jter != createdMeshes_.end())
 						{
 							cloud = jter->second.cloud;
@@ -2193,7 +2295,7 @@ bool RTABMapApp::exportMesh(
 							}
 
 							Eigen::Vector3f viewpoint( iter->second.x(),  iter->second.y(),  iter->second.z());
-							pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, normalK, viewpoint);
+							pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, normalK, 0.0f, viewpoint);
 
 							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 							pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals);
@@ -2272,7 +2374,7 @@ bool RTABMapApp::exportMesh(
 						poisson.setDepth(optimizedDepth);
 						poisson.setInputCloud(mergedClouds);
 						poisson.reconstruct(*mesh);
-						LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), mesh->polygons.size());
+						LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), (int)mesh->polygons.size());
 
 						if(progressionStatus_.isCanceled())
 						{
@@ -2603,7 +2705,7 @@ bool RTABMapApp::exportMesh(
 						// save in database
 						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
-						cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database
+						cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
 						std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
 						polygons[0].resize(polygonMesh->polygons.size());
 						for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p)
@@ -2611,7 +2713,8 @@ bool RTABMapApp::exportMesh(
 							polygons[0][p] = polygonMesh->polygons[p].vertices;
 						}
 						boost::mutex::scoped_lock  lock(rtabmapMutex_);
-						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons);
+
+						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons);
 						success = true;
 					}
 				}
@@ -2619,7 +2722,7 @@ bool RTABMapApp::exportMesh(
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
-					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database
+					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false)); // for database
 
 					// save in database
 					std::vector<std::vector<std::vector<unsigned int> > > polygons(textureMesh->tex_polygons.size());
@@ -2632,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
@@ -2655,7 +2758,8 @@ bool RTABMapApp::exportMesh(
 				std::map<int, Mesh>::iterator jter=createdMeshes_.find(iter->first);
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 				pcl::IndicesPtr indices(new std::vector<int>);
-				float gains[3] = {1.0f};
+				float gains[3];
+				gains[0] = gains[1] = gains[2] = 1.0f;
 				if(regenerateCloud)
 				{
 					if(jter != createdMeshes_.end())
@@ -2754,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;
 				}
 			}
@@ -2784,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;
 }
 
@@ -2793,10 +2911,10 @@ bool RTABMapApp::postExportation(bool visualize)
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
 	exportedMeshUpdated_ = false;
-	visualizingMesh_ = false;
 
 	if(visualize)
 	{
+		visualizingMesh_ = false;
 		cv::Mat cloudMat;
 		std::vector<std::vector<std::vector<unsigned int> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -2805,10 +2923,9 @@ 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.");
@@ -2825,6 +2942,19 @@ bool RTABMapApp::postExportation(bool visualize)
 			}
 		}
 	}
+	else if(visualizingMesh_)
+	{
+		rtabmapMutex_.lock();
+		if(!rtabmap_->getLocalOptimizedPoses().empty())
+		{
+			rtabmap::Statistics stats;
+			stats.setPoses(rtabmap_->getLocalOptimizedPoses());
+			rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
+		}
+		rtabmapMutex_.unlock();
+
+		visualizingMesh_ = false;
+	}
 
 	return visualizingMesh_;
 }
@@ -2846,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!");
@@ -3062,8 +3191,16 @@ bool RTABMapApp::handleEvent(UEvent * event)
 			LOGI("Received RtabmapEvent event!");
 			if(camera_->isRunning())
 			{
-				boost::mutex::scoped_lock lock(rtabmapMutex_);
-				rtabmapEvents_.push_back((rtabmap::RtabmapEvent*)event);
+				if(visualizingMesh_)
+				{
+					boost::mutex::scoped_lock lock(visLocalizationMutex_);
+					visLocalizationEvents_.push_back((rtabmap::RtabmapEvent*)event);
+				}
+				else
+				{
+					boost::mutex::scoped_lock lock(rtabmapMutex_);
+					rtabmapEvents_.push_back((rtabmap::RtabmapEvent*)event);
+				}
 				return true;
 			}
 		}
@@ -3170,8 +3307,11 @@ bool RTABMapApp::handleEvent(UEvent * event)
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopVisual_matches(), uValue(stats.data(), rtabmap::Statistics::kLoopVisual_matches(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopRejectedHypothesis(), uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopOptimization_max_error(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f)));
+			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopOptimization_max_error_ratio(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error_ratio(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryRehearsal_sim(), uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f)));
+			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f)));
+			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryFast_movement(), uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f)));
 		}
 		// else use last data
 
@@ -3185,8 +3325,17 @@ bool RTABMapApp::handleEvent(UEvent * event)
 		int matches = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopVisual_matches(), 0.0f);
 		int rejected = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
 		float optimizationMaxError = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f);
+		float optimizationMaxErrorRatio = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error_ratio(), 0.0f);
 		float rehearsalValue = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f);
 		float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
+		float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
+		int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
+		rtabmap::Transform currentPose = main_scene_.GetCameraPose();
+		float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f;
+		if(!currentPose.isNull())
+		{
+			currentPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		}
 
 		// Call JAVA callback with some stats
 		UINFO("Send statistics to GUI");
@@ -3200,7 +3349,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFF)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFFFFIFFFFFF)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -3211,7 +3360,6 @@ bool RTABMapApp::handleEvent(UEvent * event)
 								updateTime,
 								loopClosureId,
 								highestHypId,
-								(int)((processMemoryUsedBytes+processGPUMemoryUsedBytes)/(1024*1024)),
 								databaseMemoryUsed,
 								inliers,
 								matches,
@@ -3221,7 +3369,16 @@ bool RTABMapApp::handleEvent(UEvent * event)
 								renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
 								rejected,
 								rehearsalValue,
-								optimizationMaxError);
+								optimizationMaxError,
+								optimizationMaxErrorRatio,
+								distanceTravelled,
+								fastMovement,
+								x,
+								y,
+								z,
+								roll,
+								pitch,
+								yaw);
 						success = true;
 					}
 				}

app/android/jni/RTABMapApp.h

@@ -147,6 +147,7 @@ class RTABMapApp : public UEventsHandler {
   void setRenderingTextureDecimation(int value);
   void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
+  void setGPS(const rtabmap::GPS & gps);
 
   void resetMapping();
   void save(const std::string & databasePath);
@@ -227,26 +228,29 @@ class RTABMapApp : public UEventsHandler {
   int lastDrawnCloudsCount_;
   float renderingTime_;
   double lastPostRenderEventTime_;
-  long processMemoryUsedBytes;
-  long processGPUMemoryUsedBytes;
+  double lastPoseEventTime_;
   std::map<std::string, float> bufferedStatsData_;
 
   bool visualizingMesh_;
   bool exportedMeshUpdated_;
   pcl::TextureMesh::Ptr optMesh_;
   cv::Mat optTexture_;
+  int optRefId_;
+  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
   Scene main_scene_;
 
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> visLocalizationEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 
 	rtabmap::Transform mapToOdom_;
 
 	boost::mutex rtabmapMutex_;
+	boost::mutex visLocalizationMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;

app/android/jni/jni_interface.cpp

@@ -339,6 +339,24 @@ Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
 	return app.setMappingParameter(keyC, valueC);
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGPS(
+		JNIEnv*, jobject,
+		double stamp,
+		double longitude,
+		double latitude,
+		double altitude,
+		double accuracy,
+		double bearing)
+{
+	return app.setGPS(rtabmap::GPS(stamp,
+			longitude,
+			latitude,
+			altitude,
+			accuracy,
+			bearing));
+}
+
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
 		JNIEnv*, jobject)

app/android/res/layout/activity_rtabmap.xml

@@ -96,9 +96,9 @@
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
         android:layout_marginRight="5dp"
-        android:layout_alignParentTop="true"
         android:layout_marginTop="10dp"
         android:layout_alignParentRight="true"
+        android:layout_below="@+id/pause_button"
         android:text="@string/share_to_sketchfab" />
 
     <Button

app/android/res/layout/activity_settings.xml

@@ -58,12 +58,12 @@
    			android:entries="@array/pref_background_color_keys"
    			android:entryValues="@array/pref_background_color_values"
    			android:defaultValue="@string/pref_default_background_color"/>
-        <SwitchPreference
+        <com.introlab.rtabmap.CustomSwitchPreference
             android:key="@string/pref_key_blending"
             android:title="@string/pref_title_blending"
             android:summary="@string/pref_summary_blending"
             android:defaultValue="@string/pref_default_blending"/>
-        <SwitchPreference
+        <com.introlab.rtabmap.CustomSwitchPreference
             android:key="@string/pref_key_nodes_filtering"
             android:title="@string/pref_title_nodes_filtering"
             android:summary="@string/pref_summary_nodes_filtering"
@@ -79,22 +79,22 @@
             android:summary="@string/pref_summary_mapping"
             android:persistent="false">
         
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_append"
 	            android:title="@string/pref_title_append"
 	            android:summary="@string/pref_summary_append"
 	            android:defaultValue="@string/pref_default_append"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_resolution"
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"
 		        android:summary="@string/pref_summary_smoothing"
 		        android:defaultValue="@string/pref_default_smoothing"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_fisheye"
 		        android:title="@string/pref_title_fisheye"
 		        android:summary="@string/pref_summary_fisheye"
@@ -109,6 +109,13 @@
 		            android:entries="@array/pref_update_rate_keys"
 		   			android:entryValues="@array/pref_update_rate_values"
 		   			android:defaultValue="@string/pref_default_update_rate"/>
+		        <ListPreference
+		            android:key="@string/pref_key_max_speed"
+		            android:title="@string/pref_title_max_speed"
+		            android:summary="@string/pref_summary_max_speed"
+		            android:entries="@array/pref_max_speed_keys"
+		   			android:entryValues="@array/pref_max_speed_values"
+		   			android:defaultValue="@string/pref_default_max_speed"/>
 		        <ListPreference 
 		            android:key="@string/pref_key_time_thr"
 		    		android:title="@string/pref_title_time_thr"
@@ -179,7 +186,7 @@
 		   			android:entries="@array/pref_optimizer_keys"
 		   			android:entryValues="@array/pref_optimizer_values"
 		   			android:defaultValue="@string/pref_default_optimizer"/>
-		        <SwitchPreference 
+		        <com.introlab.rtabmap.CustomSwitchPreference 
 		            android:key="@string/pref_key_optimize_end"
 		    		android:title="@string/pref_title_optimize_end"
 		    		android:summary="@string/pref_summary_optimize_end"
@@ -187,17 +194,22 @@
 		    </PreferenceCategory>
 	        <PreferenceCategory
 	        	android:title="@string/pref_title_mapping_database">
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_keep_all_db"
 		            android:title="@string/pref_title_keep_all_db"
 		            android:summary="@string/pref_summary_keep_all_db"
 		            android:defaultValue="@string/pref_default_keep_all_db"/>
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_raw_scan_saved"
 		            android:title="@string/pref_title_raw_scan_saved"
 		            android:summary="@string/pref_summary_raw_scan_saved"
 		            android:defaultValue="@string/pref_default_raw_scan_saved"/>
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_gps_saved"
+		            android:title="@string/pref_title_gps_saved"
+		            android:summary="@string/pref_summary_gps_saved"
+		            android:defaultValue="@string/pref_default_gps_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_db_in_memory"
 		            android:title="@string/pref_title_db_in_memory"
 		            android:summary="@string/pref_summary_db_in_memory"
@@ -259,7 +271,7 @@
 	   			android:entryValues="@array/pref_min_texture_cluster_size_values"
 	   			android:defaultValue="@string/pref_default_min_texture_cluster_size"/>
 	        
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_block_render"
 	            android:title="@string/pref_title_block_render"
 	            android:summary="@string/pref_summary_block_render"
@@ -286,7 +298,7 @@
 		   			android:entryValues="@array/pref_opt_color_radius_values"
 		   			android:defaultValue="@string/pref_default_opt_color_radius"/>
 		        
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_opt_clean_white"
 		            android:title="@string/pref_title_opt_clean_white"
 		            android:summary="@string/pref_summary_opt_clean_white"
@@ -316,7 +328,7 @@
    			android:entries="@array/pref_cluster_ratio_keys"
    			android:entryValues="@array/pref_cluster_ratio_values"
    			android:defaultValue="@string/pref_default_cluster_ratio"/>
-        <SwitchPreference 
+        <com.introlab.rtabmap.CustomSwitchPreference 
             android:key="@string/pref_key_notification_sound"
     		android:title="@string/pref_title_notification_sound"
     		android:summary="@string/pref_summary_notification_sound"

app/android/res/values/strings.xml

@@ -34,6 +34,9 @@
     <string name="memory">"Used Memory (MB): "</string>
     <string name="hypothesis">"Hypothesis (%): "</string>
     <string name="fps">"FPS (rendering): "</string>
+    <string name="distance">"Distance travelled: "</string>
+    <string name="gps">"GPS (long,lat,alt,bearing,err): "</string>
+    <string name="time">"Time: "</string>
     
     <!-- Preference keys: BEGIN -->
     <string name="pref_key_tags">pref_key_tags</string>
@@ -51,7 +54,7 @@
     <string name="pref_key_depth">pref_key_depth</string>
     <string name="pref_default_depth">2.5</string>
     <string name="pref_key_point_size">pref_key_point_size</string>
-    <string name="pref_default_point_size">5</string>
+    <string name="pref_default_point_size">10</string>
     <string name="pref_key_angle">pref_key_angle</string>
     <string name="pref_default_angle">20</string>
     <string name="pref_key_triangle">pref_key_triangle</string>
@@ -75,6 +78,8 @@
     
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
+    <string name="pref_key_max_speed">pref_key_max_speed</string>
+    <string name="pref_default_max_speed">0</string>
     <string name="pref_key_time_thr">pref_key_time_thr</string>
     <string name="pref_default_time_thr">1000</string>
     <string name="pref_key_mem_thr">pref_key_mem_thr</string>
@@ -86,7 +91,7 @@
     <string name="pref_key_min_inliers">pref_key_min_inliers</string>
     <string name="pref_default_min_inliers">25</string>
     <string name="pref_key_opt_error">pref_key_opt_error</string>
-    <string name="pref_default_opt_error">0.1</string>
+    <string name="pref_default_opt_error">2</string>
     <string name="pref_key_features_voc">pref_key_features_voc</string>
     <string name="pref_default_features_voc">200</string>
     <string name="pref_key_features">pref_key_features</string>
@@ -101,8 +106,10 @@
     <string name="pref_default_keep_all_db">true</string>
     <string name="pref_key_raw_scan_saved">pref_key_raw_scan_saved</string>
     <string name="pref_default_raw_scan_saved">false</string>
+    <string name="pref_key_gps_saved">pref_key_gps_saved</string>
+    <string name="pref_default_gps_saved">false</string>
     <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
-    <string name="pref_default_db_in_memory">true</string>
+    <string name="pref_default_db_in_memory">false</string>
     
     <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
     <string name="pref_default_cloud_voxel">0.01</string>
@@ -313,6 +320,8 @@
     <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
     <string name="pref_title_update_rate">Update Rate</string>
     <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
+    <string name="pref_title_max_speed">Maximum Motion Speed</string>
+    <string name="pref_summary_max_speed">Images taken when the camera is moving too fast are ignored to avoid blurry textures.</string>
     <string name="pref_title_time_thr">Time Limit</string>
     <string name="pref_summary_time_thr">Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates.</string>
     <string name="pref_title_mem_thr">Memory Limit</string>
@@ -324,7 +333,7 @@
     <string name="pref_title_min_inliers">Min Inliers</string>
     <string name="pref_summary_min_inliers">Minimum visual inliers to accept a loop closure.</string>
     <string name="pref_title_opt_error">Max Optimization Error</string>
-    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this threshold.</string>
+    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this factor of the link\'s variance.</string>
     <string name="pref_title_features_voc">Max Features Extracted (Vocabulary)</string>
     <string name="pref_summary_features_voc">Extracting more features per image would result in better loop closure hypotheses but more processing time is required.</string>
     <string name="pref_title_features">Max Features Extracted (Loop Closure)</string>
@@ -339,6 +348,8 @@
     <string name="pref_summary_keep_all_db">Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop.</string>
     <string name="pref_title_raw_scan_saved">Save Raw Scan</string>
     <string name="pref_summary_raw_scan_saved">Save raw point clouds in database.</string>
+    <string name="pref_title_gps_saved">Save GPS</string>
+    <string name="pref_summary_gps_saved">Save GPS in database.</string>
     <string name="pref_title_db_in_memory">Database In Memory</string>
     <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
 
@@ -360,6 +371,20 @@
         <item>"1"</item>
         <item>"0.5"</item>
     </string-array>
+    <string-array name="pref_max_speed_keys">
+        <item>"No Limit"</item>
+        <item>"High"</item>
+        <item>"Medium"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_max_speed_values">
+        <item>"0"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+    </string-array>
     <string-array name="pref_time_thr_keys">
         <item>"No Limit"</item>
         <item>"1500 ms"</item>
@@ -469,25 +494,29 @@
         <item>"10"</item>
     </string-array>
     <string-array name="pref_opt_error_keys">
-        <item>"1.0 m"</item>
-        <item>"0.5 m"</item>
-        <item>"0.35 m"</item>
-        <item>"0.2 m"</item>
-        <item>"0.1 m"</item>
-        <item>"0.05 m"</item>
-        <item>"0.025 m"</item>
-        <item>"0.01 m"</item>
+        <item>"10x"</item>
+        <item>"9x"</item>
+        <item>"8x"</item>
+        <item>"7x"</item>
+        <item>"6x"</item>
+        <item>"5x"</item>
+        <item>"4x"</item>
+        <item>"3x"</item>
+        <item>"2x"</item>
+        <item>"1x"</item>
         <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_opt_error_values">
-        <item>"1.0"</item>
-        <item>"0.5"</item>
-        <item>"0.35"</item>
-        <item>"0.2"</item>
-        <item>"0.1"</item>
-        <item>"0.05"</item>
-        <item>"0.025"</item>
-        <item>"0.01"</item>
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
         <item>"0"</item>
     </string-array>
     <string-array name="pref_features_voc_keys">

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

@@ -0,0 +1,82 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.preference.SwitchPreference;
+import android.util.AttributeSet;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.Switch;
+
+/**
+ * 
+ * @author mathieu
+ * Bug fix of switch preferences changing states 
+ * when scrolling on Jelly Bean: 
+ * https://issuetracker.google.com/issues/36941388#comment4
+ */
+
+public class CustomSwitchPreference extends SwitchPreference { 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     * @param defStyle Theme attribute defining the default style options 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs, int defStyle) { 
+        super(context, attrs, defStyle); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs) { 
+        super(context, attrs); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with default style options. 
+     * 
+     * @param context The Context that will style this preference 
+     */ 
+    public CustomSwitchPreference(Context context) { 
+        super(context, null); 
+    } 
+
+    @Override 
+    protected void onBindView(View view) { 
+        // Clean listener before invoke SwitchPreference.onBindView 
+        ViewGroup viewGroup= (ViewGroup)view; 
+        clearListenerInViewGroup(viewGroup); 
+        super.onBindView(view); 
+    } 
+
+    /** 
+     * Clear listener in Switch for specify ViewGroup. 
+     * 
+     * @param viewGroup The ViewGroup that will need to clear the listener. 
+     */ 
+    private void clearListenerInViewGroup(ViewGroup viewGroup) { 
+        if (null == viewGroup) { 
+            return; 
+        } 
+
+        int count = viewGroup.getChildCount(); 
+        for(int n = 0; n < count; ++n) { 
+            View childView = viewGroup.getChildAt(n); 
+            if(childView instanceof Switch) { 
+                final Switch switchView = (Switch) childView; 
+                switchView.setOnCheckedChangeListener(null); 
+                return; 
+            } else if (childView instanceof ViewGroup){ 
+                ViewGroup childGroup = (ViewGroup)childView; 
+                clearListenerInViewGroup(childGroup); 
+            } 
+        } 
+    } 
+
+}

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

@@ -92,6 +92,13 @@ public class RTABMapLib
     public static native void setRenderingTextureDecimation(int value);
     public static native void setBackgroundColor(float gray);
     public static native int setMappingParameter(String key, String value);
+    public static native void setGPS(
+			double stamp,
+			double longitude, 
+			double latitude, 
+			double altitude,  
+			double accuracy,
+			double bearing);
 
     public static native void resetMapping();
     public static native void save(String outputDatabasePath);

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

@@ -195,6 +195,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
         ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
@@ -253,6 +254,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
         	if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));

cmake_modules/FindFlyCapture2.cmake

@@ -19,13 +19,13 @@ set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib)
 endif()
 
 #FlyCapture2 SDK
-find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include)
-find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR})
+find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include $ENV{FlyCapture2_ROOT_DIR}/include/flycapture)
+find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 flycapture NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR})
 
 # Triclops SDK
-find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include)
-find_library(Triclops_LIBRARY NAMES triclops triclops_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include $ENV{Triclops_ROOT_DIR}/include/triclops)
+find_library(Triclops_LIBRARY NAMES triclops triclops_v100 libtriclops.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 libflycapture2bridge.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
 find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
 
 IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)

cmake_modules/FindG2O.cmake

@@ -17,6 +17,14 @@ FIND_LIBRARY(CHOLMOD_LIB cholmod)
 FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 
   PATHS "C:\\Program Files\\g2o\\include")
 
+FIND_FILE(G2O_CONFIG_FILE g2o/config.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
+#ifdef G2O_NUMBER_FORMAT_STR
+#define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
+#endif
+
 # Macro to unify finding both the debug and release versions of the
 # libraries; this is adapted from the rtabmap config
 
@@ -75,7 +83,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER
 
 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
   SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
   SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -105,5 +113,15 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	  ${CHOLMOD_LIB})
   ENDIF(G2O_SOLVER_CHOLMOD)
 
+  FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
+  STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
+  IF(${matchres} EQUAL -1)
+    MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
+    SET(G2O_CPP11 0)
+  ELSE()
+    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
+    SET(G2O_CPP11 1)
+  ENDIF()
+
   SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)

cmake_modules/FindKinectSDK2.cmake

@@ -0,0 +1,183 @@
+#.rst:
+# FindKinectSDK2
+# --------------
+#
+# Find Kinect for Windows SDK v2 (Kinect SDK v2) include dirs, library dirs, libraries
+#
+# Use this module by invoking find_package with the form::
+#
+#    find_package( KinectSDK2 [REQUIRED] )
+#
+# Results for users are reported in following variables::
+#
+#    KinectSDK2_FOUND                - Return "TRUE" when Kinect SDK v2 found. Otherwise, Return "FALSE".
+#    KinectSDK2_INCLUDE_DIRS         - Kinect SDK v2 include directories. (${KinectSDK2_DIR}/inc)
+#    KinectSDK2_LIBRARY_DIRS         - Kinect SDK v2 library directories. (${KinectSDK2_DIR}/Lib/x86 or ${KinectSDK2_DIR}/Lib/x64)
+#    KinectSDK2_LIBRARIES            - Kinect SDK v2 library files. (${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib (If check the box of any application festures, corresponding library will be added.))
+#    KinectSDK2_COMMANDS             - Copy commands of redist files for application functions of Kinect SDK v2. (If uncheck the box of all application features, this variable has defined empty command.)
+#
+# This module reads hints about search locations from following environment variables::
+#
+#    KINECTSDK20_DIR                 - Kinect SDK v2 root directory. (This environment variable has been set by installer of Kinect SDK v2.)
+#
+# CMake entries::
+#
+#    KinectSDK2_DIR                  - Kinect SDK v2 root directory. (Default $ENV{KINECTSDK20_DIR})
+#    KinectSDK2_FACE                 - Check the box when using Face or HDFace features. (Default uncheck)
+#    KinectSDK2_FUSION               - Check the box when using Fusion features. (Default uncheck)
+#    KinectSDK2_VGB                  - Check the box when using Visual Gesture Builder features. (Default uncheck)
+#
+# Example to find Kinect SDK v2::
+#
+#    cmake_minimum_required( VERSION 2.8 )
+#
+#    project( project )
+#    add_executable( project main.cpp )
+#    set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "project" )
+#
+#    # Find package using this module.
+#    find_package( KinectSDK2 REQUIRED )
+#
+#    if(KinectSDK2_FOUND)
+#      # [C/C++]>[General]>[Additional Include Directories]
+#      include_directories( ${KinectSDK2_INCLUDE_DIRS} )
+#
+#      # [Linker]>[General]>[Additional Library Directories]
+#      link_directories( ${KinectSDK2_LIBRARY_DIRS} )
+#
+#      # [Linker]>[Input]>[Additional Dependencies]
+#      target_link_libraries( project ${KinectSDK2_LIBRARIES} )
+#
+#      # [Build Events]>[Post-Build Event]>[Command Line]
+#      add_custom_command( TARGET project POST_BUILD ${KinectSDK2_COMMANDS} )
+#    endif()
+#
+# =============================================================================
+#
+# Copyright (c) 2016 Tsukasa SUGIURA
+# Distributed under the MIT License.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+#
+# =============================================================================
+
+##### Utility #####
+
+# Check Directory Macro
+macro(CHECK_DIR _DIR)
+  if(NOT EXISTS "${${_DIR}}")
+    message(WARNING "Directory \"${${_DIR}}\" not found.")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_DIR)
+  endif()
+endmacro()
+
+# Check Files Macro
+macro(CHECK_FILES _FILES _DIR)
+  set(_MISSING_FILES)
+  foreach(_FILE ${${_FILES}})
+    if(NOT EXISTS "${_FILE}")
+      get_filename_component(_FILE ${_FILE} NAME)
+      set(_MISSING_FILES "${_MISSING_FILES}${_FILE}, ")
+    endif()
+  endforeach()
+  if(_MISSING_FILES)
+    message(WARNING "In directory \"${${_DIR}}\" not found files: ${_MISSING_FILES}")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_FILES)
+  endif()
+endmacro()
+
+# Target Platform
+set(TARGET_PLATFORM)
+if(NOT CMAKE_CL_64)
+  set(TARGET_PLATFORM x86)
+else()
+  set(TARGET_PLATFORM x64)
+endif()
+
+##### Find Kinect SDK v2 #####
+
+# Found
+set(KinectSDK2_FOUND TRUE)
+if(MSVC_VERSION LESS 1700)
+  message(WARNING "Kinect for Windows SDK v2 supported Visual Studio 2012 or later.")
+  set(KinectSDK2_FOUND FALSE)
+endif()
+
+# Options
+option(KinectSDK2_FACE "Face and HDFace features" FALSE)
+option(KinectSDK2_FUSION "Fusion features" FALSE)
+option(KinectSDK2_VGB "Visual Gesture Builder features" FALSE)
+
+# Root Directoty
+set(KinectSDK2_DIR)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_DIR $ENV{KINECTSDK20_DIR} CACHE PATH "Kinect for Windows SDK v2 Install Path." FORCE)
+  check_dir(KinectSDK2_DIR)
+endif()
+
+# Include Directories
+set(KinectSDK2_INCLUDE_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_INCLUDE_DIRS ${KinectSDK2_DIR}/inc)
+  check_dir(KinectSDK2_INCLUDE_DIRS)
+endif()
+
+# Library Directories
+set(KinectSDK2_LIBRARY_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARY_DIRS ${KinectSDK2_DIR}/Lib/${TARGET_PLATFORM})
+  check_dir(KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Dependencies
+set(KinectSDK2_LIBRARIES)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib)
+
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Face.lib)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Fusion.lib)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.VisualGestureBuilder.lib)
+  endif()
+
+  check_files(KinectSDK2_LIBRARIES KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Custom Commands
+set(KinectSDK2_COMMANDS)
+if(KinectSDK2_FOUND)
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Face/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Fusion/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/VGB/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  # Empty Commands
+  if(NOT KinectSDK2_COMMANDS)
+    set(KinectSDK2_COMMANDS COMMAND)
+  endif()
+endif()
+
+message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")

cmake_modules/FindORB_SLAM2.cmake

@@ -9,13 +9,15 @@
 
 find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
 find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
-find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib)
+find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
+find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
 
-IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
    SET(ORB_SLAM2_FOUND TRUE)
-   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
-   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY})
-ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
+   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
+   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY})
+ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
 
 IF (ORB_SLAM2_FOUND)
    # show which ORB_SLAM2 was found only if not quiet

corelib/include/rtabmap/core/BayesFilter.h

@@ -59,18 +59,14 @@ public:
 	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
 	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
 
-	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
+	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,
 			const std::vector<int> & oldIds,
-			const std::vector<int> & newIds) const;
+			const std::vector<int> & newIds);
 	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
-	float addNeighborProb(cv::Mat & prediction,
-			unsigned int col,
-			const std::map<int, int> & neighbors,
-			const std::map<int, int> & idToIndexMap) const;
 	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
 
 private:
@@ -80,6 +76,7 @@ private:
 	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
 	bool _fullPredictionUpdate;
 	float _totalPredictionLCValues;
+	std::map<int, std::map<int, int> > _neighborsIndex;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Camera.h

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

corelib/include/rtabmap/core/CameraInfo.h

@@ -59,6 +59,7 @@ public:
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
+	float timeStereoExposureCompensation;
 	float timeImageDecimation;
 	float timeScanFromDepth;
 	float timeUndistortDepth;
@@ -66,6 +67,7 @@ public:
 	float timeTotal;
 	Transform odomPose;
 	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraRGB.h

@@ -68,7 +68,7 @@ public:
 	const CameraModel & cameraModel() const {return _model;}
 
 	void setPath(const std::string & dir) {_path=dir;}
-	void setStartIndex(int index) {_startAt = index;} // negative means last
+	virtual void setStartIndex(int index) {_startAt = index;} // negative means last
 	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
 	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
 	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
@@ -85,19 +85,19 @@ public:
 			int maxScanPts = 0,
 			int downsampleStep = 1,
 			float voxelSize = 0.0f,
-			int normalsK = 0, // compute normals if > 0
-			const Transform & localTransform=Transform::getIdentity())
+			int normalsK = 0,        // compute normals if > 0
+			float normalsRadius = 0, // compute normals if > 0
+			const Transform & localTransform=Transform::getIdentity(),
+			bool forceGroundNormalsUp = false)
 	{
 		_scanPath = dir;
 		_scanLocalTransform = localTransform;
 		_scanMaxPts = maxScanPts;
 		_scanDownsampleStep = downsampleStep;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 		_scanVoxelSize = voxelSize;
-		if(_scanDownsampleStep>1)
-		{
-			_scanMaxPts /= _scanDownsampleStep;
-		}
+		_scanForceGroundNormalsUp = forceGroundNormalsUp;
 	}
 
 	void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
@@ -121,6 +121,9 @@ public:
 		_groundTruthFormat = format;
 	}
 
+	void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
+	double getMaxPoseTimeDiff() const {return _maxPoseTimeDiff;}
+
 	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
 	{
 		_isDepth = isDepth;
@@ -133,7 +136,8 @@ protected:
 			std::list<Transform> & outputPoses,
 			std::list<double> & stamps,
 			const std::string & filePath,
-			int format) const;
+			int format,
+			double maxTimeDiff) const;
 
 private:
 	std::string _path;
@@ -158,6 +162,8 @@ private:
 	int _scanDownsampleStep;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
+	bool _scanForceGroundNormalsUp;
 
 	bool _depthFromScan;
 	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
@@ -169,9 +175,9 @@ private:
 
 	std::string _odometryPath;
 	int _odometryFormat;
-
 	std::string _groundTruthPath;
 	int _groundTruthFormat;
+	double _maxPoseTimeDiff;
 
 	std::list<double> _stamps;
 	std::list<Transform> odometry_;

corelib/include/rtabmap/core/CameraRGBD.h

@@ -77,6 +77,13 @@ namespace rs
 typedef struct _freenect_context freenect_context;
 typedef struct _freenect_device freenect_device;
 
+typedef struct IKinectSensor IKinectSensor;
+typedef struct ICoordinateMapper ICoordinateMapper;
+typedef struct _DepthSpacePoint DepthSpacePoint;
+typedef struct _ColorSpacePoint ColorSpacePoint;
+typedef struct tagRGBQUAD RGBQUAD;
+typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
+
 namespace rtabmap
 {
 
@@ -178,6 +185,7 @@ public:
 	bool setGain(int value);
 	bool setMirroring(bool enabled);
 	void setOpenNI2StampsAndIDsUsed(bool used);
+	void setIRDepthShift(int horizontal, int vertical);
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -193,6 +201,8 @@ private:
 	std::string _deviceId;
 	bool _openNI2StampsAndIDsUsed;
 	StereoCameraModel _stereoModel;
+	int _depthHShift;
+	int _depthVShift;
 #endif
 };
 
@@ -256,14 +266,15 @@ public:
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect2(int deviceId= 0,
-					Type type = kTypeColor2DepthSD,
+					Type type = kTypeDepth2ColorSD,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity(),
 					float minDepth = 0.3f,
 					float maxDepth = 12.0f,
 					bool bilateralFiltering = true,
 					bool edgeAwareFiltering = true,
-					bool noiseFiltering = true);
+					bool noiseFiltering = true,
+					const std::string & pipelineName = "");
 	virtual ~CameraFreenect2();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -287,6 +298,61 @@ private:
 	bool bilateralFiltering_;
 	bool edgeAwareFiltering_;
 	bool noiseFiltering_;
+	std::string pipelineName_;
+#endif
+};
+
+/////////////////////////
+// CameraK4W2
+/////////////////////////
+
+class RTABMAP_EXP CameraK4W2 :
+	public Camera
+{
+public:
+	static bool available();
+
+	enum Type {
+		kTypeColor2DepthSD,
+		kTypeDepth2ColorSD,
+		kTypeDepth2ColorHD
+	};
+
+public:
+	static const int        cDepthWidth = 512;
+	static const int        cDepthHeight = 424;
+	static const int        cColorWidth = 1920;
+	static const int        cColorHeight = 1080;
+
+public:
+	// default local transform z in, x right, y down));
+	CameraK4W2(int deviceId = 0, // not used
+		Type type = kTypeDepth2ColorSD,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraK4W2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+	void close();
+
+private:
+#ifdef RTABMAP_K4W2
+	Type type_;
+	IKinectSensor*          pKinectSensor_;
+	ICoordinateMapper*      pCoordinateMapper_;
+	DepthSpacePoint*        pDepthCoordinates_;
+	ColorSpacePoint*        pColorCoordinates_;
+	IMultiSourceFrameReader* pMultiSourceFrameReader_;
+	RGBQUAD * pColorRGBX_;
+	INT_PTR hMSEvent;
+	CameraModel colorCameraModel_;
 #endif
 };
 
@@ -363,6 +429,8 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 

corelib/include/rtabmap/core/CameraStereo.h

@@ -188,6 +188,8 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 

corelib/include/rtabmap/core/CameraThread.h

@@ -60,6 +60,7 @@ public:
 	virtual ~CameraThread();
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
@@ -73,13 +74,15 @@ public:
 			int decimation=4,
 			float maxDepth=4.0f,
 			float voxelSize = 0.0f,
-			int normalsK = 0)
+			int normalsK = 0,
+			int normalsRadius = 0.0f)
 	{
 		_scanFromDepth = enabled;
 		_scanDecimation=decimation;
 		_scanMaxDepth = maxDepth;
 		_scanVoxelSize = voxelSize;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 	}
 
 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -98,6 +101,7 @@ private:
 private:
 	Camera * _camera;
 	bool _mirroring;
+	bool _stereoExposureCompensation;
 	bool _colorOnly;
 	int _imageDecimation;
 	bool _stereoToDepth;
@@ -107,6 +111,7 @@ private:
 	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;

corelib/include/rtabmap/core/Compression.h

@@ -83,5 +83,8 @@ cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
 
+cv::Mat RTABMAP_EXP compressString(const std::string & str);
+std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -68,6 +68,7 @@ public:
 	virtual ~DBDriver();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return _url.empty();}
 	const std::string & getUrl() const {return _url;}
 
 	void beginTransaction() const;
@@ -91,6 +92,7 @@ public:
 				int nodeId,
 				const cv::Mat & ground,
 				const cv::Mat & obstacles,
+				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
@@ -100,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
@@ -111,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,
@@ -144,7 +148,9 @@ public:
 	int getTotalNodesSize() const;
 	int getTotalDictionarySize() const;
 	ParametersMap getLastParameters() const;
-	std::map<std::string, float> getStatistics(int nodeId, double & stamp) const;
+	std::map<std::string, float> getStatistics(int nodeId, double & stamp, std::vector<int> * wmState=0) const;
+	std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatistics() const;
+	std::map<int, std::vector<int> > getAllStatisticsWmStates() const;
 
 	void executeNoResult(const std::string & sql) const;
 
@@ -158,8 +164,8 @@ public:
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	bool getLaserScanInfo(int signatureId, LaserScanInfo & info) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
+	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -195,13 +201,15 @@ private:
 	virtual int getTotalNodesSizeQuery() const = 0;
 	virtual int getTotalDictionarySizeQuery() const = 0;
 	virtual ParametersMap getLastParametersQuery() const = 0;
-	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const = 0;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const = 0;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const = 0;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const = 0;
 
 	virtual void executeNoResultQuery(const std::string & sql) const = 0;
 
 	virtual void getWeightQuery(int signatureId, int & weight) const = 0;
 
-	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void saveQuery(const std::list<Signature *> & signatures) = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
@@ -213,6 +221,7 @@ private:
 				int nodeId,
 				const cv::Mat & ground,
 				const cv::Mat & obstacles,
+				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint) const = 0;
 
@@ -223,9 +232,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,
@@ -234,7 +246,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,
@@ -252,8 +263,8 @@ private:
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
-	virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const = 0;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
@@ -263,7 +274,7 @@ private:
 
 private:
 	//non-abstract methods
-	void saveOrUpdate(const std::vector<Signature *> & signatures) const;
+	void saveOrUpdate(const std::vector<Signature *> & signatures);
 	void saveOrUpdate(const std::vector<VisualWord *> & words) const;
 
 	//thread stuff

corelib/include/rtabmap/core/Features2d.h

@@ -177,6 +177,8 @@ private:
 	int _subPixWinSize;
 	int _subPixIterations;
 	double _subPixEps;
+	int gridRows_;
+	int gridCols_;
 	// Stereo stuff
 	Stereo * _stereo;
 };

corelib/include/rtabmap/core/FlannIndex.h

@@ -49,21 +49,25 @@ public:
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
 			const cv::Mat & features,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildKDTreeIndex(
 			const cv::Mat & features,
 			int trees = 4,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildKDTreeSingleIndex(
 			const cv::Mat & features,
 			int leafMaxSize = 10,
 			bool reorder = true,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildLSHIndex(
 			const cv::Mat & features,
 			unsigned int table_number = 12,
 			unsigned int key_size = 20,
-			unsigned int multi_probe_level = 2);
+			unsigned int multi_probe_level = 2,
+			float rebalancingFactor = 2.0f);
 
 	bool isBuilt();
 
@@ -74,7 +78,7 @@ public:
 
 	void removePoint(unsigned int index);
 
-	// return squared distances
+	// return squared distances (indices should be casted in size_t)
 	void knnSearch(
 			const cv::Mat & query,
 			cv::Mat & indices,
@@ -102,6 +106,7 @@ private:
 	int featuresDim_;
 	bool isLSH_;
 	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
+	float rebalancingFactor_;
 
 	// keep feature in memory until the tree is rebuilt
 	// (in case the word is deleted when removed from the VWDictionary)

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -25,6 +25,20 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+/*
+ * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
+ * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
+ */
+
+/* +---------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)               |
+   |                          http://www.mrpt.org/                             |
+   |                                                                           |
+   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
+   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
+   | Released under BSD License. See details in http://www.mrpt.org/License    |
+   +---------------------------------------------------------------------------+ */
+
 
 #ifndef GEODETICCOORDS_H_
 #define GEODETICCOORDS_H_
@@ -52,12 +66,58 @@ public:
 	cv::Point3d toGeocentric_WGS84() const;
 	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
 
+	void fromGeocentric_WGS84(const cv::Point3d& geocentric);
+	void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+
+	static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+
 private:
 	double latitude_;  // deg
 	double longitude_; // deg
 	double altitude_;  // m
 };
 
+class GPS
+{
+public:
+	GPS():
+		stamp_(0.0),
+		longitude_(0.0),
+		latitude_(0.0),
+		altitude_(0.0),
+		error_(0.0),
+		bearing_(0.0)
+	{}
+	GPS(const double & stamp,
+		const double & longitude,
+		const double & latitude,
+		const double & altitude,
+		const double & error,
+		const double & bearing):
+			stamp_(stamp),
+			longitude_(longitude),
+			latitude_(latitude),
+			altitude_(altitude),
+			error_(error),
+			bearing_(bearing)
+	{}
+	const double & stamp() const {return stamp_;}
+	const double & longitude() const {return longitude_;}
+	const double & latitude() const {return latitude_;}
+	const double & altitude() const {return altitude_;}
+	const double & error() const {return error_;}
+	const double & bearing() const {return bearing_;}
+
+	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
+private:
+	double stamp_;     // in sec
+	double longitude_; // DD
+	double latitude_;  // DD
+	double altitude_;  // m
+	double error_;     // m
+	double bearing_;   // deg (North 0->360 clockwise)
+};
+
 }
 
 #endif /* GEODETICCOORDS_H_ */

corelib/include/rtabmap/core/Graph.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <map>
 #include <list>
 #include <rtabmap/core/Link.h>
+#include <rtabmap/core/GeodeticCoords.h>
 
 namespace rtabmap {
 class Memory;
@@ -58,6 +59,11 @@ bool RTABMAP_EXP importPoses(
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1
 
+bool RTABMAP_EXP exportGPS(
+		const std::string & filePath,
+		const std::map<int, GPS> & gpsValues,
+		unsigned int rgba = 0xFFFFFFFF);
+
 /**
  * Compute translation and rotation errors for KITTI datasets.
  * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
@@ -72,6 +78,30 @@ void RTABMAP_EXP calcKittiSequenceErrors(
 		float & t_err,
 		float & r_err);
 
+/**
+ * Compute root-mean-square error (RMSE) like the TUM RGBD
+ * dataset's evaluation tool (absolute trajectory error).
+ * See https://vision.in.tum.de/data/datasets/rgbd-dataset
+ * @param groundTruth, Ground Truth poses
+ * @param poses, Estimated poses
+ * @return Gt to Map transform
+ */
+Transform RTABMAP_EXP calcRMSE(
+		const std::map<int, Transform> &groundTruth,
+		const std::map<int, Transform> &poses,
+		float & translational_rmse,
+		float & translational_mean,
+		float & translational_median,
+		float & translational_std,
+		float & translational_min,
+		float & translational_max,
+		float & rotational_rmse,
+		float & rotational_mean,
+		float & rotational_median,
+		float & rotational_std,
+		float & rotational_min,
+		float & rotational_max);
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
@@ -93,6 +123,8 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		int to,
 		bool checkBothWays = true);
 
+std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
+		const std::multimap<int, Link> & links);
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType);
@@ -197,6 +229,11 @@ int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose);
 
+std::vector<int> RTABMAP_EXP findNearestNodes(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose,
+		int k);
+
 /**
  * Get nodes near the query
  * @param nodeId the query id

corelib/include/rtabmap/core/IMU.h

@@ -0,0 +1,109 @@
+/*
+ * IMU.h
+ *
+ *  Created on: 2018-03-05
+ *      Author: mathieu
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include <opencv2/core/core.hpp>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+
+// Correspondence class to sensor_msgs/IMU
+class IMU
+{
+public:
+	IMU() {}
+	IMU(const cv::Vec4d & orientation,
+		const cv::Mat & orientationCovariance,
+		const cv::Vec3d & angularVelocity,
+		const cv::Mat & angularVelocityCovariance,
+		const cv::Vec3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			orientation_(orientation),
+			orientationCovariance_(orientationCovariance),
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			linearAccelerationCovariance_(linearAccelerationCovariance),
+			localTransform_(localTransform)
+	{
+		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,
+		const cv::Vec3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			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_;}
+	const cv::Mat & orientationCovariance() const {return orientationCovariance_;} // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+
+	const cv::Vec3d & angularVelocity() const {return angularVelocity_;}
+	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;} // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	const cv::Vec3d linearAcceleration() const {return linearAcceleration_;}
+	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;} // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	const Transform & localTransform() const {return localTransform_;}
+
+	bool empty() const
+	{
+		return orientationCovariance_.empty() && angularVelocityCovariance_.empty() && linearAccelerationCovariance_.empty();
+	}
+
+
+private:
+	cv::Vec4d orientation_;
+	cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+
+	cv::Vec3d angularVelocity_;
+	cv::Mat angularVelocityCovariance_; // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	cv::Vec3d linearAcceleration_;
+	cv::Mat linearAccelerationCovariance_; // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	Transform localTransform_;
+};
+
+class IMUEvent : public UEvent
+{
+public:
+	IMUEvent() :
+		stamp_(0.0)
+	{}
+	IMUEvent(const IMU & data, double stamp) :
+		data_(data),
+		stamp_(stamp)
+	{
+	}
+	virtual std::string getClassName() const {return "IMUEvent";}
+	const IMU & getData() const {return data_;}
+	double getStamp() const {return stamp_;}
+
+private:
+	IMU data_;
+	double stamp_;
+};
+
+}
+
+
+#endif /* IMU_H_ */

corelib/include/rtabmap/core/IMUThread.h

@@ -1,65 +1,70 @@
-/*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_
-#define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_
-
-#include <rtabmap/utilite/ULogger.h>
-
-namespace rtabmap {
-
-class LaserScanInfo
-{
-public:
-	LaserScanInfo() :
-		maxPoints_(0),
-		maxRange_(0),
-		localTransform_(Transform::getIdentity())
-	{
-	}
-
-	LaserScanInfo(int maxPoints, float maxRange, const Transform & localTransform = Transform::getIdentity()) :
-		maxPoints_(maxPoints),
-		maxRange_(maxRange),
-		localTransform_(localTransform)
-	{
-		UASSERT(!localTransform.isNull());
-	}
-
-	int maxPoints() const {return maxPoints_;}
-	float maxRange() const {return maxRange_;}
-	Transform localTransform() const {return localTransform_;}
-
-private:
-	int maxPoints_;
-	float maxRange_;
-	Transform localTransform_;
-};
-
-}
-
-#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_ */
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <fstream>
+
+namespace rtabmap
+{
+
+/**
+ * Class IMUThread
+ *
+ */
+class RTABMAP_EXP IMUThread :
+	public UThread,
+	public UEventsSender
+{
+public:
+	IMUThread(int rate, const Transform & localTransform);
+	virtual ~IMUThread();
+
+	bool init(const std::string & path);
+	void setRate(int rate);
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+private:
+	int rate_;
+	Transform localTransform_;
+	std::ifstream imuFile_;
+	UTimer frameRateTimer_;
+	double captureDelay_;
+	double previousStamp_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/LaserScan.h

@@ -0,0 +1,95 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP LaserScan
+{
+public:
+	enum Format{kUnknown=0,
+		kXY=1,
+		kXYI=2,
+		kXYNormal=3,
+		kXYINormal=4,
+		kXYZ=5,
+		kXYZI=6,
+		kXYZRGB=7,
+		kXYZNormal=8,
+		kXYZINormal=9,
+		kXYZRGBNormal=10};
+
+	static int channels(Format format);
+	static bool isScan2d(const Format & format);
+	static bool isScanHasNormals(const Format & format);
+	static bool isScanHasRGB(const Format & format);
+	static bool isScanHasIntensity(const Format & format);
+	static LaserScan backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints = 0, int maxRange = 0, const Transform & localTransform = Transform::getIdentity());
+
+public:
+	LaserScan();
+	LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform = Transform::getIdentity());
+
+	const cv::Mat & data() const {return data_;}
+	int maxPoints() const {return maxPoints_;}
+	float maxRange() const {return maxRange_;}
+	Format format() const {return format_;}
+	Transform localTransform() const {return localTransform_;}
+
+	bool isEmpty() const {return data_.empty();}
+	int size() const {return data_.cols;}
+	int dataType() const {return data_.type();}
+	bool is2d() const {return isScan2d(format_);}
+	bool hasNormals() const {return isScanHasNormals(format_);}
+	bool hasRGB() const {return isScanHasRGB(format_);}
+	bool hasIntensity() const {return isScanHasIntensity(format_);}
+	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
+	LaserScan clone() const {return LaserScan(data_.clone(), maxPoints_, maxRange_, format_, localTransform_.clone());}
+
+	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
+	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
+	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
+
+	void clear() {data_ = cv::Mat();}
+
+private:
+	cv::Mat data_;
+	int maxPoints_;
+	float maxRange_;
+	Format format_;
+	Transform localTransform_;
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ */

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,
@@ -127,6 +129,8 @@ public:
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			bool ignoreIntermediateNodes = false,
+			bool ignoreLocalSpaceLoopIds = false,
+			const std::set<int> & nodesSet = std::set<int>(),
 			double * dbAccessTime = 0) const;
 	std::map<int, float> getNeighborsIdRadius(
 			int signatureId,
@@ -134,6 +138,7 @@ public:
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
+	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);
 	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
 
@@ -177,6 +182,7 @@ public:
 			double & stamp,
 			Transform & groundTruth,
 			std::vector<float> & velocity,
+			GPS & gps,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
@@ -219,7 +225,6 @@ public:
 
 	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
 	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
-	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(
 			int newId,
 			int oldId,
@@ -278,11 +283,14 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
+	bool _depthAsMask;
 	int _imagePreDecimation;
 	int _imagePostDecimation;
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
+	float _laserScanVoxelSize;
 	int _laserScanNormalK;
+	int _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
@@ -290,6 +298,8 @@ private:
 	bool _useOdometryFeatures;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
+	int _visCorType;
+	bool _imagesAlreadyRectified;
 
 	int _idCount;
 	int _idMapCount;
@@ -298,6 +308,7 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
+	GPS _gpsOrigin;
 
 	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
@@ -311,7 +322,7 @@ private:
 	bool _parallelized;
 
 	Registration * _registrationPipeline;
-	RegistrationIcp * _registrationIcp;
+	RegistrationIcp * _registrationIcpMulti;
 
 	OccupancyGrid * _occupancy;
 };

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -42,11 +42,14 @@ class RTABMAP_EXP OccupancyGrid
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
+	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
+	void setCloudAssembling(bool enabled);
 	float getMinMapSize() const {return minMapSize_;}
-	bool isGridFromDepth() const {return occupancyFromCloud_;}
+	bool isGridFromDepth() const {return occupancyFromDepth_;}
 	bool isFullUpdate() const {return fullUpdate_;}
+	float getUpdateError() const {return updateError_;}
 	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
@@ -63,17 +66,30 @@ public:
 
 	void createLocalMap(
 			const Signature & node,
-			cv::Mat & ground,
-			cv::Mat & obstacles,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
 			cv::Point3f & viewPoint) const;
 
+	void createLocalMap(
+			const LaserScan & cloud,
+			const Transform & pose,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPointInOut) const;
+
 	void clear();
 	void addToCache(
 			int nodeId,
 			const cv::Mat & ground,
-			const cv::Mat & obstacles);
+			const cv::Mat & obstacles,
+			const cv::Mat & empty);
 	void update(const std::map<int, Transform> & poses);
-	const cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getMap(float & xMin, float & yMin) const;
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
 
 private:
 	ParametersMap parameters_;
@@ -86,7 +102,8 @@ private:
 	float footprintHeight_;
 	int scanDecimation_;
 	float cellSize_;
-	bool occupancyFromCloud_;
+	bool preVoxelFiltering_;
+	bool occupancyFromDepth_;
 	bool projMapFrame_;
 	float maxObstacleHeight_;
 	int normalKSearch_;
@@ -102,20 +119,25 @@ private:
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;
-	double scan2dMaxUnknownSpaceFilledRange_;
-	bool projRayTracing_;
+	bool rayTracing_;
 	bool fullUpdate_;
 	float minMapSize_;
 	bool erode_;
 	float footprintRadius_;
+	float updateError_;
 
-	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
 	cv::Mat map_;
 	cv::Mat mapInfo_;
 	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
 	float xMin_;
 	float yMin_;
 	std::map<int, Transform> addedNodes_;
+
+	bool cloudAssembling_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledEmptyCells_;
 };
 
 }

corelib/include/rtabmap/core/OctoMap.h

@@ -37,45 +37,163 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
 
 #include <map>
 #include <string>
 
 namespace rtabmap {
 
-class OcTreeNodeInfo
+// forward declaraton for "friend"
+class RtabmapColorOcTree;
+
+class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode
 {
 public:
-	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
-		nodeRefId_(nodeRefId),
-		key_(key),
-		isObstacle_(isObstacle) {}
+	enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100};
+
+public:
+	friend class RtabmapColorOcTree; // needs access to node children (inherited)
+
+	RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {}
+	RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
+
+	void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
+	void setOccupancyType(char type) {type_=type;}
+	void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
+	int getNodeRefId() const {return nodeRefId_;}
+	int getOccupancyType() const {return type_;}
+	const octomap::point3d & getPointRef() const {return pointRef_;}
+
+	// following methods defined for octomap < 1.8 compatibility
+	RtabmapColorOcTreeNode* getChild(unsigned int i);
+	const RtabmapColorOcTreeNode* getChild(unsigned int i) const;
+	bool pruneNode();
+	void expandNode();
+	bool createChild(unsigned int i);
+
+private:
 	int nodeRefId_;
-	octomap::OcTreeKey key_;
-	bool isObstacle_;
+	int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
+	octomap::point3d pointRef_;
 };
 
+// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
+class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
+
+  public:
+    /// Default constructor, sets resolution of leafs
+	RtabmapColorOcTree(double resolution);
+
+    /// virtual constructor: creates a new object of same type
+    /// (Covariant return type requires an up-to-date compiler)
+	RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
+
+    std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
+
+     /**
+     * Prunes a node when it is collapsible. This overloaded
+     * version only considers the node occupancy for pruning,
+     * different colors of child nodes are ignored.
+     * @return true if pruning was successful
+     */
+    virtual bool pruneNode(RtabmapColorOcTreeNode* node);
+
+    virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
+
+    // set node color at given key or coordinate. Replaces previous color.
+    RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                 uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* setNodeColor(float x, float y,
+                                 float z, uint8_t r,
+                                 uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return setNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* averageNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap:: OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return averageNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* integrateNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return integrateNodeColor(key,r,g,b);
+    }
+
+    // update inner nodes, sets color to average child color
+    void updateInnerOccupancy();
+
+  protected:
+    void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
+
+    /**
+     * Static member object which ensures that this OcTree's prototype
+     * ends up in the classIDMapping only once. You need this as a
+     * static member in any derived octree class in order to read .ot
+     * files through the AbstractOcTree factory. You should also call
+     * ensureLinking() once from the constructor.
+     */
+    class StaticMemberInitializer{
+       public:
+         StaticMemberInitializer();
+
+         /**
+         * Dummy function to ensure that MSVC does not drop the
+         * StaticMemberInitializer, causing this tree failing to register.
+         * Needs to be called from the constructor of this octree.
+         */
+         void ensureLinking() {};
+    };
+    /// static member to ensure static initialization (only once)
+    static StaticMemberInitializer RtabmapColorOcTreeMemberInit;
+
+  };
+
 class RTABMAP_EXP OctoMap {
 public:
-	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
+	static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
+
+public:
+	OctoMap(const ParametersMap & parameters);
+	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
 
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	void addToCache(int nodeId,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
 			const pcl::PointXYZ & viewPoint);
 	void addToCache(int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			const cv::Point3f & viewPoint);
 	void update(const std::map<int, Transform> & poses);
 
-	const octomap::ColorOcTree * octree() const {return octree_;}
+	const RtabmapColorOcTree * octree() const {return octree_;}
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
 			unsigned int treeDepth = 0,
 			std::vector<int> * obstacleIndices = 0,
-			std::vector<int> * emptyIndices = 0) const;
+			std::vector<int> * emptyIndices = 0,
+			std::vector<int> * groundIndices = 0,
+			bool originalRefPoints = true) const;
 
 	cv::Mat createProjectionMap(
 			float & xMin,
@@ -89,16 +207,30 @@ public:
 	virtual ~OctoMap();
 	void clear();
 
+	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
+	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
+
+	void setMaxRange(float value) {rangeMax_ = value;}
+	void setRayTracing(bool enabled) {rayTracing_ = enabled;}
+	bool hasColor() const {return hasColor_;}
+
 private:
-	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
-	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_;
+	void updateMinMax(const octomap::point3d & point);
+
+private:
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; // [id: < <ground, obstacles>, empty>]
+	std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
 	std::map<int, cv::Point3f> cacheViewPoints_;
-	octomap::ColorOcTree * octree_;
-	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	RtabmapColorOcTree * octree_;
 	std::map<int, Transform> addedNodes_;
 	octomap::KeyRay keyRay_;
 	bool hasColor_;
 	bool fullUpdate_;
+	float updateError_;
+	float rangeMax_;
+	bool rayTracing_;
+	double minValues_[3];
+	double maxValues_[3];
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Odometry.h

@@ -49,7 +49,8 @@ public:
 		kTypeFovis = 2,
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
-		kTypeORBSLAM2 = 5
+		kTypeORBSLAM2 = 5,
+		kTypeOkvis = 6
 	};
 
 public:
@@ -62,12 +63,15 @@ public:
 	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() = 0;
+	virtual bool canProcessRawImages() const {return false;}
 
 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
 	double previousStamp() const {return previousStamp_;}
+	unsigned int framesProcessed() const {return framesProcessed_;}
+	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -92,12 +96,15 @@ private:
 	float _kalmanMeasurementNoise;
 	int _imageDecimation;
 	bool _alignWithGround;
+	bool _publishRAMUsage;
+	bool _imagesAlreadyRectified;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
 	Transform previousVelocityTransform_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
+	unsigned int framesProcessed_;
 
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;

corelib/include/rtabmap/core/OdometryDVO.h

@@ -53,10 +53,12 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_DVO
 	dvo::DenseTracker * dvo_;
 	dvo::core::RgbdImagePyramid * reference_;
 	dvo::core::RgbdCameraPyramid * camera_;
 	bool lost_;
+#endif
 	Transform motionFromKeyFrame_;
 	Transform previousLocalTransform_;
 

corelib/include/rtabmap/core/OdometryEvent.h

@@ -41,7 +41,7 @@ class OdometryEvent : public UEvent
 public:
 	OdometryEvent()
 	{
-		_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+		_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 	OdometryEvent(
 		const SensorData & data,
@@ -51,17 +51,17 @@ public:
 			_pose(pose),
 			_info(info)
 	{
-		if(_info.covariance.empty())
+		if(_info.reg.covariance.empty())
 		{
-			_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
+			_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
 		}
-		UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1);
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
 	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
@@ -69,7 +69,7 @@ public:
 	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
 	const Transform & pose() const {return _pose;}
-	const cv::Mat & covariance() const {return _info.covariance;}
+	const cv::Mat & covariance() const {return _info.reg.covariance;}
 	std::vector<float> velocity() const {
 		if(_info.interval>0.0)
 		{
@@ -82,6 +82,7 @@ public:
 			velocity[3] = roll/_info.interval;
 			velocity[4] = pitch/_info.interval;
 			velocity[5] = yaw/_info.interval;
+			return velocity;
 		}
 		return std::vector<float>();
 	}
@@ -96,9 +97,12 @@ private:
 class OdometryResetEvent : public UEvent
 {
 public:
-	OdometryResetEvent(){}
+	OdometryResetEvent(const Transform & pose = Transform::getIdentity()){_pose = pose;}
 	virtual ~OdometryResetEvent() {}
 	virtual std::string getClassName() const {return "OdometryResetEvent";}
+	const Transform & getPose() const {return _pose;}
+private:
+	Transform _pose;
 };
 
 }

corelib/include/rtabmap/core/OdometryF2F.h

@@ -59,6 +59,7 @@ private:
 	Registration * registrationPipeline_;
 	Signature refFrame_;
 	Transform lastKeyFramePose_;
+	ParametersMap parameters_;
 };
 
 }

corelib/include/rtabmap/core/OdometryF2M.h

@@ -64,12 +64,14 @@ private:
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
+	float scanSubtractAngle_;
 	int bundleAdjustment_;
 	int bundleMaxFrames_;
 
 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
+	int lastFrameOldestNewId_;
 	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
 
 	std::map<int, std::map<int, cv::Point3f> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
@@ -79,6 +81,7 @@ private:
 	std::map<int, int> bundlePoseReferences_;
 	int bundleSeq_;
 	Optimizer * sba_;
+	ParametersMap parameters_;
 };
 
 }

corelib/include/rtabmap/core/OdometryFovis.h

@@ -53,13 +53,15 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_FOVIS
 	fovis::VisualOdometry * fovis_;
 	fovis::Rectification * rect_;
 	fovis::StereoCalibration * stereoCalib_;
 	fovis::DepthImage * depthImage_;
 	fovis::StereoDepth * stereoDepth_;
-	ParametersMap fovisParameters_;
 	bool lost_;
+#endif
+	ParametersMap fovisParameters_;
 	Transform previousLocalTransform_;
 };
 

corelib/include/rtabmap/core/OdometryInfo.h

@@ -30,6 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <map>
 #include "rtabmap/core/Transform.h"
+#include "rtabmap/core/RegistrationInfo.h"
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/LaserScan.h"
 #include <opencv2/features2d/features2d.hpp>
 
 namespace rtabmap {
@@ -39,9 +42,6 @@ class OdometryInfo
 public:
 	OdometryInfo() :
 		lost(true),
-		matches(0),
-		inliers(0),
-		icpInliersRatio(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
@@ -55,6 +55,7 @@ public:
 		stamp(0),
 		interval(0),
 		distanceTravelled(0.0f),
+		memoryUsage(0),
 		type(0)
 	{}
 
@@ -62,10 +63,7 @@ public:
 	{
 		OdometryInfo output;
 		output.lost = lost;
-		output.matches = matches;
-		output.inliers = inliers;
-		output.icpInliersRatio = icpInliersRatio;
-		output.covariance = covariance.clone();
+		output.reg = reg.copyWithoutData();
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
@@ -73,6 +71,8 @@ public:
 		output.localBundleOutliers = localBundleOutliers;
 		output.localBundleConstraints = localBundleConstraints;
 		output.localBundleTime = localBundleTime;
+		output.localBundlePoses = localBundlePoses;
+		output.localBundleModels = localBundleModels;
 		output.keyFrameAdded = keyFrameAdded;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
@@ -82,15 +82,13 @@ public:
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
 		output.distanceTravelled = distanceTravelled;
+		output.memoryUsage = memoryUsage;
 		output.type = type;
 		return output;
 	}
 
 	bool lost;
-	int matches;
-	int inliers;
-	float icpInliersRatio;
-	cv::Mat covariance;
+	RegistrationInfo reg;
 	int features;
 	int localMapSize;
 	int localScanMapSize;
@@ -98,6 +96,8 @@ public:
 	int localBundleOutliers;
 	int localBundleConstraints;
 	float localBundleTime;
+	std::map<int, Transform> localBundlePoses;
+	std::map<int, CameraModel> localBundleModels;
 	bool keyFrameAdded;
 	float timeEstimation;
 	float timeParticleFiltering;
@@ -107,15 +107,14 @@ public:
 	Transform transformFiltered;
 	Transform transformGroundTruth;
 	float distanceTravelled;
+	int memoryUsage; //MB
 
-	int type; // 0=F2M, 1=F2F
+	int type;
 
 	// F2M
 	std::multimap<int, cv::KeyPoint> words;
-	std::vector<int> wordMatches;
-	std::vector<int> wordInliers;
 	std::map<int, cv::Point3f> localMap;
-	cv::Mat localScanMap;
+	LaserScan localScanMap;
 
 	// F2F
 	std::vector<cv::Point2f> refCorners;

corelib/include/rtabmap/core/OdometryORBSLAM2.h

@@ -51,9 +51,10 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_ORB_SLAM2
 	ORBSLAM2System * orbslam2_;
-	ORB_SLAM2::System * system_;
 	bool firstFrame_;
+#endif
 	Transform originLocalTransform_;
 
 };

corelib/include/rtabmap/core/OdometryOkvis.h

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

corelib/include/rtabmap/core/OdometryThread.h

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

corelib/include/rtabmap/core/OdometryViso2.h

@@ -47,12 +47,14 @@ private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
+#ifdef RTABMAP_VISO2
 	VisualOdometryStereo * viso2_;
 	int ref_frame_change_method_;       // Reference frame method (defautl 0): 0=under inliers threshold, 1=min pixel motion,
 	int ref_frame_inlier_threshold_;    // method 0. Change the reference frame if the number of inliers is low
 	double ref_frame_motion_threshold_; // method 1. Change the reference frame if last motion is small
 	bool lost_;
 	bool keep_reference_frame_;
+#endif
 	Transform reference_motion_;
 	Transform previousLocalTransform_;
 	ParametersMap viso2Parameters_;

corelib/include/rtabmap/core/Optimizer.h

@@ -75,6 +75,7 @@ public:
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
 	double epsilon() const {return epsilon_;}
 	bool isRobust() const {return robust_;}
+	bool priorsIgnored() const {return priorsIgnored_;}
 
 	// setters
 	void setIterations(int iterations) {iterations_ = iterations;}
@@ -82,9 +83,18 @@ public:
 	void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
 	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
 	void setRobust(bool enabled) {robust_ = enabled;}
+	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	std::map<int, Transform> optimizeIncremental(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 			int rootId,
@@ -128,7 +138,8 @@ protected:
 			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
-			bool robust            = Parameters::defaultOptimizerRobust());
+			bool robust            = Parameters::defaultOptimizerRobust(),
+			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored());
 	Optimizer(const ParametersMap & parameters);
 
 private:
@@ -137,6 +148,7 @@ private:
 	bool covarianceIgnored_;
 	double epsilon_;
 	bool robust_;
+	bool priorsIgnored_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -172,6 +172,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, PublishLastSignature,         bool, true,  "Publishing last signature.");
     RTABMAP_PARAM(Rtabmap, PublishPdf,                   bool, true,  "Publishing pdf.");
     RTABMAP_PARAM(Rtabmap, PublishLikelihood,            bool, true,  "Publishing likelihood.");
+    RTABMAP_PARAM(Rtabmap, 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, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
@@ -183,6 +186,7 @@ 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, 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.");
 
     // Hypotheses selection
     RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
@@ -207,26 +211,35 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, GenerateIds,                 bool, true,     "True=Generate location IDs, False=use input image IDs.");
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+    RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
     RTABMAP_PARAM(Mem, ImagePreDecimation,          int, 1,         "Image decimation (>=1) before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
     RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
     RTABMAP_PARAM(Mem, CompressionParallelized,     bool, true,     "Compression of sensor data is multi-threaded.");
     RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, false,    "Use odometry features.");
+    RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).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, UseOdomFeatures,             bool, true,     "Use odometry features.");
 
     // KeypointMemory (Keypoint-based)
     RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
     RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
-    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   "When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
+    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
+    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
-    RTABMAP_PARAM(Kp, MaxFeatures,              int, 400,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
+    RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
     RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
-#ifdef RTABMAP_NONFREE
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 0,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
+#ifndef RTABMAP_NONFREE
+#ifdef RTABMAP_OPENCV3
+    // OpenCV 3 without xFeatures2D module doesn't have BRIEF
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 2,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
+#endif
+#else
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
 #endif
     RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
     RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
@@ -236,6 +249,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
     RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
     RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Kp, GridRows,                 int, 1,       uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));
+    RTABMAP_PARAM(Kp, GridCols,                 int, 1,       uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));
 
     //Database
     RTABMAP_PARAM(DbSqlite3, InMemory,     bool, false,      "Using database in the memory instead of a file on the hard disk.");
@@ -270,8 +285,8 @@ class RTABMAP_EXP Parameters
     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(GFTT, QualityLevel,      double, 0.01, "");
-    RTABMAP_PARAM(GFTT, MinDistance,       double, 5,    "");
+    RTABMAP_PARAM(GFTT, QualityLevel,      double, 0.001, "");
+    RTABMAP_PARAM(GFTT, MinDistance,       double, 3,    "");
     RTABMAP_PARAM(GFTT, BlockSize,         int, 3,       "");
     RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false,  "");
     RTABMAP_PARAM(GFTT, K,                 double, 0.04, "");
@@ -314,11 +329,14 @@ class RTABMAP_EXP Parameters
 
     // RGB-D SLAM
     RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
-    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
+    RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest 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,    uFormat("Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, 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.");
@@ -337,7 +355,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true,  "Detection over locations (in Working Memory) near in space.");
     RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,     "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
     RTABMAP_PARAM(RGBD, ProximityMaxPaths,            int, 3,      "Maximum paths compared (from the most recent) for proximity detection by space. 0 means no limit.");
-    RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,  "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 1,    "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
     RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path. Set to 0 to disable merging the laser scans.");
     RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
     RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for visual proximity detection.");
@@ -360,8 +378,13 @@ class RTABMAP_EXP Parameters
 #endif
     RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
     RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
 
+#ifdef RTABMAP_ORB_SLAM2
+    RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
+#else
     RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
+#endif
     RTABMAP_PARAM(g2o, Optimizer,         int, 0,          "0=Levenberg 1=GaussNewton");
     RTABMAP_PARAM(g2o, PixelVariance,     double, 1.0,     "Pixel variance used for bundle adjustment.");
     RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
@@ -383,10 +406,10 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Odom, ParticleLambdaR,        float, 100,   "Lambda of rotational components (roll,pitch,yaw).");
     RTABMAP_PARAM(Odom, KalmanProcessNoise,     float, 0.001, "Process noise covariance value.");
     RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,  "Process measurement covariance value.");
-    RTABMAP_PARAM(Odom, GuessMotion,            bool, false,  "Guess next transformation from the last motion computed.");
+    RTABMAP_PARAM(Odom, GuessMotion,            bool, true,   "Guess next transformation from the last motion computed.");
     RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,   "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
-    RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 100,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
-    RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.7,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
+    RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 150,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
+    RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,       "Decimation of the images before registration. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
     RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
 
@@ -395,8 +418,13 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
     RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
+    RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+#else
     RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
-    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
+#endif
+    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
 
     // Odometry Mono
     RTABMAP_PARAM(OdomMono, InitMinFlow,        float, 100,  "Minimum optical flow required for the initialization step.");
@@ -422,7 +450,7 @@ class RTABMAP_EXP Parameters
 
     RTABMAP_PARAM(OdomFovis, InlierMaxReprojectionError,      double, 1.5,  "The maximum image-space reprojection error (in pixels) a feature match is allowed to have and still be considered an inlier in the set of features used for motion estimation.");
     RTABMAP_PARAM(OdomFovis, CliqueInlierThreshold,           double, 0.1,  "See Howard's greedy max-clique algorithm for determining the maximum set of mutually consisten feature matches. This specifies the compatibility threshold, in meters.");
-    RTABMAP_PARAM(OdomFovis, MinFeaturesForEstimate,          int, 10,      "Minimum number of features in the inlier set for the motion estimate to be considered valid.");
+    RTABMAP_PARAM(OdomFovis, MinFeaturesForEstimate,          int, 20,      "Minimum number of features in the inlier set for the motion estimate to be considered valid.");
     RTABMAP_PARAM(OdomFovis, MaxMeanReprojectionError,        double, 10.0, "Maximum mean reprojection error over the inlier feature matches for the motion estimate to be considered valid.");
     RTABMAP_PARAM(OdomFovis, UseSubpixelRefinement,           bool, true,   "Specifies whether or not to refine feature matches to subpixel resolution.");
     RTABMAP_PARAM(OdomFovis, FeatureSearchWindow,             int, 25,      "Specifies the size of the search window to apply when searching for feature matches across time frames.  The search is conducted around the feature location predicted by the initial rotation estimate.");
@@ -452,13 +480,18 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
 
     // Odometry ORB_SLAM2
-    RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath,            "", "Path to ORB vocabulary (*.txt).");
-    RTABMAP_PARAM(OdomORBSLAM2, Bf,          double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
-    RTABMAP_PARAM(OdomORBSLAM2, ThDepth,     double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM(OdomORBSLAM2, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM2, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM2, Fps,             float,  0.0,   "Camera FPS.");
+    RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
+    RTABMAP_PARAM(OdomORBSLAM2, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");
+
+    // Odometry OKVIS
+    RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
 
     // Common registration parameters
-    RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
-    RTABMAP_PARAM(Reg, VarianceNormalized,       bool, false,   "Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform.");
+    RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
     RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
     RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
 
@@ -469,10 +502,15 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, PnPReprojError,           float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, PnPFlags,                 int, 0,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
-    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 0,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
+#else
+    RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
+#endif
+
     RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
-    RTABMAP_PARAM(Vis, Iterations,               int, 100,      "Maximum iterations to compute the transform.");
+    RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
 #ifndef RTABMAP_NONFREE
 #ifdef RTABMAP_OPENCV3
     // OpenCV 3 without xFeatures2D module doesn't have BRIEF
@@ -486,39 +524,68 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,   "0 no limits.");
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,   "Max depth of the features (0 means no limit).");
     RTABMAP_PARAM(Vis, MinDepth,                  float, 0,   "Min depth of the features (0 means no limit).");
+    RTABMAP_PARAM(Vis, DepthAsMask,               bool, true, "Use depth image as mask when extracting features.");
     RTABMAP_PARAM_STR(Vis, RoiRatios,      "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,      "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,      "See cv::cornerSubPix(). 0 disables sub pixel refining.");
     RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
+    RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
     RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,      uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
+    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 1,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+#else
     RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
+#endif
 
     // ICP registration parameters
     RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
     RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
     RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.0,   "Uniform sampling voxel size (0=disabled).");
     RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1,       "Downsampling step size (1=no sampling). This is done before uniform sampling.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1,   "Max distance for point correspondences.");
+#else
     RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
+#endif
     RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
     RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
-    RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.2,   "Ratio of matching correspondences to accept the transform.");
+    RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+#else
     RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
-    RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
+#endif
+    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
+
+    // libpointmatcher
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PM,                       bool, true,    "Use libpointmatcher for ICP registration instead of PCL's implementation.");
+#else
+    RTABMAP_PARAM(Icp, PM,                       bool, false,   "Use libpointmatcher for ICP registration instead of PCL's implementation.");
+#endif
+    RTABMAP_PARAM_STR(Icp, PMConfig,             "",            uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
+    RTABMAP_PARAM(Icp, PMMatcherKnn,             int, 1,        "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMMatcherEpsilon,         float, 0.0,    "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMOutlierRatio,           float, 0.95,   "TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.");
 
     // Stereo disparity
     RTABMAP_PARAM(Stereo, WinWidth,              int, 15,       "Window width.");
     RTABMAP_PARAM(Stereo, WinHeight,             int, 3,        "Window height.");
     RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "Maximum iterations.");
-    RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "Maximum pyramid level.");
-    RTABMAP_PARAM(Stereo, MinDisparity,          int, 1,        "Minimum disparity.");
-    RTABMAP_PARAM(Stereo, MaxDisparity,          int, 128,      "Maximum disparity.");
+    RTABMAP_PARAM(Stereo, MaxLevel,              int, 5,        "Maximum pyramid level.");
+    RTABMAP_PARAM(Stereo, MinDisparity,          float, 0.5,    "Minimum disparity.");
+    RTABMAP_PARAM(Stereo, MaxDisparity,          float, 128.0,  "Maximum disparity.");
     RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
     RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
     RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
@@ -536,19 +603,20 @@ class RTABMAP_EXP Parameters
     // Occupancy Grid
     RTABMAP_PARAM(Grid, FromDepth,               bool,   true,    "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
     RTABMAP_PARAM(Grid, DepthDecimation,         int,    4,       uFormat("[%s=true] Decimation of the depth image before creating cloud. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).", kGridDepthDecimation().c_str()));
-    RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridFromDepth().c_str()));
-    RTABMAP_PARAM(Grid, DepthMax,                float,  4.0,     uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, RangeMin,                float,  0.0,     "Minimum range from sensor.");
+    RTABMAP_PARAM(Grid, RangeMax,                float,  5.0,     "Maximum range from sensor. 0=inf.");
     RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));
     RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
     RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
     RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
     RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridFromDepth().c_str()));
     RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
+    RTABMAP_PARAM(Grid, PreVoxelFiltering,       bool,   true,    uFormat("Input cloud is downsampled by voxel filter (voxel size is \"%s\") before doing segmentation of obstacles and ground.", kGridCellSize().c_str()));
     RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
     RTABMAP_PARAM(Grid, NormalsSegmentation,     bool,   true,    "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
     RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "Maximum obstacles height (0=disabled).");
     RTABMAP_PARAM(Grid, MinGroundHeight,         float,  0.0,     "Minimum ground height (0=disabled).");
-    RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is false.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  45,      uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NormalK,                 int,    20,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, ClusterRadius,           float,  0.1,     uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str()));
@@ -562,14 +630,16 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
     RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
-    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  "Unknown space filled. Only used with 2D laser scans.");
-    RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,       float,   4.0,    "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
-    RTABMAP_PARAM(Grid, ProjRayTracing,             bool,   true,    uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
+    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
+    RTABMAP_PARAM(Grid, RayTracing,                 bool,   false,   uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
 
     RTABMAP_PARAM(GridGlobal, FullUpdate,           bool,   true,    "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
+    RTABMAP_PARAM(GridGlobal, UpdateError,          float,  0.01,    "Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value.");
     RTABMAP_PARAM(GridGlobal, FootprintRadius,      float,  0.0,     "Footprint radius (m) used to clear all obstacles under the graph.");
     RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
     RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
+    RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
+    RTABMAP_PARAM(GridGlobal, OctoMapOccupancyThr,  float,  0.5,     "OctoMap occupancy threshold (value between 0 and 1).");
 
 public:
     virtual ~Parameters();
@@ -617,7 +687,7 @@ public:
     static ParametersMap getDefaultParameters(const std::string & group);
     static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);
 
-    static void readINI(const std::string & configFile, ParametersMap & parameters);
+    static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
     static void writeINI(const std::string & configFile, const ParametersMap & parameters);
 
     /**

corelib/include/rtabmap/core/ProgressState.h

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 
+namespace rtabmap {
+
 class ProgressState
 {
 public:
@@ -55,5 +57,6 @@ private:
 	bool canceled_;
 };
 
+}
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_ */

corelib/include/rtabmap/core/Recovery.h

@@ -0,0 +1,56 @@
+/*
+Copyright (c) 2010-2017, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RECOVERY_H_
+#define RECOVERY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <string>
+
+namespace rtabmap {
+
+class ProgressState;
+
+/**
+ * Return true on success. The database is
+ * renamed to "*.backup.db" before recovering.
+ * @param corruptedDatabase database to recover
+ * @param keepCorruptedDatabase if false and on recovery success, the backup database is removed
+ * @param errorMsg error message if the function returns false
+ * @param progressState A ProgressState object used to get status of the recovery process
+ */
+bool RTABMAP_EXP databaseRecovery(
+		const std::string & corruptedDatabase,
+		bool keepCorruptedDatabase = true,
+		std::string * errorMsg = 0,
+		ProgressState * progressState = 0);
+
+}
+
+
+#endif /* RECOVERY_H_ */

corelib/include/rtabmap/core/Registration.h

@@ -45,6 +45,7 @@ public:
 		kTypeIcp = 1,
 		kTypeVisIcp = 2
 	};
+	static double COVARIANCE_EPSILON;
 
 public:
 	static Registration * create(const ParametersMap & parameters);
@@ -58,12 +59,13 @@ public:
 	bool isScanRequired() const;
 	bool isUserDataRequired() const;
 
+	bool canUseGuess() const;
+
 	int getMinVisualCorrespondences() const;
 	float getMinGeometryCorrespondencesRatio() const;
 
-	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
+	bool repeatOnce() const {return repeatOnce_;}
 	bool force3DoF() const {return force3DoF_;}
-	bool covarianceNormalized() const {return covarianceNormalized_;}
 
 	// take ownership!
 	void setChildRegistration(Registration * child);
@@ -85,8 +87,6 @@ public:
 			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;
 
-	void normalizeCovariance(cv::Mat & covariance, const Transform & transform) const;
-
 protected:
 	// take ownership of child
 	Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
@@ -102,12 +102,12 @@ protected:
 	virtual bool isImageRequiredImpl() const {return false;}
 	virtual bool isScanRequiredImpl() const {return false;}
 	virtual bool isUserDataRequiredImpl() const {return false;}
+	virtual bool canUseGuessImpl() const {return false;}
 	virtual int getMinVisualCorrespondencesImpl() const {return 0;}
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}
 
 private:
-	bool varianceFromInliersCount_;
-	bool covarianceNormalized_;
+	bool repeatOnce_;
 	bool force3DoF_;
 	Registration * child_;
 

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -41,7 +41,7 @@ class RTABMAP_EXP RegistrationIcp : public Registration
 public:
 	// take ownership of child
 	RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
-	virtual ~RegistrationIcp() {}
+	virtual ~RegistrationIcp();
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
@@ -52,6 +52,7 @@ protected:
 			Transform guess,
 			RegistrationInfo & info) const;
 	virtual bool isScanRequiredImpl() const {return true;}
+	virtual bool canUseGuessImpl() const {return true;}
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
 
 private:
@@ -64,7 +65,15 @@ private:
 	float _epsilon;
 	float _correspondenceRatio;
 	bool _pointToPlane;
-	int _pointToPlaneNormalNeighbors;
+	int _pointToPlaneK;
+	float _pointToPlaneRadius;
+	float _pointToPlaneMinComplexity;
+	bool _libpointmatcher;
+	std::string _libpointmatcherConfig;
+	int _libpointmatcherKnn;
+	float _libpointmatcherEpsilon;
+	float _libpointmatcherOutlierRatio;
+	void * _libpointmatcherICP;
 };
 
 }

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -35,27 +35,48 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
+		totalTime(0.0),
 		inliers(0),
 		matches(0),
 		icpInliersRatio(0),
 		icpTranslation(0.0f),
-		icpRotation(0.0f)
+		icpRotation(0.0f),
+		icpStructuralComplexity(0.0f)
+
 	{
 	}
 
+	RegistrationInfo copyWithoutData() const
+	{
+		RegistrationInfo output;
+		output.totalTime = totalTime;
+		output.covariance = covariance.clone();
+		output.rejectedMsg = rejectedMsg;
+		output.inliers = inliers;
+		output.matches = matches;
+		output.icpInliersRatio = icpInliersRatio;
+		output.icpTranslation = icpTranslation;
+		output.icpRotation = icpRotation;
+		output.icpStructuralComplexity = icpStructuralComplexity;
+		return output;
+	}
+
 	cv::Mat covariance;
 	std::string rejectedMsg;
+	double totalTime;
 
 	// RegistrationVis
 	int inliers;
 	std::vector<int> inliersIDs;
 	int matches;
 	std::vector<int> matchesIDs;
+	std::vector<int> projectedIDs; // "From" IDs
 
 	// RegistrationIcp
 	float icpInliersRatio;
 	float icpTranslation;
 	float icpRotation;
+	float icpStructuralComplexity;
 };
 
 }

corelib/include/rtabmap/core/RegistrationVis.h

@@ -61,6 +61,7 @@ protected:
 			RegistrationInfo & info) const;
 
 	virtual bool isImageRequiredImpl() const {return true;}
+	virtual bool canUseGuessImpl() const {return _correspondencesApproach != 0 || _guessWinSize>0;}
 	virtual int getMinVisualCorrespondencesImpl() const {return _minInliers;}
 
 private:
@@ -81,7 +82,9 @@ private:
 	int _flowMaxLevel;
 	float _nndr;
 	int _guessWinSize;
+	bool _guessMatchToProjection;
 	int _bundleAdjustment;
+	bool _depthAsMask;
 
 	ParametersMap _featureParameters;
 	ParametersMap _bundleParameters;

corelib/include/rtabmap/core/Rtabmap.h

@@ -118,6 +118,7 @@ public:
 	const Statistics & getStatistics() const;
 	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
 	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
+	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
@@ -128,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);
 	/**
@@ -161,12 +163,12 @@ public:
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			bool optimized,
-			bool global,
+			bool global,
 			std::map<int, Signature> * signatures = 0);
 	int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1, const ProgressState * state = 0);
 	int refineLinks();
 
-	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
+	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
@@ -211,6 +213,9 @@ private:
 	bool _publishLastSignatureData;
 	bool _publishPdf;
 	bool _publishLikelihood;
+	bool _publishRAMUsage;
+	bool _computeRMSE;
+	bool _saveWMState;
 	float _maxTimeAllowed; // in ms
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
@@ -225,6 +230,8 @@ private:
 	bool _rgbdSlamMode;
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
+	float _rgbdLinearSpeedUpdate;
+	float _rgbdAngularSpeedUpdate;
 	float _newMapOdomChangeDistance;
 	bool _neighborLinkRefining;
 	bool _proximityByTime;
@@ -247,6 +254,7 @@ private:
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
+	bool _savedLocalizationIgnored;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -291,6 +299,5 @@ private:
 
 };
 
-#endif /* RTABMAP_H_ */
-
 } // namespace rtabmap
+#endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/RtabmapThread.h

@@ -66,7 +66,6 @@ public:
 		kStateCleanDataBuffer,
 		kStatePublishingMap,
 		kStateTriggeringMap,
-		kStateAddingUserData,
 		kStateSettingGoal,
 		kStateCancellingGoal,
 		kStateLabelling
@@ -114,6 +113,7 @@ private:
 	std::queue<ParametersMap> _stateParam;
 
 	std::list<OdometryEvent> _dataBuffer;
+	std::list<double> _newMapEvents;
 	UMutex _dataMutex;
 	USemaphore _dataAdded;
 	unsigned int _dataBufferMaxSize;

corelib/include/rtabmap/core/SensorData.h

@@ -33,9 +33,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/core/LaserScanInfo.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/IMU.h>
 
 namespace rtabmap
 {
@@ -75,8 +77,7 @@ public:
 
 	// RGB-D constructor + laser scan
 	SensorData(
-			const cv::Mat & laserScan,
-			const LaserScanInfo & laserScanInfo,
+			const LaserScan & laserScan,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const CameraModel & cameraModel,
@@ -95,8 +96,7 @@ public:
 
 	// Multi-cameras RGB-D constructor + laser scan
 	SensorData(
-			const cv::Mat & laserScan,
-			const LaserScanInfo & laserScanInfo,
+			const LaserScan & laserScan,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const std::vector<CameraModel> & cameraModels,
@@ -115,8 +115,7 @@ public:
 
 	// Stereo constructor + laser scan
 	SensorData(
-			const cv::Mat & laserScan,
-			const LaserScanInfo & laserScanInfo,
+			const LaserScan & laserScan,
 			const cv::Mat & left,
 			const cv::Mat & right,
 			const StereoCameraModel & cameraModel,
@@ -124,7 +123,13 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
-	virtual ~SensorData() {}
+	// IMU constructor
+	SensorData(
+			const IMU & imu,
+			int id = 0,
+			double stamp = 0.0);
+
+	virtual ~SensorData();
 
 	bool isValid() const {
 		return !(_id == 0 &&
@@ -133,32 +138,32 @@ public:
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
 			_depthOrRightCompressed.empty() &&
-			_laserScanRaw.empty() &&
-			_laserScanCompressed.empty() &&
+			_laserScanRaw.isEmpty() &&
+			_laserScanCompressed.isEmpty() &&
 			_cameraModels.size() == 0 &&
 			!_stereoCameraModel.isValidForProjection() &&
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
-			_descriptors.empty());
+			_descriptors.empty() &&
+			imu_.empty());
 	}
 
 	int id() const {return _id;}
 	void setId(int id) {_id = id;}
 	double stamp() const {return _stamp;}
 	void setStamp(double stamp) {_stamp = stamp;}
-	const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}
 
 	const cv::Mat & imageCompressed() const {return _imageCompressed;}
 	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
-	const cv::Mat & laserScanCompressed() const {return _laserScanCompressed;}
+	const LaserScan & laserScanCompressed() const {return _laserScanCompressed;}
 
 	const cv::Mat & imageRaw() const {return _imageRaw;}
 	const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
-	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
+	const LaserScan & laserScanRaw() const {return _laserScanRaw;}
 	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
 	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
-	void setLaserScanRaw(const cv::Mat & laserScanRaw, const LaserScanInfo & info) {_laserScanRaw =laserScanRaw;_laserScanInfo = info;}
+	void setLaserScanRaw(const LaserScan & laserScanRaw) {_laserScanRaw =laserScanRaw;}
 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
 	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
@@ -171,17 +176,19 @@ public:
 	void uncompressData(
 			cv::Mat * imageRaw,
 			cv::Mat * depthOrRightRaw,
-			cv::Mat * laserScanRaw = 0,
+			LaserScan * laserScanRaw = 0,
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
-			cv::Mat * obstacleCellsRaw = 0);
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0);
 	void uncompressDataConst(
 			cv::Mat * imageRaw,
 			cv::Mat * depthOrRightRaw,
-			cv::Mat * laserScanRaw = 0,
+			LaserScan * laserScanRaw = 0,
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
-			cv::Mat * obstacleCellsRaw = 0) const;
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
 	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
@@ -202,6 +209,7 @@ public:
 	void setOccupancyGrid(
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewPoint);
 	// remove raw occupancy grids
@@ -210,6 +218,8 @@ public:
 	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
 	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
 	const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
+	const cv::Mat & gridEmptyCellsRaw() const {return _emptyCellsRaw;}
+	const cv::Mat & gridEmptyCellsCompressed() const {return _emptyCellsCompressed;}
 	float gridCellSize() const {return _cellSize;}
 	const cv::Point3f & gridViewPoint() const {return _viewPoint;}
 
@@ -225,7 +235,22 @@ public:
 	const Transform & globalPose() const {return globalPose_;}
 	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
 
+	void setGPS(const GPS & gps)
+	{
+		gps_ = gps;
+	}
+	const GPS & gps() const {return gps_;}
+
+	void setIMU(const IMU & imu)
+	{
+		imu_ = imu;
+	}
+	const IMU & imu() const {return imu_;}
+
 	long getMemoryUsed() const; // Return memory usage in Bytes
+	void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
+
+	bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
 
 private:
 	int _id;
@@ -233,17 +258,15 @@ private:
 
 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image
-	cv::Mat _laserScanCompressed;      // compressed data
+	LaserScan _laserScanCompressed;      // compressed data
 
 	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
 	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
-	cv::Mat _laserScanRaw;      // CV_32FC2 or CV_32FC3
+	LaserScan _laserScanRaw;
 
 	std::vector<CameraModel> _cameraModels;
 	StereoCameraModel _stereoCameraModel;
 
-	LaserScanInfo _laserScanInfo;
-
 	// user data
 	cv::Mat _userDataCompressed;      // compressed data
 	cv::Mat _userDataRaw;
@@ -251,8 +274,10 @@ private:
 	// occupancy grid
 	cv::Mat _groundCellsCompressed;
 	cv::Mat _obstacleCellsCompressed;
+	cv::Mat _emptyCellsCompressed;
 	cv::Mat _groundCellsRaw;
 	cv::Mat _obstacleCellsRaw;
+	cv::Mat _emptyCellsRaw;
 	float _cellSize;
 	cv::Point3f _viewPoint;
 
@@ -265,6 +290,10 @@ private:
 
 	Transform globalPose_;
 	cv::Mat globalPoseCovariance_; // 6x6 double
+
+	GPS gps_;
+
+	IMU imu_;
 };
 
 }

corelib/include/rtabmap/core/Signature.h

@@ -134,6 +134,8 @@ public:
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}
 
+	long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
+
 private:
 	int _id;
 	int _mapId;

corelib/include/rtabmap/core/Statistics.h

@@ -64,6 +64,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Visual_matches,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
+	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
 	RTABMAP_STATS(Loop, Optimization_error, );
 	RTABMAP_STATS(Loop, Optimization_iterations, );
 
@@ -77,7 +78,10 @@ class RTABMAP_EXP Statistics
 
 	RTABMAP_STATS(NeighborLinkRefining, Accepted,);
 	RTABMAP_STATS(NeighborLinkRefining, Inliers,);
-	RTABMAP_STATS(NeighborLinkRefining, Inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,);
 	RTABMAP_STATS(NeighborLinkRefining, Variance,);
 	RTABMAP_STATS(NeighborLinkRefining, Pts,);
 
@@ -95,13 +99,16 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
 	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Fast_movement,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
+	RTABMAP_STATS(Memory, RAM_usage, MB);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
 	RTABMAP_STATS(Timing, Proximity_by_time, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
 	RTABMAP_STATS(Timing, Reactivation, ms);
@@ -125,19 +132,32 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Subpixel, ms);
 	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
+	RTABMAP_STATS(TimingMem, Rectification, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
 	RTABMAP_STATS(TimingMem, Add_new_words, ms);
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
 	RTABMAP_STATS(TimingMem, Post_decimation, ms);
-	RTABMAP_STATS(TimingMem, Scan_downsampling, ms);
-	RTABMAP_STATS(TimingMem, Scan_normals, ms);
+	RTABMAP_STATS(TimingMem, Scan_filtering, ms);
 	RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
 	RTABMAP_STATS(Keypoint, Indexed_words, words);
 	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);
 
+	RTABMAP_STATS(Gt, Translational_rmse, m);
+	RTABMAP_STATS(Gt, Translational_mean, m);
+	RTABMAP_STATS(Gt, Translational_median, m);
+	RTABMAP_STATS(Gt, Translational_std, m);
+	RTABMAP_STATS(Gt, Translational_min, m);
+	RTABMAP_STATS(Gt, Translational_max, m);
+	RTABMAP_STATS(Gt, Rotational_rmse, deg);
+	RTABMAP_STATS(Gt, Rotational_mean, deg);
+	RTABMAP_STATS(Gt, Rotational_median, deg);
+	RTABMAP_STATS(Gt, Rotational_std, deg);
+	RTABMAP_STATS(Gt, Rotational_min, deg);
+	RTABMAP_STATS(Gt, Rotational_max, deg);
+
 public:
 	static const std::map<std::string, float> & defaultData();
 	static std::string serializeData(const std::map<std::string, float> & data);
@@ -170,6 +190,7 @@ public:
 	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
 	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 	void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
+	void setWmState(const std::vector<int> & state) {_wmState = state;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -191,6 +212,7 @@ public:
 	const std::vector<int> & localPath() const {return _localPath;}
 	int currentGoalId() const {return _currentGoalId;}
 	const std::map<int, int> & reducedIds() const {return _reducedIds;}
+	const std::vector<int> & wmState() const {return _wmState;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -219,6 +241,8 @@ private:
 
 	std::map<int, int> _reducedIds;
 
+	std::vector<int> _wmState;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/include/rtabmap/core/Stereo.h

@@ -53,8 +53,8 @@ public:
 	cv::Size winSize() const {return cv::Size(winWidth_, winHeight_);}
 	int iterations() const   {return iterations_;}
 	int maxLevel() const     {return maxLevel_;}
-	int minDisparity() const {return minDisparity_;}
-	int maxDisparity() const {return maxDisparity_;}
+	float minDisparity() const {return minDisparity_;}
+	float maxDisparity() const {return maxDisparity_;}
 	bool winSSD() const      {return winSSD_;}
 
 private:
@@ -62,8 +62,8 @@ private:
 	int winHeight_;
 	int iterations_;
 	int maxLevel_;
-	int minDisparity_;
-	int maxDisparity_;
+	float minDisparity_;
+	float maxDisparity_;
 	bool winSSD_;
 };
 

corelib/include/rtabmap/core/Transform.h

@@ -56,6 +56,8 @@ public:
 	// x,y, theta
 	Transform(float x, float y, float theta);
 
+	Transform clone() const;
+
 	float r11() const {return data()[0];}
 	float r12() const {return data()[1];}
 	float r13() const {return data()[2];}
@@ -112,6 +114,7 @@ public:
 	float getDistance(const Transform & t) const;
 	float getDistanceSquared(const Transform & t) const;
 	Transform interpolate(float t, const Transform & other) const;
+	void normalizeRotation();
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;

corelib/include/rtabmap/core/VWDictionary.h

@@ -109,6 +109,7 @@ protected:
 private:
 	bool _incrementalDictionary;
 	bool _incrementalFlann;
+	float _rebalancingFactor;
 	float _nndrRatio;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt)
 	bool _newWordsComparedTogether;

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

@@ -45,14 +45,34 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		pcl::IndicesPtr * flatObstacles) const
 {
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
-
-	// voxelize to grid cell size
-	cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
 	pcl::IndicesPtr indices(new std::vector<int>);
-	indices->resize(cloud->size());
-	for(unsigned int i=0; i<indices->size(); ++i)
+
+	if(preVoxelFiltering_)
 	{
-		indices->at(i) = i;
+		// voxelize to grid cell size
+		cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
+
+		indices->resize(cloud->size());
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			indices->at(i) = i;
+		}
+	}
+	else
+	{
+		cloud = cloudIn;
+		if(indicesIn->empty() && cloud->is_dense)
+		{
+			indices->resize(cloud->size());
+			for(unsigned int i=0; i<indices->size(); ++i)
+			{
+				indices->at(i) = i;
+			}
+		}
+		else
+		{
+			indices = indicesIn;
+		}
 	}
 
 	// add pose rotation without yaw

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

@@ -83,8 +83,6 @@ void segmentObstaclesFromGround(
 				normalKSearch,
 				viewPoint);
 
-		UDEBUG("cloud=%d, indices=%d flatSurfaces=%d", (int)cloud->size(), (int)indices->size(), (int)flatSurfaces->size());
-
 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
 			int biggestFlatSurfaceIndex;
@@ -95,7 +93,6 @@ void segmentObstaclesFromGround(
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);
-			UDEBUG("clusteredFlatSurfaces=%d", (int)clusteredFlatSurfaces.size());
 
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
@@ -112,8 +109,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(centroid[2] >= min[2]-0.01 &&
-							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight!=0.0f && centroid[2] <= maxGroundHeight+0.01))) // epsilon
+							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= max[2]) // epsilon
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -140,8 +136,6 @@ void segmentObstaclesFromGround(
 			ground = flatSurfaces;
 		}
 
-		UDEBUG("ground=%d", (int)ground->size());
-
 		if(ground->size() != cloud->size())
 		{
 			// Remove ground

corelib/include/rtabmap/core/util2d.h

@@ -54,8 +54,8 @@ std::vector<cv::Point2f> RTABMAP_EXP calcStereoCorrespondences(
 		cv::Size winSize = cv::Size(6,3),
 		int maxLevel = 3,
 		int iterations = 5,
-		int minDisparity = 0,
-		int maxDisparity = 64,
+		float minDisparity = 0.0f,
+		float maxDisparity = 64.0f,
 		bool ssdApproach = true); // SSD by default, otherwise it is SAD
 
 // exactly as cv::calcOpticalFlowPyrLK but it should be called with pyramid (from cv::buildOpticalFlowPyramid()) and delta drops the y error.
@@ -107,7 +107,7 @@ float RTABMAP_EXP getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,
 		bool smoothing,
-		float maxZError = 0.02f,
+		float depthErrorRatio = 0.02f, //ratio
 		bool estWithNeighborsIfNull = false);
 
 cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::string & roiRatios);

corelib/include/rtabmap/core/util3d.h

@@ -72,7 +72,13 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		float cx, float cy,
 		float fx, float fy,
 		bool smoothing,
-		float maxZError = 0.02f);
+		float depthErrorRatio = 0.02f);
+
+Eigen::Vector3f RTABMAP_EXP projectDepthTo3DRay(
+		const cv::Size & imageSize,
+		float x, float y,
+		float cx, float cy,
+		float fx, float fy);
 
 RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
@@ -190,36 +196,68 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
 		float maxDepth,
 		float minDepth);
 
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs = true);
 // return CV_32FC3  (x,y,z)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
-// return CV_32FC6 (x,y,z,normal_z,normal_y,normalz)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC4 (x,y,z,rgb)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
-// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz)
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC4 (x,y,z,I)
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z)
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC2  (x,y)
-cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
-// For laserScan of type CV_32FC2, z is set to null.
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC3  (x,y,I)
+cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC5  (x,y,normal_x, normal_y, normal_z)
+cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC6  (x,y,I,normal_x, normal_y, normal_z)
+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);
 
-// For laserScan of type CV_32FC2, z is set to null.
-pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
-pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index);
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
-pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
-// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
-pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);
+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.
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform = Transform());
+// For laserScan without rgb, rgb is set to default r,g,b parameters.
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+// For laserScan without intensity, intensity is set to intensity parameter.
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
+// For laserScan without rgb, rgb is set to default r,g,b parameters.
+// For laserScan without normals, normals are set to null.
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+// For laserScan without intensity, intensity is set to default intensity parameter.
+// For laserScan without normals, normals are set to null.
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
+
+// For 2d laserScan, z is set to null.
+pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const LaserScan & laserScan, int index);
+// For laserScan without normals, normals are set to null.
+pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const LaserScan & laserScan, int index);
+// For laserScan without rgb, rgb is set to default r,g,b parameters.
+pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
+// For laserScan without intensity, intensity is set to intensity parameter.
+pcl::PointXYZI RTABMAP_EXP laserScanToPointI(const LaserScan & laserScan, int index, float intensity);
+// For laserScan without rgb, rgb is set to default r,g,b parameters.
+// For laserScan without normals, normals are set to null.
+pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);
+// For laserScan without intensity, intensity is set to default intensity parameter.
+// For laserScan without normals, normals are set to null.
+pcl::PointXYZINormal RTABMAP_EXP laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity);
 
 void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max);
 void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max);
@@ -304,22 +342,22 @@ void RTABMAP_EXP savePCDWords(
 		const std::multimap<int, cv::Point3f> & words,
 		const Transform & transform = Transform::getIdentity());
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
+/**
+ * Assume KITTI velodyne format
+ * Return scan 4 channels (format=XYZI).
+ */
+cv::Mat RTABMAP_EXP loadBINScan(const std::string & fileName);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName);
+RTABMAP_DEPRECATED(pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim), "Use interface without dim argument.");
 
-// Load *.pcd, *.ply or *.bin (KITTI format) with optional filtering.
-// If normals are computed (normalsK>0), the returned scan type is CV_32FC6 instead of CV_32FC3
-cv::Mat RTABMAP_EXP loadScan(
+// Load *.pcd, *.ply or *.bin (KITTI format).
+LaserScan RTABMAP_EXP loadScan(const std::string & path);
+
+RTABMAP_DEPRECATED(pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadCloud(
 		const std::string & path,
 		const Transform & transform = Transform::getIdentity(),
 		int downsampleStep = 1,
-		float voxelSize = 0.0f,
-		int normalsK = 0);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadCloud(
-		const std::string & path,
-		const Transform & transform = Transform::getIdentity(),
-		int downsampleStep = 1,
-		float voxelSize = 0.0f);
+		float voxelSize = 0.0f), "Use loadScan() instead.");
 
 } // namespace util3d
 } // namespace rtabmap

corelib/include/rtabmap/core/util3d_filtering.h

@@ -30,11 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 #include <rtabmap/core/Transform.h>
-
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
 #include <pcl/ModelCoefficients.h>
+#include <rtabmap/core/LaserScan.h>
 
 namespace rtabmap
 {
@@ -42,8 +42,29 @@ namespace rtabmap
 namespace util3d
 {
 
-cv::Mat RTABMAP_EXP downsample(
-		const cv::Mat & cloud,
+/**
+ * Do some filtering approaches and try to
+ * avoid converting between pcl and opencv and to avoid not needed
+ * operations like computing normals while the scan has already
+ * normals and voxel filtering is not used.
+ */
+LaserScan RTABMAP_EXP commonFiltering(
+		const LaserScan & scan,
+		int downsamplingStep,
+		float rangeMin = 0.0f,
+		float rangeMax = 0.0f,
+		float voxelSize = 0.0f,
+		int normalK = 0,
+		float normalRadius = 0.0f,
+		bool forceGroundNormalsUp = false);
+
+LaserScan RTABMAP_EXP rangeFiltering(
+		const LaserScan & scan,
+		float rangeMin,
+		float rangeMax);
+
+LaserScan RTABMAP_EXP downsample(
+		const LaserScan & cloud,
 		int step);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP downsample(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -51,6 +72,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP downsample(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int step);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		int step);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP downsample(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		int step);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -68,6 +95,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float voxelSize);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float voxelSize);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float voxelSize);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float voxelSize);
@@ -80,6 +115,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		float voxelSize);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		float voxelSize);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float voxelSize);
 
 inline pcl::PointCloud<pcl::PointXYZ>::Ptr uniformSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -123,6 +164,34 @@ pcl::IndicesPtr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,
@@ -135,12 +204,30 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const std::string & axis,
 		float min,
 		float max,
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -149,6 +236,13 @@ pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -156,18 +250,37 @@ pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Eigen::Vector4f & min,
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Eigen::Vector4f & min,
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 pcl::IndicesPtr RTABMAP_EXP frustumFiltering(
@@ -431,6 +544,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		int normalKSearch,
+		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -474,6 +594,13 @@ std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
 		int minClusterSize,
 		int maxClusterSize = std::numeric_limits<int>::max(),
 		int * biggestClusterIndex = 0);
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
 std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -493,6 +620,10 @@ pcl::IndicesPtr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		bool negative);
+pcl::IndicesPtr RTABMAP_EXP extractIndices(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative);
 pcl::IndicesPtr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -512,6 +643,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP extractIndices(
 		const pcl::IndicesPtr & indices,
 		bool negative,
 		bool keepOrganized);
+// PCL default lacks of pcl::PointNormal type support
+//pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP extractIndices(
+//		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+//		const pcl::IndicesPtr & indices,
+//		bool negative,
+//		bool keepOrganized);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

corelib/include/rtabmap/core/util3d_mapping.h

@@ -45,8 +45,8 @@ namespace util3d
 
 RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
 		const cv::Mat & scan, // in /base_link frame
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled = false,
 		float scanMaxRange = 0.0f), "Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base.");
@@ -54,8 +54,8 @@ RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
 RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
 		const cv::Mat & scan, // in /base_link frame
 		const cv::Point3f & viewpoint, // /base_link -> /base_scan
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled = false,
 		float scanMaxRange = 0.0f), "Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method.");
@@ -64,8 +64,8 @@ void RTABMAP_EXP occupancy2DFromLaserScan(
 		const cv::Mat & scanHit, // in /base_link frame
 		const cv::Mat & scanNoHit, // in /base_link frame
 		const cv::Point3f & viewpoint, // /base_link -> /base_scan
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled = false,
 		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
@@ -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_registration.h

@@ -63,6 +63,7 @@ void RTABMAP_EXP computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference
 		double & variance,
 		int & correspondencesOut);
 void RTABMAP_EXP computeVarianceAndCorrespondences(

corelib/include/rtabmap/core/util3d_surface.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/ProgressState.h>
+#include <rtabmap/core/LaserScan.h>
 #include <set>
 #include <list>
 
@@ -174,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,
@@ -193,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
@@ -212,38 +240,99 @@ 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,
+		float searchRadius);
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
-pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals2D(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,
 		float normalSmoothingSize = 10.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
-pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float maxDepthChangeFactor = 0.02f,
 		float normalSmoothingSize = 10.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
+float RTABMAP_EXP computeNormalsComplexity(
+		const LaserScan & scan,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::Normal> & normals,
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
+float RTABMAP_EXP computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		bool is2d = false,
+		cv::Mat * pcaEigenVectors = 0,
+		cv::Mat * pcaEigenValues = 0);
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float searchRadius = 0.0f,
@@ -266,6 +355,18 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		float dilationVoxelSize = 1.0f,  // VOXEL_GRID_DILATION
 		int dilationIterations = 0);     // VOXEL_GRID_DILATION
 
+LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
+		const LaserScan & scan,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp);
+void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		bool forceGroundNormalsUp = false);
+void RTABMAP_EXP adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
+		bool forceGroundNormalsUp = false);
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,

corelib/include/rtabmap/core/util3d_transforms.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/LaserScan.h>
 
 namespace rtabmap
 {
@@ -41,15 +42,15 @@ namespace rtabmap
 namespace util3d
 {
 
-cv::Mat RTABMAP_EXP transformLaserScan(
-		const cv::Mat & laserScan,
+LaserScan RTABMAP_EXP transformLaserScan(
+		const LaserScan & laserScan,
 		const Transform & transform);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & transform);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const Transform & transform);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -60,13 +61,16 @@ pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP transformPointCloud(
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Transform & transform);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
@@ -81,6 +85,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
 
 cv::Point3f RTABMAP_EXP transformPoint(
 		const cv::Point3f & pt,
@@ -88,6 +96,9 @@ cv::Point3f RTABMAP_EXP transformPoint(
 pcl::PointXYZ RTABMAP_EXP transformPoint(
 		const pcl::PointXYZ & pt,
 		const Transform & transform);
+pcl::PointXYZI RTABMAP_EXP transformPoint(
+		const pcl::PointXYZI & pt,
+		const Transform & transform);
 pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 		const pcl::PointXYZRGB & pt,
 		const Transform & transform);
@@ -97,6 +108,9 @@ pcl::PointNormal RTABMAP_EXP transformPoint(
 pcl::PointXYZRGBNormal RTABMAP_EXP transformPoint(
 		const pcl::PointXYZRGBNormal & point,
 		const Transform & transform);
+pcl::PointXYZINormal RTABMAP_EXP transformPoint(
+		const pcl::PointXYZINormal & point,
+		const Transform & transform);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/src/BayesFilter.cpp

@@ -30,6 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Parameters.h"
 #include <iostream>
+#include <set>
+#if __cplusplus >= 201103L
+#include <unordered_map>
+#include <unordered_set>
+#endif
 
 #include "rtabmap/utilite/UtiLite.h"
 
@@ -125,6 +130,7 @@ void BayesFilter::reset()
 {
 	_posterior.clear();
 	_prediction = cv::Mat();
+	_neighborsIndex.clear();
 }
 
 const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
@@ -219,7 +225,43 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
 	return _posterior;
 }
 
-cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
+float addNeighborProb(cv::Mat & prediction,
+			unsigned int col,
+			const std::map<int, int> & neighbors,
+			const std::vector<double> & predictionLC,
+#if __cplusplus >= 201103L
+			const std::unordered_map<int, int> & idToIndex
+#else
+			const std::map<int, int> & idToIndex
+#endif
+			)
+{
+	UASSERT(col < (unsigned int)prediction.cols &&
+			col < (unsigned int)prediction.rows);
+
+	float sum=0.0f;
+	float * dataPtr = (float*)prediction.data;
+	for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+	{
+		if(iter->first>=0)
+		{
+#if __cplusplus >= 201103L
+			std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
+#else
+			std::map<int, int>::const_iterator jter = idToIndex.find(iter->first);
+#endif
+			if(jter != idToIndex.end())
+			{
+				UASSERT((iter->second+1) < (int)predictionLC.size());
+				sum += dataPtr[col + jter->second*prediction.cols] = predictionLC[iter->second+1];
+			}
+		}
+	}
+	return sum;
+}
+
+
+cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
 {
 	if(!_fullPredictionUpdate && !_prediction.empty())
 	{
@@ -236,13 +278,21 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 	UTimer timerGlobal;
 	timerGlobal.start();
 
-	std::map<int, int> idToIndexMap;
+#if __cplusplus >= 201103L
+	std::unordered_map<int,int> idToIndexMap;
+	idToIndexMap.reserve(ids.size());
+#else
+	std::map<int,int> idToIndexMap;
+#endif
 	for(unsigned int i=0; i<ids.size(); ++i)
 	{
-		UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
-		idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
+		if(ids[i]>0)
+		{
+			idToIndexMap[ids[i]] = i;
+		}
 	}
 
+
 	//int rows = prediction.rows;
 	cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
 	int cols = prediction.cols;
@@ -260,7 +310,13 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 				// Set high values (gaussians curves) to loop closure neighbors
 
 				// ADD prob for each neighbors
-				std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true);
+				std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true, true);
+
+				if(!_fullPredictionUpdate)
+				{
+					uInsert(_neighborsIndex, std::make_pair(ids[i], neighbors));
+				}
+
 				std::list<int> idsLoopMargin;
 				//filter neighbors in STM
 				for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -271,7 +327,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 					}
 					else
 					{
-						if(iter->second == 0)
+						if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end())
 						{
 							idsLoopMargin.push_back(iter->first);
 						}
@@ -288,10 +344,16 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 				// same neighbor tree for loop signatures (margin = 0)
 				for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
 				{
+					if(!_fullPredictionUpdate)
+					{
+						uInsert(_neighborsIndex, std::make_pair(*iter, neighbors));
+					}
+
 					float sum = 0.0f; // sum values added
-					sum += this->addNeighborProb(prediction, idToIndexMap.at(*iter), neighbors, idToIndexMap);
+					int index = idToIndexMap.at(*iter);
+					sum += addNeighborProb(prediction, index, neighbors, _predictionLC, idToIndexMap);
 					idsDone.insert(*iter);
-					this->normalize(prediction, idToIndexMap.at(*iter), sum, ids[0]<0);
+					this->normalize(prediction, index, sum, ids[0]<0);
 				}
 			}
 			else
@@ -405,7 +467,7 @@ void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float adde
 cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 		const Memory * memory,
 		const std::vector<int> & oldIds,
-		const std::vector<int> & newIds) const
+		const std::vector<int> & newIds)
 {
 	UTimer timer;
 	UDEBUG("");
@@ -417,34 +479,40 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 		oldIds.size() == (unsigned int)oldPrediction.rows);
 
 	cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
+	UDEBUG("time creating prediction = %fs", timer.restart());
 
 	// Create id to index maps
-	std::map<int, int> oldIdToIndexMap;
-	std::map<int, int> newIdToIndexMap;
-	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
+#if __cplusplus >= 201103L
+	std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
+#else
+	std::set<int> oldIdsSet(oldIds.begin(), oldIds.end());
+#endif
+	UDEBUG("time creating old ids set = %fs", timer.restart());
+
+#if __cplusplus >= 201103L
+	std::unordered_map<int,int> newIdToIndexMap;
+	newIdToIndexMap.reserve(newIds.size());
+#else
+	std::map<int,int> newIdToIndexMap;
+#endif
+	for(unsigned int i=0; i<newIds.size(); ++i)
 	{
-		if(i<oldIds.size())
+		if(newIds[i]>0)
 		{
-			UASSERT(oldIds[i]);
-			oldIdToIndexMap.insert(oldIdToIndexMap.end(), std::make_pair(oldIds[i], i));
-			//UDEBUG("oldIdToIndexMap[%d] = %d", oldIds[i], i);
-		}
-		if(i<newIds.size())
-		{
-			UASSERT(newIds[i]);
-			newIdToIndexMap.insert(newIdToIndexMap.end(), std::make_pair(newIds[i], i));
-			//UDEBUG("newIdToIndexMap[%d] = %d", newIds[i], i);
+			newIdToIndexMap[newIds[i]] = i;
 		}
 	}
-	UDEBUG("time creating id-index maps = %fs", timer.restart());
+
+	UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
 
 	//Get removed ids
 	std::set<int> removedIds;
 	for(unsigned int i=0; i<oldIds.size(); ++i)
 	{
-		if(!uContains(newIdToIndexMap, oldIds[i]))
+		if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
 		{
 			removedIds.insert(removedIds.end(), oldIds[i]);
+			_neighborsIndex.erase(oldIds[i]);
 			UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
 		}
 	}
@@ -476,16 +544,32 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 				UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
 			}
 		}
-		if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
+		if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
 		{
-			std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true);
-			float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
+			if(_neighborsIndex.find(newIds[i]) == _neighborsIndex.end())
+			{
+				std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
+
+				for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+				{
+					std::map<int, std::map<int, int> >::iterator jter = _neighborsIndex.find(iter->first);
+					if(jter != _neighborsIndex.end())
+					{
+						uInsert(jter->second, std::make_pair(newIds[i], iter->second));
+					}
+				}
+				_neighborsIndex.insert(std::make_pair(newIds[i], neighbors));
+			}
+			const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]);
+			//std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
+
+			float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap);
 			this->normalize(prediction, i, sum, newIds[0]<0);
 			++added;
 			int count = 0;
-			for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+			for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
 			{
-				if(uContains(oldIdToIndexMap, iter->first) &&
+				if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
 				   removedIds.find(iter->first) == removedIds.end())
 				{
 					idsToUpdate.insert(iter->first);
@@ -497,51 +581,33 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	}
 	UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart());
 
+	UTimer t1;
+	double e0=0,e1=0, e2=0, e3=0, e4=0;
 	// update modified/added ids
 	int modified = 0;
-	std::set<int> idsDone;
 	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
 	{
-		if(idsDone.find(*iter) == idsDone.end() && *iter > 0)
+		int id = *iter;
+		if(id > 0)
 		{
-			std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
+			int index = newIdToIndexMap.at(id);
 
-			std::list<int> idsLoopMargin;
-			//filter neighbors in STM
-			for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end();)
-			{
-				if(memory->isInSTM(jter->first))
-				{
-					neighbors.erase(jter++);
-				}
-				else
-				{
-					if(jter->second == 0)
-					{
-						idsLoopMargin.push_back(jter->first);
-					}
-					++jter;
-				}
-			}
+			e0 = t1.ticks();
+			std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id);
+			UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str());
+			const std::map<int, int> & neighbors = kter->second;
+			//std::map<int, int> neighbors = memory->getNeighborsId(id, _predictionLC.size()-1, 0, false, false, true, true);
+			e1+=t1.ticks();
 
-			// should at least have 1 id in idsMarginLoop
-			if(idsLoopMargin.size() == 0)
-			{
-				UFATAL("No 0 margin neighbor for signature %d !?!?", *iter);
-			}
+			float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap);
+			e3+=t1.ticks();
 
-			// same neighbor tree for loop signatures (margin = 0)
-			for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
-			{
-				int index = newIdToIndexMap.at(*iter);
-				float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
-				idsDone.insert(*iter);
-				this->normalize(prediction, index, sum, newIds[0]<0);
-				++modified;
-			}
+			this->normalize(prediction, index, sum, newIds[0]<0);
+			++modified;
+			e4+=t1.ticks();
 		}
 	}
-	UDEBUG("time updating modified/added %d ids = %fs", idsToUpdate.size(), timer.restart());
+	UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
 
 	//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
 	//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
@@ -624,24 +690,4 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
 	_posterior = newPosterior;
 }
 
-float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
-{
-	UASSERT((unsigned int)prediction.cols == idToIndexMap.size() &&
-			(unsigned int)prediction.rows == idToIndexMap.size() &&
-			col < (unsigned int)prediction.cols &&
-			col < (unsigned int)prediction.rows);
-
-	float sum=0;
-	for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
-	{
-		int index = uValue(idToIndexMap, iter->first, -1);
-		if(index >= 0)
-		{
-			sum += ((float*)prediction.data)[col + index*prediction.cols] = _predictionLC[iter->second+1];
-		}
-	}
-	return sum;
-}
-
-
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -11,6 +11,8 @@ SET(SRC_FILES
 	DBDriverSqlite3.cpp
 	DBReader.cpp
 	
+	Recovery.cpp
+	
     Camera.cpp
     CameraThread.cpp
     CameraRGB.cpp
@@ -44,6 +46,7 @@ SET(SRC_FILES
 	Graph.cpp
 	Compression.cpp
 	Link.cpp
+	LaserScan.cpp
 	
 	Optimizer.cpp
 	OptimizerTORO.cpp
@@ -63,8 +66,11 @@ SET(SRC_FILES
 	OdometryFovis.cpp
 	OdometryViso2.cpp
 	OdometryDVO.cpp
+	OdometryOkvis.cpp
 	OdometryORBSLAM2.cpp
 	
+	IMUThread.cpp
+	
 	Stereo.cpp
 	StereoDense.cpp
 	StereoCameraModel.cpp
@@ -90,9 +96,20 @@ 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)
+    FILE(GLOB HEADERS
+       ../include/${PROJECT_PREFIX}/core/*.h
+    )
+    SET(SRC_FILES ${SRC_FILES} ${HEADERS})
+ENDIF(MSVC)
 
 SET(INCLUDE_DIRS
     ${PROJECT_SOURCE_DIR}/utilite/include
@@ -146,6 +163,17 @@ IF(freenect2_FOUND)
 	)
 ENDIF(freenect2_FOUND)
 
+IF(KinectSDK2_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${KinectSDK2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${KinectSDK2_LIBRARIES}
+	)
+ENDIF(KinectSDK2_FOUND)
+
 IF(RealSense_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -213,25 +241,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} 
@@ -243,6 +252,17 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)
 
+IF(libpointmatcher_FOUND)
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${libpointmatcher_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${libpointmatcher_LIBRARIES}
+	)
+ENDIF(libpointmatcher_FOUND)
+
 IF(ZED_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -312,17 +332,57 @@ IF(dvo_core_FOUND)
 	)
 ENDIF(dvo_core_FOUND)
 
-IF(ORB_SLAM2_FOUND)
+IF(okvis_FOUND)
     SET(INCLUDE_DIRS
+		${OKVIS_INCLUDE_DIRS}
+		${BRISK_INCLUDE_DIRS}
+		${OPENGV_INCLUDE_DIRS}
+		${CERES_INCLUDE_DIRS}
 		${INCLUDE_DIRS}
-		${ORB_SLAM2_INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
+		${OKVIS_LIBRARIES}
+		${BRISK_LIBRARIES}
+		${OPENGV_LIBRARIES}
+		${CERES_LIBRARIES}
+		${LIBRARIES}
+	)
+ENDIF(okvis_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
+		${INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+	    ${ORB_SLAM2_LIBRARIES}
 		${LIBRARIES}
-		${ORB_SLAM2_LIBRARIES}
 	)
 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/Camera.cpp

@@ -59,6 +59,11 @@ Camera::~Camera()
 	UDEBUG("");
 }
 
+void Camera::resetTimer()
+{
+	_frameRateTimer->start();
+}
+
 SensorData Camera::takeImage(CameraInfo * info)
 {
 	bool warnFrameRateTooHigh = false;

corelib/src/CameraRGB.cpp

@@ -70,6 +70,8 @@ CameraImages::CameraImages() :
 		_scanDownsampleStep(1),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_scanNormalsRadius(0),
+		_scanForceGroundNormalsUp(false),
 		_depthFromScan(false),
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
@@ -77,6 +79,7 @@ CameraImages::CameraImages() :
 		_syncImageRateWithStamps(true),
 		_odometryFormat(0),
 		_groundTruthFormat(0),
+		_maxPoseTimeDiff(0.02),
 		_captureDelay(0.0)
 	{}
 CameraImages::CameraImages(const std::string & path,
@@ -99,6 +102,8 @@ CameraImages::CameraImages(const std::string & path,
 	_scanDownsampleStep(1),
 	_scanVoxelSize(0.0f),
 	_scanNormalsK(0),
+	_scanNormalsRadius(0),
+	_scanForceGroundNormalsUp(false),
 	_depthFromScan(false),
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
@@ -106,6 +111,7 @@ CameraImages::CameraImages(const std::string & path,
 	_syncImageRateWithStamps(true),
 	_odometryFormat(0),
 	_groundTruthFormat(0),
+	_maxPoseTimeDiff(0.02),
 	_captureDelay(0.0)
 {
 
@@ -238,15 +244,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			const std::list<std::string> & filenames = _dir->getFileNames();
 			for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
 			{
-				// format is text_12234456.12334_text.png
+				// format is text_1223445645.12334_text.png or text_122344564512334_text.png
+				// If no decimals, 10 first number are the seconds
 				std::list<std::string> list = uSplit(*iter, '.');
-				if(list.size() == 3)
+				if(list.size() == 3 || list.size() == 2)
 				{
 					list.pop_back(); // remove extension
-					std::string decimals = uSplitNumChar(list.back()).front();
-					list.pop_back();
-					std::string sec = uSplitNumChar(list.back()).back();
-					double stamp = uStr2Double(sec + "." + decimals);
+					double stamp = 0.0;
+					if(list.size() == 1)
+					{
+						std::list<std::string> numberList = uSplitNumChar(list.front());
+						for(std::list<std::string>::iterator iter=numberList.begin(); iter!=numberList.end(); ++iter)
+						{
+							if(uIsNumber(*iter))
+							{
+								std::string decimals;
+								std::string sec;
+								if(iter->length()>10)
+								{
+									decimals = iter->substr(10, iter->size()-10);
+									sec = iter->substr(0, 10);
+								}
+								else
+								{
+									sec = *iter;
+								}
+								stamp = uStr2Double(sec + "." + decimals);
+								break;
+							}
+						}
+					}
+					else
+					{
+						std::string decimals = uSplitNumChar(list.back()).front();
+						list.pop_back();
+						std::string sec = uSplitNumChar(list.back()).back();
+						stamp = uStr2Double(sec + "." + decimals);
+					}
 					if(stamp > 0.0)
 					{
 						_stamps.push_back(stamp);
@@ -310,12 +344,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 
 		if(success && _odometryPath.size())
 		{
-			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat);
+			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
 		}
 
 		if(success && _groundTruthPath.size())
 		{
-			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat);
+			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
 		}
 	}
 
@@ -324,7 +358,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	return success;
 }
 
-bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format) const
+bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format, double maxTimeDiff) const
 {
 	outputPoses.clear();
 	std::map<int, Transform> poses;
@@ -334,19 +368,19 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
 		UERROR("Cannot read pose file \"%s\".", filePath.c_str());
 		return false;
 	}
-	else if((format != 1 && format != 5 && format != 6 && format != 7) && poses.size() != this->imagesCount())
+	else if((format != 1 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
 	{
 		UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
 				"the pose file path if you don't want to use it (current file path=%s).",
 				(int)poses.size(), this->imagesCount(), filePath.c_str());
 		return false;
 	}
-	else if((format == 1 || format == 5 || format == 6 || format == 7) && inOutStamps.size() == 0)
+	else if((format == 1 || format == 5 || format == 6 || format == 7 || format == 9) && inOutStamps.size() == 0)
 	{
-		UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats, images must have timestamps!");
+		UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
 		return false;
 	}
-	else if(format == 1 || format == 5 || format == 6 || format == 7)
+	else if(format == 1 || format == 5 || format == 6 || format == 7 || format == 9)
 	{
 		UDEBUG("");
 		//Match ground truth values with images
@@ -378,16 +412,21 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
 					double stampBeg = beginIter->first;
 					double stampEnd = endIter->first;
 					UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
-					if(stampEnd - stampBeg > 10.0)
+					if(fabs(*ster-stampEnd) > maxTimeDiff || fabs(*ster-stampBeg) > maxTimeDiff)
 					{
-						warned = true;
-						UDEBUG("Cannot interpolate pose for stamp %f between %f and %f (>10 sec)",
-							*ster,
-							stampBeg,
-							stampEnd);
+						if(!warned)
+						{
+							UWARN("Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
+								*ster,
+								stampBeg,
+								stampEnd,
+								maxTimeDiff);
+						}
+						warned=true;
 					}
 					else
 					{
+						warned=false;
 						float t = (*ster - stampBeg) / (stampEnd-stampBeg);
 						Transform & ta = poses.at(beginIter->second);
 						Transform & tb = poses.at(endIter->second);
@@ -490,7 +529,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	_captureDelay = 0.0;
 
 	cv::Mat img;
-	cv::Mat scan;
+	LaserScan scan(cv::Mat(), _scanMaxPts, 0, LaserScan::kUnknown, _scanLocalTransform);
 	double stamp = UTimer::now();
 	Transform odometryPose;
 	Transform groundTruthPose;
@@ -531,6 +570,26 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					}
 				}
 			}
+
+			if(_stamps.size())
+			{
+				stamp = _stamps.front();
+				_stamps.pop_front();
+				if(_stamps.size())
+				{
+					_captureDelay = _stamps.front() - stamp;
+				}
+				if(odometry_.size())
+				{
+					odometryPose = odometry_.front();
+					odometry_.pop_front();
+				}
+				if(groundTruth_.size())
+				{
+					groundTruthPose = groundTruth_.front();
+					groundTruth_.pop_front();
+				}
+			}
 		}
 		else
 		{
@@ -539,9 +598,48 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			if(!fileName.empty())
 			{
 				imageFilePath = _path + fileName;
+				if(_stamps.size())
+				{
+					stamp = _stamps.front();
+					_stamps.pop_front();
+					if(_stamps.size())
+					{
+						_captureDelay = _stamps.front() - stamp;
+					}
+					if(odometry_.size())
+					{
+						odometryPose = odometry_.front();
+						odometry_.pop_front();
+					}
+					if(groundTruth_.size())
+					{
+						groundTruthPose = groundTruth_.front();
+						groundTruth_.pop_front();
+					}
+				}
+
 				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
 				{
 					imageFilePath = _path + fileName;
+					if(_stamps.size())
+					{
+						stamp = _stamps.front();
+						_stamps.pop_front();
+						if(_stamps.size())
+						{
+							_captureDelay = _stamps.front() - stamp;
+						}
+						if(odometry_.size())
+						{
+							odometryPose = odometry_.front();
+							odometry_.pop_front();
+						}
+						if(groundTruth_.size())
+						{
+							groundTruthPose = groundTruth_.front();
+							groundTruth_.pop_front();
+						}
+					}
 				}
 			}
 			if(_scanDir)
@@ -558,26 +656,6 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			}
 		}
 
-		if(_stamps.size())
-		{
-			stamp = _stamps.front();
-			_stamps.pop_front();
-			if(_stamps.size())
-			{
-				_captureDelay = _stamps.front() - stamp;
-			}
-			if(odometry_.size())
-			{
-				odometryPose = odometry_.front();
-				odometry_.pop_front();
-			}
-			if(groundTruth_.size())
-			{
-				groundTruthPose = groundTruth_.front();
-				groundTruth_.pop_front();
-			}
-		}
-
 		if(!imageFilePath.empty())
 		{
 			ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
@@ -594,9 +672,9 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			{
 				if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
 				{
-					UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
-							"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
-							imageFilePath.c_str());
+					UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\", type=%d). "
+							"Formats supported are 16 bits 1 channel (mm) and 32 bits 1 channel (m).",
+							imageFilePath.c_str(), img.type());
 					img = cv::Mat();
 				}
 
@@ -650,8 +728,9 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		if(!scanFilePath.empty())
 		{
 			// load without filtering
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::loadCloud(scanFilePath, _scanLocalTransform);
-			UDEBUG("Loaded scan=%d points", (int)cloud->size());
+			scan = util3d::loadScan(scanFilePath);
+			scan = LaserScan(scan.data(), _scanMaxPts, 0.0f, scan.format(), _scanLocalTransform);
+			UDEBUG("Loaded scan=%d points", (int)scan.size());
 			if(_depthFromScan && !img.empty())
 			{
 				UDEBUG("Computing depth from scan...");
@@ -665,6 +744,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				}
 				else
 				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
 					depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
 					if(_depthFromScanFillHoles!=0)
 					{
@@ -673,29 +753,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				}
 			}
 			// filter the scan after registration
-			int previousSize = (int)cloud->size();
-			if(_scanDownsampleStep > 1 && cloud->size())
-			{
-				cloud = util3d::downsample(cloud, _scanDownsampleStep);
-				UDEBUG("Downsampling scan (step=%d): %d -> %d", _scanDownsampleStep, previousSize, (int)cloud->size());
-			}
-			previousSize = (int)cloud->size();
-			if(_scanVoxelSize > 0.0f && cloud->size())
-			{
-				cloud = util3d::voxelize(cloud, _scanVoxelSize);
-				UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size());
-			}
-			if(_scanNormalsK > 0 && cloud->size())
-			{
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-				pcl::concatenateFields(*cloud, *normals, *cloudNormals);
-				scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
-			}
-			else
-			{
-				scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform.inverse());
-			}
+			scan = util3d::commonFiltering(scan, _scanDownsampleStep, 0, 0, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp);
 		}
 	}
 	else
@@ -708,7 +766,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		_model.setImageSize(img.size());
 	}
 
-	SensorData data(scan, LaserScanInfo(scan.empty()?0:_scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
+	SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
 
 	if(info && !odometryPose.isNull())

corelib/src/CameraRGBD.cpp

@@ -63,6 +63,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <libfreenect2/config.h>
 #endif
 
+#ifdef RTABMAP_K4W2
+#include <Kinect.h>
+#endif
+
 #ifdef RTABMAP_REALSENSE
 #include <librealsense/rs.hpp>
 #ifdef RTABMAP_REALSENSE_SLAM
@@ -396,7 +400,9 @@ CameraOpenNI2::CameraOpenNI2(
 	_depthFx(0.0f),
 	_depthFy(0.0f),
 	_deviceId(deviceId),
-	_openNI2StampsAndIDsUsed(false)
+	_openNI2StampsAndIDsUsed(false),
+	_depthHShift(0),
+	_depthVShift(0)
 #endif
 {
 }
@@ -496,6 +502,16 @@ void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
 #endif
 }
 
+void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
+{
+#ifdef RTABMAP_OPENNI2
+	UASSERT(horizontal >= 0);
+	UASSERT(vertical >= 0);
+	_depthHShift = horizontal;
+	_depthVShift = vertical;
+#endif
+}
+
 bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 #ifdef RTABMAP_OPENNI2
@@ -848,6 +864,12 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 
 						if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
 						{
+							if (_depthHShift > 0 || _depthVShift > 0)
+							{
+								cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
+								depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
+								depth = out;
+							}
 							depth = _stereoModel.left().rectifyImage(depth, 0);
 							depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
 						}
@@ -1343,7 +1365,8 @@ CameraFreenect2::CameraFreenect2(
 	float maxDepth,
 	bool bilateralFiltering,
 	bool edgeAwareFiltering,
-	bool noiseFiltering) :
+	bool noiseFiltering,
+	const std::string & pipelineName) :
 		Camera(imageRate, localTransform)
 #ifdef RTABMAP_FREENECT2
         ,
@@ -1357,7 +1380,8 @@ CameraFreenect2::CameraFreenect2(
 		maxKinect2Depth_(maxDepth),
 		bilateralFiltering_(bilateralFiltering),
 		edgeAwareFiltering_(edgeAwareFiltering),
-		noiseFiltering_(noiseFiltering)
+		noiseFiltering_(noiseFiltering),
+		pipelineName_(pipelineName)
 #endif
 {
 #ifdef RTABMAP_FREENECT2
@@ -1410,6 +1434,74 @@ CameraFreenect2::~CameraFreenect2()
 #endif
 }
 
+#ifdef RTABMAP_FREENECT2
+libfreenect2::PacketPipeline *createPacketPipelineByName(const std::string & name)
+{
+	std::string availablePipelines;
+#if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
+	availablePipelines += "gl ";
+	if (name == "gl")
+	{
+		UINFO("Using 'gl' pipeline.");
+		return new libfreenect2::OpenGLPacketPipeline();
+	}
+#endif
+#if defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
+	availablePipelines += "cuda cudakde ";
+	if (name == "cuda")
+	{
+		UINFO("Using 'cuda' pipeline.");
+		return new libfreenect2::CudaPacketPipeline();
+	}
+	if (name == "cudakde")
+	{
+		UINFO("Using 'cudakde' pipeline.");
+		return new libfreenect2::CudaKdePacketPipeline();
+	}
+#endif
+#if defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
+	availablePipelines += "cl clkde ";
+	if (name == "cl")
+	{
+		UINFO("Using 'cl' pipeline.");
+		return new libfreenect2::OpenCLPacketPipeline();
+	}
+	if (name == "clkde")
+	{
+		UINFO("Using 'clkde' pipeline.");
+		return new libfreenect2::OpenCLKdePacketPipeline();
+	}
+#endif
+	availablePipelines += "cpu";
+	if (name == "cpu")
+	{
+		UINFO("Using 'cpu' pipeline.");
+		return new libfreenect2::CpuPacketPipeline();
+	}
+
+	if (!name.empty())
+	{
+		UERROR("'%s' pipeline is not available. Available pipelines are: \"%s\". Default one is used instead (first one in the list).", 
+			name.c_str(), availablePipelines.c_str());
+	}
+
+	// create default pipeline
+#if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
+	UINFO("Using 'gl' pipeline.");
+	return new libfreenect2::OpenGLPacketPipeline();
+#elif defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
+	UINFO("Using 'cuda' pipeline.");
+	return new libfreenect2::CudaPacketPipeline();
+#elif defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
+	UINFO("Using 'cl' pipeline.");
+	return new libfreenect2::OpenCLPacketPipeline();
+#else
+	UINFO("Using 'cpu' pipeline.");
+	return new libfreenect2::CpuPacketPipeline();
+#endif
+}
+#endif
+
 bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 #ifdef RTABMAP_FREENECT2
@@ -1426,20 +1518,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 		reg_ = 0;
 	}
 
-	libfreenect2::PacketPipeline * pipeline;
-#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
-	pipeline = new libfreenect2::CudaPacketPipeline();
-#else
-#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
-	pipeline = new libfreenect2::OpenGLPacketPipeline();
-#else
-#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
-	pipeline = new libfreenect2::OpenCLPacketPipeline();
-#else
-	pipeline = new libfreenect2::CpuPacketPipeline();
-#endif
-#endif
-#endif
+	libfreenect2::PacketPipeline * pipeline = createPacketPipelineByName(pipelineName_);
 
 	if(deviceId_ <= 0)
 	{
@@ -1966,6 +2045,550 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 	return data;
 }
 
+//
+// CameraK4W2
+//
+
+#ifdef RTABMAP_K4W2
+// Safe release for interfaces
+template<class Interface>
+inline void SafeRelease(Interface *& pInterfaceToRelease)
+{
+	if (pInterfaceToRelease != NULL)
+	{
+		pInterfaceToRelease->Release();
+		pInterfaceToRelease = NULL;
+	}
+}
+#endif
+
+bool CameraK4W2::available()
+{
+#ifdef RTABMAP_K4W2
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraK4W2::CameraK4W2(
+	int deviceId,
+	Type type,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_K4W2
+	,
+	type_(type),
+	pKinectSensor_(NULL),
+	pCoordinateMapper_(NULL),
+	pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
+	pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
+	pMultiSourceFrameReader_(NULL),
+	pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
+	hMSEvent(NULL)
+#endif
+{
+}
+
+CameraK4W2::~CameraK4W2()
+{
+#ifdef RTABMAP_K4W2
+	if (pDepthCoordinates_)
+	{
+		delete[] pDepthCoordinates_;
+		pDepthCoordinates_ = NULL;
+	}
+
+	if (pColorCoordinates_)
+	{
+		delete[] pColorCoordinates_;
+		pColorCoordinates_ = NULL;
+	}
+
+	if (pColorRGBX_)
+	{
+		delete[] pColorRGBX_;
+		pColorRGBX_ = NULL;
+	}
+
+	close();
+#endif
+}
+
+void CameraK4W2::close()
+{
+#ifdef RTABMAP_K4W2
+	if (pMultiSourceFrameReader_)
+	{
+		pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
+		CloseHandle((HANDLE)hMSEvent);
+		hMSEvent = NULL;
+	}
+
+	// done with frame reader
+	SafeRelease(pMultiSourceFrameReader_);
+
+	// done with coordinate mapper
+	SafeRelease(pCoordinateMapper_);
+
+	// close the Kinect Sensor
+	if (pKinectSensor_)
+	{
+		pKinectSensor_->Close();
+	}
+
+	SafeRelease(pKinectSensor_);
+
+	colorCameraModel_ = CameraModel();
+#endif
+}
+
+bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_K4W2
+	HRESULT hr;
+
+	close();
+
+	hr = GetDefaultKinectSensor(&pKinectSensor_);
+	if (FAILED(hr))
+	{
+		return false;
+	}
+
+	if (pKinectSensor_)
+	{
+		// Initialize the Kinect and get coordinate mapper and the frame reader
+
+		hr = pKinectSensor_->Open();
+
+		if (SUCCEEDED(hr))
+		{
+			hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
+
+			if (SUCCEEDED(hr))
+			{
+				hr = pKinectSensor_->OpenMultiSourceFrameReader(
+					FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
+					&pMultiSourceFrameReader_);
+
+				if (SUCCEEDED(hr))
+				{
+					hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
+				}
+			}
+		}
+	}
+
+	if (!pKinectSensor_ || FAILED(hr))
+	{
+		UERROR("No ready Kinect found!");
+		close();
+		return false;
+	}
+
+	// to query camera parameters, we should wait a little
+	uSleep(3000);
+
+	// initialize color calibration if not set yet
+	CameraIntrinsics intrinsics;
+	hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
+	if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
+	{
+		// guess color intrinsics by comparing two reprojections
+		CameraModel depthModel(
+			intrinsics.FocalLengthX,
+			intrinsics.FocalLengthY,
+			intrinsics.PrincipalPointX,
+			intrinsics.PrincipalPointY);
+
+		cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
+		hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
+		if (SUCCEEDED(hr))
+		{
+			int firstIndex = -1;
+			int lastIndex = -1;
+			for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
+			{
+				ColorSpacePoint p = pColorCoordinates_[depthIndex];
+				// Values that are negative infinity means it is an invalid color to depth mapping so we
+				// skip processing for this pixel
+				if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+				{
+					if (firstIndex == -1)
+					{
+						firstIndex = depthIndex;
+					}
+					lastIndex = depthIndex;
+				}
+			}
+
+			UASSERT(firstIndex >= 0 && lastIndex >= 0);
+			float fx, fy, cx, cy;
+			float x1, y1, z1, x2, y2, z2;
+			depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
+			depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
+			ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
+			ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
+			fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
+			cx = uv1.X - (x1 / z1) * fx;
+			fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
+			cy = uv1.Y - (y1 / z1) * fy;
+
+			colorCameraModel_ = CameraModel(
+				fx,
+				fy,
+				float(cColorWidth) - cx,
+				cy,
+				this->getLocalTransform(),
+				0,
+				cv::Size(cColorWidth, cColorHeight));
+		}
+	}
+
+	if (!colorCameraModel_.isValidForProjection())
+	{
+		UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
+		close();
+		return false;
+	}
+
+	std::string serial = getSerial();
+	if (!serial.empty())
+	{
+		UINFO("Running kinect device \"%s\"", serial.c_str());
+	}
+
+	return true;
+#else
+	UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
+	return false;
+#endif
+}
+
+bool CameraK4W2::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraK4W2::getSerial() const
+{
+#ifdef RTABMAP_K4W2
+	if (pKinectSensor_)
+	{
+		wchar_t uid[255] = { 0 };
+		// It seems to fail every time!?
+		HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
+		if (SUCCEEDED(hr))
+		{
+			std::wstring ws(uid);
+			return std::string(ws.begin(), ws.end());
+		}
+	}
+#endif
+	return "";
+}
+
+SensorData CameraK4W2::captureImage(CameraInfo * info)
+{
+	SensorData data;
+
+#ifdef RTABMAP_K4W2
+
+	if (!pMultiSourceFrameReader_)
+	{
+		return data;
+	}
+
+	HRESULT hr;
+
+	//now check for frame events
+	HANDLE handles[] = { reinterpret_cast<HANDLE>(hMSEvent) };
+
+	double t = UTimer::now();
+	while((UTimer::now()-t < 5.0) && WaitForMultipleObjects(_countof(handles), handles, false, 5000) == WAIT_OBJECT_0)
+	{
+		IMultiSourceFrameArrivedEventArgs* pArgs = NULL;
+
+		hr = pMultiSourceFrameReader_->GetMultiSourceFrameArrivedEventData(hMSEvent, &pArgs);
+		if (SUCCEEDED(hr))
+		{
+			IMultiSourceFrameReference * pFrameRef = NULL;
+			hr = pArgs->get_FrameReference(&pFrameRef);
+			if (SUCCEEDED(hr))
+			{
+				IMultiSourceFrame* pMultiSourceFrame = NULL;
+				IDepthFrame* pDepthFrame = NULL;
+				IColorFrame* pColorFrame = NULL;
+
+				hr = pFrameRef->AcquireFrame(&pMultiSourceFrame);
+				if (FAILED(hr))
+				{
+					UERROR("Failed getting latest frame.");
+				}
+
+				IDepthFrameReference* pDepthFrameReference = NULL;
+				hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
+				if (SUCCEEDED(hr))
+				{
+					hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
+				}
+				SafeRelease(pDepthFrameReference);
+
+				IColorFrameReference* pColorFrameReference = NULL;
+				hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
+				if (SUCCEEDED(hr))
+				{
+					hr = pColorFrameReference->AcquireFrame(&pColorFrame);
+				}
+				SafeRelease(pColorFrameReference);
+
+				if (pDepthFrame && pColorFrame)
+				{
+					IFrameDescription* pDepthFrameDescription = NULL;
+					int nDepthWidth = 0;
+					int nDepthHeight = 0;
+					UINT nDepthBufferSize = 0;
+					UINT16 *pDepthBuffer = NULL;
+
+					IFrameDescription* pColorFrameDescription = NULL;
+					int nColorWidth = 0;
+					int nColorHeight = 0;
+					ColorImageFormat imageFormat = ColorImageFormat_None;
+					UINT nColorBufferSize = 0;
+					RGBQUAD *pColorBuffer = NULL;
+
+					// get depth frame data
+					if (SUCCEEDED(hr))
+						hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
+					if (SUCCEEDED(hr))
+						hr = pDepthFrameDescription->get_Width(&nDepthWidth);
+					if (SUCCEEDED(hr))
+						hr = pDepthFrameDescription->get_Height(&nDepthHeight);
+					if (SUCCEEDED(hr))
+						hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
+
+					// get color frame data
+					if (SUCCEEDED(hr))
+						hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
+					if (SUCCEEDED(hr))
+						hr = pColorFrameDescription->get_Width(&nColorWidth);
+					if (SUCCEEDED(hr))
+						hr = pColorFrameDescription->get_Height(&nColorHeight);
+					if (SUCCEEDED(hr))
+						hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
+					if (SUCCEEDED(hr))
+					{
+						if (imageFormat == ColorImageFormat_Bgra)
+						{
+							hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
+						}
+						else if (pColorRGBX_)
+						{
+							pColorBuffer = pColorRGBX_;
+							nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
+							hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
+						}
+						else
+						{
+							hr = E_FAIL;
+						}
+					}
+
+					if(SUCCEEDED(hr))
+					{
+						//ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
+						//	pColorBuffer, nColorWidth, nColorHeight,
+						//	pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight);
+
+						// Make sure we've received valid data
+						if (pCoordinateMapper_ &&
+							pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
+							pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
+						{
+							if (type_ == kTypeColor2DepthSD)
+							{
+								HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
+								if (SUCCEEDED(hr))
+								{
+									cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
+									cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
+									// loop over output pixels
+									for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
+									{
+										DepthSpacePoint p = pDepthCoordinates_[colorIndex];
+										// Values that are negative infinity means it is an invalid color to depth mapping so we
+										// skip processing for this pixel
+										if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+										{
+											// To avoid black lines caused by rounding pixel values, we should set 4 pixels
+											// At the same do mirror
+											int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+											pixel_x_l = nDepthWidth - static_cast<int>(p.X);
+											pixel_y_l = static_cast<int>(p.Y);
+											pixel_x_h = pixel_x_l - 1;
+											pixel_y_h = pixel_y_l + 1;
+
+											const RGBQUAD* pSrc = pColorBuffer + colorIndex;
+											if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
+											}
+											if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
+											}
+											if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
+											}
+											if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
+											}
+										}
+									}
+
+									CameraIntrinsics intrinsics;
+									pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
+									CameraModel model(
+										intrinsics.FocalLengthX,
+										intrinsics.FocalLengthY,
+										intrinsics.PrincipalPointX,
+										intrinsics.PrincipalPointY,
+										this->getLocalTransform(),
+										0,
+										depth.size());
+									data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
+								}
+								else
+								{
+									UERROR("Failed color to depth registration!");
+								}
+							}
+							else //depthToColor
+							{
+								HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
+								if (SUCCEEDED(hr))
+								{
+									cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
+									cv::Mat depthRegistered = cv::Mat::zeros(
+										type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight, 
+										type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
+										CV_16UC1);
+									cv::Mat imageColor;
+									if(type_ == kTypeDepth2ColorSD)
+									{
+										cv::Mat tmp;
+										cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
+										cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
+									}
+									else
+									{
+										cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
+									}
+									// loop over output pixels
+									for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
+									{
+										ColorSpacePoint p = pColorCoordinates_[depthIndex];
+										// Values that are negative infinity means it is an invalid color to depth mapping so we
+										// skip processing for this pixel
+										if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+										{
+											if (type_ == kTypeDepth2ColorSD)
+											{
+												p.X /= 2.0f;
+												p.Y /= 2.0f;
+											}
+											const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
+											int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+											// get the coordinate on image plane.
+											pixel_x_l = depthRegistered.cols - p.X; // flip depth
+											pixel_y_l = p.Y;
+											pixel_x_h = pixel_x_l - 1;
+											pixel_y_h = pixel_y_l + 1;
+
+											if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
+												pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line
+												depth_value)
+											{
+												unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
+												if (depthPixel == 0 || depthPixel > depth_value)
+												{
+													depthPixel = depth_value;
+												}
+											}
+											if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
+												pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line
+												depth_value)
+											{
+												unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
+												if (depthPixel == 0 || depthPixel > depth_value)
+												{
+													depthPixel = depth_value;
+												}
+											}
+										}
+									}
+
+									CameraModel model = colorCameraModel_;
+									if (type_ == kTypeDepth2ColorSD)
+									{
+										model = model.scaled(0.5);
+									}
+									util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
+									depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
+									cv::flip(imageColor, imageColor, 1);
+									data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
+								}
+								else
+								{
+									UERROR("Failed depth to color registration!");
+								}
+							}
+						}
+					}
+
+					SafeRelease(pDepthFrameDescription);
+					SafeRelease(pColorFrameDescription);
+				}
+
+				pFrameRef->Release();
+
+				SafeRelease(pDepthFrame);
+				SafeRelease(pColorFrame);
+				SafeRelease(pMultiSourceFrame);
+			}	
+			pArgs->Release();
+		}
+		if (!data.imageRaw().empty())
+		{
+			break;
+		}
+	}
+#else
+	UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
+#endif
+	return data;
+}
+
 /////////////////////////
 // CameraRealSense
 /////////////////////////

corelib/src/CameraStereo.cpp

@@ -379,7 +379,7 @@ bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::
 			// look for calibration files
 			if(!calibrationFolder.empty())
 			{
-				if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName))
+				if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName, false))
 				{
 					UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 							cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str());
@@ -1147,7 +1147,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
-		if(!stereoModel_.load(calibrationFolder, cameraName))
+		if(!stereoModel_.load(calibrationFolder, cameraName, false) && !stereoModel_.isValidForProjection())
 		{
 			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 					cameraName.c_str(), calibrationFolder.c_str());
@@ -1255,7 +1255,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
 				stereoModel_.setImageSize(leftImage.size());
 			}
 
-			data = SensorData(left.laserScanRaw(), left.laserScanInfo(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
+			data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
 			data.setGroundTruth(left.groundTruth());
 		}
 	}
@@ -1372,7 +1372,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName_.empty())
 	{
-		if(!stereoModel_.load(calibrationFolder, cameraName_))
+		if(!stereoModel_.load(calibrationFolder, cameraName_, false))
 		{
 			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 				cameraName_.c_str(), calibrationFolder.c_str());

corelib/src/CameraThread.cpp

@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/StereoDense.h"
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
-
+#include <opencv2/stitching/detail/exposure_compensate.hpp>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
 
@@ -49,6 +49,7 @@ namespace rtabmap
 CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_camera(camera),
 		_mirroring(false),
+		_stereoExposureCompensation(false),
 		_colorOnly(false),
 		_imageDecimation(1),
 		_stereoToDepth(false),
@@ -58,6 +59,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_scanMinDepth(0.0f),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_scanNormalsRadius(0.0f),
 		_stereoDense(new StereoBM(parameters)),
 		_distortionModel(0),
 		_bilateralFiltering(false),
@@ -121,6 +123,7 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
+	_camera->resetTimer();
 }
 
 void CameraThread::mainLoop()
@@ -254,7 +257,9 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 					data.cameraModels()[0].fy(),
 					float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
 					data.cameraModels()[0].cy(),
-					data.cameraModels()[0].localTransform());
+					data.cameraModels()[0].localTransform(),
+					data.cameraModels()[0].Tx(),
+					data.cameraModels()[0].imageSize());
 			data.setCameraModel(tmpModel);
 		}
 		if(!data.depthRaw().empty())
@@ -265,6 +270,33 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 		if(info) info->timeMirroring = timer.ticks();
 	}
+
+	if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
+	{
+#if CV_MAJOR_VERSION < 3
+		UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
+#else
+		UDEBUG("");
+		UTimer timer;
+		cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
+		std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
+		std::vector<cv::UMat> images;
+		std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1,  cv::Scalar(255)));
+		images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
+		images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
+		compensator->feed(topLeftCorners, images, masks);
+		cv::Mat img = data.imageRaw().clone();
+		compensator->apply(0, cv::Point(0,0), img, masks[0]);
+		data.setImageRaw(img);
+		img = data.rightRaw().clone();
+		compensator->apply(1, cv::Point(0,0), img, masks[1]);
+		data.setDepthOrRightRaw(img);
+		cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
+		UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
+		if(info) info->timeStereoExposureCompensation = timer.ticks();
+#endif
+	}
+
 	if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
 	{
 		UDEBUG("");
@@ -280,7 +312,8 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 				data.stereoCameraModel().left().cx(),
 				data.stereoCameraModel().left().cy(),
 				data.stereoCameraModel().localTransform(),
-				-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
+				-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx(),
+				data.stereoCameraModel().left().imageSize());
 		data.setCameraModel(model);
 		data.setDepthOrRightRaw(depth);
 		data.setStereoCameraModel(StereoCameraModel());
@@ -292,12 +325,12 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		!data.depthRaw().empty())
 	{
 		UDEBUG("");
-		if(data.laserScanRaw().empty())
+		if(data.laserScanRaw().isEmpty())
 		{
 			UASSERT(_scanDecimation >= 1);
 			UTimer timer;
 			pcl::IndicesPtr validIndices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
 					data,
 					_scanDecimation,
 					_scanMaxDepth,
@@ -306,6 +339,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 			float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
 			cv::Mat scan;
 			const Transform & baseToScan = data.cameraModels()[0].localTransform();
+			LaserScan::Format format = LaserScan::kXYZRGB;
 			if(validIndices->size())
 			{
 				if(_scanVoxelSize>0.0f)
@@ -316,20 +350,21 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 				}
 				else if(!cloud->is_dense)
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
 					cloud = denseCloud;
 				}
 
 				if(cloud->size())
 				{
-					if(_scanNormalsK>0)
+					if(_scanNormalsK>0 || _scanNormalsRadius>0.0f)
 					{
 						Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
-						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
-						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
+						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
+						format = LaserScan::kXYZRGBNormal;
 					}
 					else
 					{
@@ -337,7 +372,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 					}
 				}
 			}
-			data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
+			data.setLaserScanRaw(LaserScan(scan, (int)maxPoints, _scanMaxDepth, format, baseToScan));
 			if(info) info->timeScanFromDepth = timer.ticks();
 		}
 		else

corelib/src/Compression.cpp

@@ -139,7 +139,11 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
 #endif
 		if(image.type() == CV_8UC4)
 		{
-			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			// Using clone() or copyTo() caused a memory leak !?!?
+			// image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			cv::Mat depth(image.size(), CV_32FC1);
+			memcpy(depth.data, image.data, image.total()*image.elemSize());
+			image = depth;
 		}
 	}
 	return image;
@@ -270,4 +274,21 @@ cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
 	return data;
 }
 
+cv::Mat compressString(const std::string & str)
+{
+	// +1 to include null character
+	return compressData2(cv::Mat(1, str.size()+1, CV_8SC1, (void *)str.data()));
+}
+
+std::string uncompressString(const cv::Mat & bytes)
+{
+	cv::Mat strMat = uncompressData(bytes);
+	if(!strMat.empty())
+	{
+		UASSERT(strMat.type() == CV_8SC1 && strMat.rows == 1);
+		return (const char*)strMat.data;
+	}
+	return "";
+}
+
 } /* namespace rtabmap */

corelib/src/DBDriver.cpp

@@ -245,15 +245,33 @@ ParametersMap DBDriver::getLastParameters() const
 	return parameters;
 }
 
-std::map<std::string, float> DBDriver::getStatistics(int nodeId, double & stamp) const
+std::map<std::string, float> DBDriver::getStatistics(int nodeId, double & stamp, std::vector<int> * wmState) const
 {
 	std::map<std::string, float> statistics;
 	_dbSafeAccessMutex.lock();
-	statistics = getStatisticsQuery(nodeId, stamp);
+	statistics = getStatisticsQuery(nodeId, stamp, wmState);
 	_dbSafeAccessMutex.unlock();
 	return statistics;
 }
 
+std::map<int, std::pair<std::map<std::string, float>, double> > DBDriver::getAllStatistics() const
+{
+	std::map<int, std::pair<std::map<std::string, float>, double> > statistics;
+	_dbSafeAccessMutex.lock();
+	statistics = getAllStatisticsQuery();
+	_dbSafeAccessMutex.unlock();
+	return statistics;
+}
+
+std::map<int, std::vector<int> > DBDriver::getAllStatisticsWmStates() const
+{
+	std::map<int, std::vector<int> > wmStates;
+	_dbSafeAccessMutex.lock();
+	wmStates = getAllStatisticsWmStatesQuery();
+	_dbSafeAccessMutex.unlock();
+	return wmStates;
+}
+
 std::string DBDriver::getDatabaseVersion() const
 {
 	std::string version = "0.0.0";
@@ -386,7 +404,7 @@ void DBDriver::asyncSave(VisualWord * vw)
 	}
 }
 
-void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
+void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures)
 {
 	ULOGGER_DEBUG("");
 	std::list<Signature *> toSave;
@@ -466,17 +484,19 @@ void DBDriver::updateOccupancyGrid(
 		int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles,
+		const cv::Mat & empty,
 		float cellSize,
 		const cv::Point3f & viewpoint)
 {
 	_dbSafeAccessMutex.lock();
 	//just to make sure the occupancy grids are compressed for convenience
 	SensorData data;
-	data.setOccupancyGrid(ground, obstacles, cellSize, viewpoint);
+	data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewpoint);
 	this->updateOccupancyGridQuery(
 			nodeId,
 			data.gridGroundCellsCompressed(),
 			data.gridObstacleCellsCompressed(),
+			data.gridEmptyCellsCompressed(),
 			cellSize,
 			viewpoint);
 	_dbSafeAccessMutex.unlock();
@@ -631,7 +651,7 @@ void DBDriver::getNodeData(
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
 		if(!s->sensorData().imageCompressed().empty() ||
-			!s->sensorData().laserScanCompressed().empty() ||
+			!s->sensorData().laserScanCompressed().isEmpty() ||
 			!s->sensorData().userDataCompressed().empty() ||
 			s->sensorData().gridCellSize() != 0.0f ||
 			!s->isSaved())
@@ -682,7 +702,7 @@ bool DBDriver::getCalibration(
 
 bool DBDriver::getLaserScanInfo(
 		int signatureId,
-		LaserScanInfo & info) const
+		LaserScan & info) const
 {
 	UDEBUG("");
 	bool found = false;
@@ -690,7 +710,7 @@ bool DBDriver::getLaserScanInfo(
 	_trashesMutex.lock();
 	if(uContains(_trashSignatures, signatureId))
 	{
-		info = _trashSignatures.at(signatureId)->sensorData().laserScanInfo();
+		info = _trashSignatures.at(signatureId)->sensorData().laserScanCompressed();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -712,7 +732,8 @@ bool DBDriver::getNodeInfo(
 		std::string & label,
 		double & stamp,
 		Transform & groundTruthPose,
-		std::vector<float> & velocity) const
+		std::vector<float> & velocity,
+		GPS & gps) const
 {
 	bool found = false;
 	// look in the trash
@@ -725,6 +746,7 @@ bool DBDriver::getNodeInfo(
 		label = _trashSignatures.at(signatureId)->getLabel();
 		stamp = _trashSignatures.at(signatureId)->getStamp();
 		groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose();
+		gps = _trashSignatures.at(signatureId)->sensorData().gps();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -732,7 +754,7 @@ bool DBDriver::getNodeInfo(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity, gps);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;
@@ -1039,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,
@@ -1051,22 +1101,21 @@ 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,
+				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
 #else
 				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
 #endif
 				cv::Mat * textures) const
 {
 	_dbSafeAccessMutex.lock();
-	cv::Mat cloud = loadOptimizedMeshQuery(poses, polygons, texCoords, textures);
+	cv::Mat cloud = loadOptimizedMeshQuery(polygons, texCoords, textures);
 	_dbSafeAccessMutex.unlock();
 	return cloud;
 }

corelib/src/DBDriverSqlite3.h

@@ -41,6 +41,7 @@ public:
 	virtual ~DBDriverSqlite3();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return getUrl().empty() || _dbInMemory;}
 	void setDbInMemory(bool dbInMemory);
 	void setJournalMode(int journalMode);
 	void setCacheSize(unsigned int cacheSize);
@@ -69,13 +70,15 @@ private:
 	virtual int getTotalNodesSizeQuery() const;
 	virtual int getTotalDictionarySizeQuery() const;
 	virtual ParametersMap getLastParametersQuery() const;
-	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const;
 
 	virtual void executeNoResultQuery(const std::string & sql) const;
 
 	virtual void getWeightQuery(int signatureId, int & weight) const;
 
-	virtual void saveQuery(const std::list<Signature *> & signatures) const;
+	virtual void saveQuery(const std::list<Signature *> & signatures);
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
@@ -87,6 +90,7 @@ private:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;
 
@@ -97,9 +101,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,
@@ -108,7 +115,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,
@@ -126,8 +132,8 @@ private:
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -161,6 +167,7 @@ private:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;
 
@@ -170,6 +177,7 @@ private:
 
 private:
 	sqlite3 * _ppDb;
+	long _memoryUsedEstimate;
 	std::string _version;
 	bool _dbInMemory;
 	unsigned int _cacheSize;

corelib/src/DBReader.cpp

@@ -268,7 +268,8 @@ SensorData DBReader::captureImage(CameraInfo * info)
 						int mapId;
 						Transform localTransform, pose, groundTruth;
 						std::vector<float> velocity;
-						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
+						GPS gps;
+						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
 						if(previousStamp && stamp && stamp > previousStamp)
 						{
 							delay = stamp - previousStamp;
@@ -322,7 +323,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			double stamp;
 			Transform groundTruth;
 			std::vector<float> velocity;
-			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
+			GPS gps;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
 
 			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
@@ -399,6 +401,10 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				_previousStamp = stamp;
 				_previousMapID = mapId;
 			}
+			else
+			{
+				stamp = 0;
+			}
 
 			data.uncompressData();
 			if(data.cameraModels().size() > 1 &&
@@ -435,8 +441,9 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			data.setId(seq);
 			data.setStamp(stamp);
 			data.setGroundTruth(groundTruth);
+			data.setGPS(gps);
 			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
-					data.laserScanRaw().empty()?0:1,
+					data.laserScanRaw().isEmpty()?0:1,
 					data.imageRaw().empty()?0:1,
 					data.depthOrRightRaw().empty()?0:1,
 					data.userDataRaw().empty()?0:1);
@@ -453,6 +460,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				{
 					info->odomPose = pose;
 					info->odomCovariance = infMatrix.inv();
+					info->odomVelocity = velocity;
 					UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
 				}
 			}

corelib/src/Features2d.cpp

@@ -327,7 +327,9 @@ Feature2D::Feature2D(const ParametersMap & parameters) :
 		_roiRatios(std::vector<float>(4, 0.0f)),
 		_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
 		_subPixIterations(Parameters::defaultKpSubPixIterations()),
-		_subPixEps(Parameters::defaultKpSubPixEps())
+		_subPixEps(Parameters::defaultKpSubPixEps()),
+		gridRows_(Parameters::defaultKpGridRows()),
+		gridCols_(Parameters::defaultKpGridCols())
 {
 	_stereo = new Stereo(parameters);
 	this->parseParameters(parameters);
@@ -346,6 +348,14 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
 	Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
 	Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
+	Parameters::parse(parameters, Parameters::kKpGridRows(), gridRows_);
+	Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);
+
+	UASSERT(gridRows_ >= 1 && gridCols_>=1);
+	if(maxFeatures_ > 0)
+	{
+		maxFeatures_ =	maxFeatures_ / (gridRows_ * gridCols_);
+	}
 
 	// convert ROI from string to vector
 	ParametersMap::const_iterator iter;
@@ -533,23 +543,36 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 
 	std::vector<cv::KeyPoint> keypoints;
 	UTimer timer;
+	cv::Rect globalRoi = Feature2D::computeRoi(image, _roiRatios);
+	if(!(globalRoi.width && globalRoi.height))
+	{
+		globalRoi = cv::Rect(0,0,image.cols, image.rows);
+	}
 
 	// Get keypoints
-	cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
-	keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows), mask);
-	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
-
-	limitKeypoints(keypoints, maxFeatures_);
-
-	if(roi.x || roi.y)
+	int rowSize = globalRoi.height / gridRows_;
+	int colSize = globalRoi.width / gridCols_;
+	for (int i = 0; i<gridRows_; ++i)
 	{
-		// Adjust keypoint position to raw image
-		for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
+		for (int j = 0; j<gridCols_; ++j)
 		{
-			iter->pt.x += roi.x;
-			iter->pt.y += roi.y;
+			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
+			std::vector<cv::KeyPoint> sub_keypoints;
+			sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
+			limitKeypoints(sub_keypoints, maxFeatures_);
+			if(roi.x || roi.y)
+			{
+				// Adjust keypoint position to raw image
+				for(std::vector<cv::KeyPoint>::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter)
+				{
+					iter->pt.x += roi.x;
+					iter->pt.y += roi.y;
+				}
+			}
+			keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
 		}
 	}
+	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
 
 	if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
 	{
@@ -1496,6 +1519,8 @@ KAZE::~KAZE()
 
 void KAZE::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+	
 	Parameters::parse(parameters, Parameters::kKAZEExtended(), extended_);
 	Parameters::parse(parameters, Parameters::kKAZEUpright(), upright_);
 	Parameters::parse(parameters, Parameters::kKAZEThreshold(), threshold_);

corelib/src/FlannIndex.cpp

@@ -38,7 +38,8 @@ FlannIndex::FlannIndex():
 		featuresType_(0),
 		featuresDim_(0),
 		isLSH_(false),
-		useDistanceL1_(false)
+		useDistanceL1_(false),
+		rebalancingFactor_(2.0f)
 {
 }
 FlannIndex::~FlannIndex()
@@ -134,7 +135,8 @@ unsigned int FlannIndex::memoryUsed() const
 
 void FlannIndex::buildLinearIndex(
 		const cv::Mat & features,
-		bool useDistanceL1)
+		bool useDistanceL1,
+		float rebalancingFactor)
 {
 	this->release();
 	UASSERT(index_ == 0);
@@ -142,6 +144,7 @@ void FlannIndex::buildLinearIndex(
 	featuresType_ = features.type();
 	featuresDim_ = features.cols;
 	useDistanceL1_ = useDistanceL1;
+	rebalancingFactor_ = rebalancingFactor;
 
 	rtflann::LinearIndexParams params;
 
@@ -180,7 +183,8 @@ void FlannIndex::buildLinearIndex(
 void FlannIndex::buildKDTreeIndex(
 		const cv::Mat & features,
 		int trees,
-		bool useDistanceL1)
+		bool useDistanceL1,
+		float rebalancingFactor)
 {
 	this->release();
 	UASSERT(index_ == 0);
@@ -188,6 +192,7 @@ void FlannIndex::buildKDTreeIndex(
 	featuresType_ = features.type();
 	featuresDim_ = features.cols;
 	useDistanceL1_ = useDistanceL1;
+	rebalancingFactor_ = rebalancingFactor;
 
 	rtflann::KDTreeIndexParams params(trees);
 
@@ -227,7 +232,8 @@ void FlannIndex::buildKDTreeSingleIndex(
 		const cv::Mat & features,
 		int leafMaxSize,
 		bool reorder,
-		bool useDistanceL1)
+		bool useDistanceL1,
+		float rebalancingFactor)
 {
 	this->release();
 	UASSERT(index_ == 0);
@@ -235,6 +241,7 @@ void FlannIndex::buildKDTreeSingleIndex(
 	featuresType_ = features.type();
 	featuresDim_ = features.cols;
 	useDistanceL1_ = useDistanceL1;
+	rebalancingFactor_ = rebalancingFactor;
 
 	rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
 
@@ -274,7 +281,8 @@ void FlannIndex::buildLSHIndex(
 		const cv::Mat & features,
 		unsigned int table_number,
 		unsigned int key_size,
-		unsigned int multi_probe_level)
+		unsigned int multi_probe_level,
+		float rebalancingFactor)
 {
 	this->release();
 	UASSERT(index_ == 0);
@@ -282,6 +290,7 @@ void FlannIndex::buildLSHIndex(
 	featuresType_ = features.type();
 	featuresDim_ = features.cols;
 	useDistanceL1_ = true;
+	rebalancingFactor_ = rebalancingFactor;
 
 	rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
 	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
@@ -315,8 +324,8 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
 		rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
 		removedPts = index->removedCount();
 		index->addPoints(points, 0);
-		// Rebuild index if it doubles in size
-		if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+		// Rebuild index if it is now X times in size
+		if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 		{
 			UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
 			index->buildIndex();
@@ -333,7 +342,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
 			removedPts = index->removedCount();
 			index->addPoints(points, 0);
 			// Rebuild index if it doubles in size
-			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 			{
 				UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
 				index->buildIndex();
@@ -347,7 +356,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
 			removedPts = index->removedCount();
 			index->addPoints(points, 0);
 			// Rebuild index if it doubles in size
-			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 			{
 				UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
 				index->buildIndex();
@@ -361,7 +370,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
 			removedPts = index->removedCount();
 			index->addPoints(points, 0);
 			// Rebuild index if it doubles in size
-			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
 			{
 				UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
 				index->buildIndex();
@@ -436,10 +445,10 @@ void FlannIndex::knnSearch(
 		UERROR("Flann index not yet created!");
 		return;
 	}
-	indices.create(query.rows, knn, CV_32S);
+	indices.create(query.rows, knn, sizeof(size_t)==8?CV_64F:CV_32S);
 	dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
 
-	rtflann::Matrix<int> indicesF((int*)indices.data, indices.rows, indices.cols);
+	rtflann::Matrix<size_t> indicesF((size_t*)indices.data, indices.rows, indices.cols);
 
 	rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
 

corelib/src/GeodeticCoords.cpp

@@ -51,8 +51,24 @@ namespace rtabmap {
 
 
 inline double DEG2RAD(const double x) { return x*M_PI/180.0;}
+inline double RAD2DEG(const double x) { return x*180.0/M_PI;}
 inline double square(const double & value) {return value*value;}
 
+GeodeticCoords::GeodeticCoords() :
+		latitude_(0.0),
+		longitude_(0.0),
+		altitude_(0.0)
+{
+
+}
+GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
+		latitude_(latitude),
+		longitude_(longitude),
+		altitude_(altitude)
+{
+
+}
+
 //*---------------------------------------------------------------
 //			geodeticToGeocentric_WGS84
 // ---------------------------------------------------------------*/
@@ -123,19 +139,72 @@ cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
 	return out;
 }
 
-GeodeticCoords::GeodeticCoords() :
-		latitude_(0.0),
-		longitude_(0.0),
-		altitude_(0.0)
+void GeodeticCoords::fromGeocentric_WGS84(const cv::Point3d& geocentric)
 {
+	static const double a = 6378137;		// Semi-major axis of the Earth (meters)
+	static const double b = 6356752.3142;	// Semi-minor axis:
 
+	const double sa2 = a*a;
+	const double sb2 = b*b;
+
+	const double e2 = (sa2 - sb2) / sa2;
+	const double ep2 = (sa2 - sb2) / sb2;
+	const double p = std::sqrt(geocentric.x * geocentric.x + geocentric.y * geocentric.y);
+	const double theta = atan2(geocentric.z * a, p * b);
+
+	longitude_ = atan2(geocentric.y, geocentric.x);
+	latitude_ = atan2(
+			geocentric.z + ep2 * b * sin(theta) * sin(theta) * sin(theta),
+		p - e2 * a * cos(theta) * cos(theta) * cos(theta));
+
+	const double clat = cos(latitude_);
+	const double slat = sin(latitude_);
+	const double N = sa2 / std::sqrt(sa2 * clat * clat + sb2 * slat * slat);
+
+	altitude_ = p / clat - N;
+	longitude_ = RAD2DEG(longitude_);
+	latitude_ = RAD2DEG(latitude_);
 }
-GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
-		latitude_(latitude),
-		longitude_(longitude),
-		altitude_(altitude)
+
+void GeodeticCoords::fromENU_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
 {
+	fromGeocentric_WGS84(ENU_WGS84ToGeocentric_WGS84(enu, origin));
+}
 
+cv::Point3d GeodeticCoords::ENU_WGS84ToGeocentric_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
+{
+	// Generate reference 3D point:
+	cv::Point3f originGeocentric;
+	originGeocentric = origin.toGeocentric_WGS84();
+
+	cv::Vec3d P_ref(originGeocentric.x, originGeocentric.y, originGeocentric.z);
+
+	// Z axis -> In direction out-ward the center of the Earth:
+	cv::Vec3d REF_X, REF_Y, REF_Z;
+	REF_Z = cv::normalize(P_ref);
+
+	// 1st column: Starting at the reference point, move in the tangent
+	// direction
+	//   east-ward: I compute this as the derivative of P_ref wrt "longitude":
+	//      A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon);  --> -Z[1]
+	//      A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon);  -->  Z[0]
+	//      A_east[2] = 0;                                       -->  0
+	// ---------------------------------------------------------------------------
+	cv::Vec3d AUX_X(-REF_Z[1], REF_Z[0], 0);
+	REF_X = cv::normalize(AUX_X);
+
+	// 2nd column: The cross product:
+	REF_Y = REF_Z.cross(REF_X);
+
+	cv::Point3d out_coords;
+	out_coords.x =
+		REF_X[0] * enu.x + REF_Y[0] * enu.y + REF_Z[0] * enu.z + originGeocentric.x;
+	out_coords.y =
+		REF_X[1] * enu.x + REF_Y[1] * enu.y + REF_Z[1] * enu.z + originGeocentric.y;
+	out_coords.z =
+		REF_X[2] * enu.x + REF_Y[2] * enu.y + REF_Z[2] * enu.z + originGeocentric.z;
+
+	return out_coords;
 }
 
 }

corelib/src/Graph.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_registration.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/common.h>
@@ -160,10 +161,10 @@ bool exportPoses(
 
 bool importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAC
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
-		std::map<int, double> * stamps) // optional for format 1
+		std::map<int, double> * stamps) // optional for format 1 and 9
 {
 	UDEBUG("%s format=%d", filePath.c_str(), format);
 	if(format==3) // TORO
@@ -201,12 +202,50 @@ bool importPoses(
 			std::string str;
 			std::getline(file, str);
 
+			if(str.size() && str.at(str.size()-1) == '\r')
+			{
+				str = str.substr(0, str.size()-1);
+			}
+
 			if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
 			{
 				continue;
 			}
 
-			if(format == 8) // Karlsruhe format
+			if(format == 9) // EuRoC format
+			{
+				std::list<std::string> strList = uSplit(str, ',');
+				if(strList.size() ==  17)
+				{
+					double stamp = uStr2Double(strList.front())/1000000000.0;
+					strList.pop_front();
+					std::vector<std::string> v = uListToVector(strList);
+					Transform pose(uStr2Float(v[0]), uStr2Float(v[1]), uStr2Float(v[2]), // x y z
+							uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[3])); // qw qx qy qz -> qx qy qz qw
+					if(pose.isNull())
+					{
+						UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
+					}
+					else
+					{
+						if(stamps)
+						{
+							stamps->insert(std::make_pair(id, stamp));
+						}
+						// we need to rotate from IMU frame to world frame
+						Transform t( 0, 0, 1, 0,
+								   0, -1, 0, 0,
+								   1, 0, 0, 0);
+						pose = pose * t;
+						poses.insert(std::make_pair(id, pose));
+					}
+				}
+				else
+				{
+					UERROR("Error parsing \"%s\" with EuRoC MAV format (should have 17 values: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az)", str.c_str());
+				}
+			}
+			else if(format == 8) // Karlsruhe format
 			{
 				std::vector<std::string> strList = uListToVector(uSplit(str));
 				if(strList.size() ==  10)
@@ -363,7 +402,7 @@ bool importPoses(
 				}
 				else
 				{
-					UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y)", str.c_str());
+					UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
 				}
 			}
 			else if(format == 1) // rgbd-slam format
@@ -425,6 +464,105 @@ bool importPoses(
 	return false;
 }
 
+bool exportGPS(
+		const std::string & filePath,
+		const std::map<int, GPS> & gpsValues,
+		unsigned int rgba)
+{
+	UDEBUG("%s", filePath.c_str());
+	std::string tmpPath = filePath;
+
+	std::string ext = UFile::getExtension(filePath);
+
+	if(ext.compare("kml")!=0 && ext.compare("txt")!=0)
+	{
+		UERROR("Only txt and kml formats are supported!");
+		return false;
+	}
+
+	FILE* fout = 0;
+#ifdef _MSC_VER
+	fopen_s(&fout, tmpPath.c_str(), "w");
+#else
+	fout = fopen(tmpPath.c_str(), "w");
+#endif
+	if(fout)
+	{
+		if(ext.compare("kml")==0)
+		{
+			std::string values;
+			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
+			{
+				values += uFormat("%f,%f,%f ", iter->second.longitude(), iter->second.latitude(), iter->second.altitude());
+			}
+
+			// switch argb (Qt format) -> abgr
+			unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF);
+
+			std::string colorHexa = uFormat("%08x", abgr);
+
+			fprintf(fout, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
+			fprintf(fout, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
+			fprintf(fout, "<Document>\n"
+						  "	<name>%s</name>\n", tmpPath.c_str());
+			fprintf(fout, "	<StyleMap id=\"msn_ylw-pushpin\">\n"
+						  "		<Pair>\n"
+						  "			<key>normal</key>\n"
+						  "			<styleUrl>#sn_ylw-pushpin</styleUrl>\n"
+						  "		</Pair>\n"
+						  "		<Pair>\n"
+						  "			<key>highlight</key>\n"
+						  "			<styleUrl>#sh_ylw-pushpin</styleUrl>\n"
+						  "		</Pair>\n"
+						  "	</StyleMap>\n"
+						  "	<Style id=\"sh_ylw-pushpin\">\n"
+						  "		<IconStyle>\n"
+						  "			<scale>1.2</scale>\n"
+						  "		</IconStyle>\n"
+						  "		<LineStyle>\n"
+						  "			<color>%s</color>\n"
+						  "		</LineStyle>\n"
+						  "	</Style>\n"
+						  "	<Style id=\"sn_ylw-pushpin\">\n"
+						  "		<LineStyle>\n"
+						  "			<color>%s</color>\n"
+						  "		</LineStyle>\n"
+						  "	</Style>\n", colorHexa.c_str(), colorHexa.c_str());
+			fprintf(fout, "	<Placemark>\n"
+						  "		<name>%s</name>\n"
+						  "		<styleUrl>#msn_ylw-pushpin</styleUrl>"
+						  "		<LineString>\n"
+						  "			<coordinates>\n"
+						  "				%s\n"
+						  "			</coordinates>\n"
+						  "		</LineString>\n"
+						  "	</Placemark>\n"
+						  "</Document>\n"
+						  "</kml>\n",
+						  uSplit(tmpPath, '.').front().c_str(),
+						  values.c_str());
+		}
+		else
+		{
+			fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
+			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
+			{
+				fprintf(fout, "%f %f %f %f %f %f\n",
+						iter->second.stamp(),
+						iter->second.longitude(),
+						iter->second.latitude(),
+						iter->second.altitude(),
+						iter->second.error(),
+						iter->second.bearing());
+			}
+		}
+
+		fclose(fout);
+		return true;
+	}
+	return false;
+}
+
 // KITTI evaluation
 float lengths[] = {100,200,300,400,500,600,700,800};
 int32_t num_lengths = 8;
@@ -543,6 +681,143 @@ void calcKittiSequenceErrors (
 }
 // KITTI evaluation end
 
+Transform calcRMSE (
+		const std::map<int, Transform> & groundTruth,
+		const std::map<int, Transform> & poses,
+		float & translational_rmse,
+		float & translational_mean,
+		float & translational_median,
+		float & translational_std,
+		float & translational_min,
+		float & translational_max,
+		float & rotational_rmse,
+		float & rotational_mean,
+		float & rotational_median,
+		float & rotational_std,
+		float & rotational_min,
+		float & rotational_max)
+{
+
+	translational_rmse = 0.0f;
+	translational_mean = 0.0f;
+	translational_median = 0.0f;
+	translational_std = 0.0f;
+	translational_min = 0.0f;
+	translational_max = 0.0f;
+
+	rotational_rmse = 0.0f;
+	rotational_mean = 0.0f;
+	rotational_median = 0.0f;
+	rotational_std = 0.0f;
+	rotational_min = 0.0f;
+	rotational_max = 0.0f;
+
+	//align with ground truth for more meaningful results
+	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+	cloud1.resize(poses.size());
+	cloud2.resize(poses.size());
+	int oi = 0;
+	int idFirst = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jter=groundTruth.find(iter->first);
+		if(jter != groundTruth.end())
+		{
+			if(oi==0)
+			{
+				idFirst = iter->first;
+			}
+			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
+			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		}
+	}
+
+	Transform t = Transform::getIdentity();
+	if(oi>5)
+	{
+		cloud1.resize(oi);
+		cloud2.resize(oi);
+
+		t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+	}
+	else if(idFirst)
+	{
+		t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
+	}
+
+	std::vector<float> translationalErrors(poses.size());
+	std::vector<float> rotationalErrors(poses.size());
+	float sumTranslationalErrors = 0.0f;
+	float sumRotationalErrors = 0.0f;
+	float sumSqrdTranslationalErrors = 0.0f;
+	float sumSqrdRotationalErrors = 0.0f;
+	float radToDegree = 180.0f / M_PI;
+	oi=0;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
+		if(jter!=groundTruth.end())
+		{
+			Transform pose = t * iter->second;
+			Eigen::Vector3f xAxis(1,0,0);
+			Eigen::Vector3f vA = pose.toEigen3f().linear()*xAxis;
+			Eigen::Vector3f vB = jter->second.toEigen3f().linear()*xAxis;
+			double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+			rotationalErrors[oi] = a*radToDegree;
+			translationalErrors[oi] = pose.getDistance(jter->second);
+
+			sumTranslationalErrors+=translationalErrors[oi];
+			sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
+			sumRotationalErrors+=rotationalErrors[oi];
+			sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
+
+			if(oi == 0)
+			{
+				translational_min = translational_max = translationalErrors[oi];
+				rotational_min = rotational_max = rotationalErrors[oi];
+			}
+			else
+			{
+				if(translationalErrors[oi] < translational_min)
+				{
+					translational_min = translationalErrors[oi];
+				}
+				else if(translationalErrors[oi] > translational_max)
+				{
+					translational_max = translationalErrors[oi];
+				}
+
+				if(rotationalErrors[oi] < rotational_min)
+				{
+					rotational_min = rotationalErrors[oi];
+				}
+				else if(rotationalErrors[oi] > rotational_max)
+				{
+					rotational_max = rotationalErrors[oi];
+				}
+			}
+
+			++oi;
+		}
+	}
+	translationalErrors.resize(oi);
+	rotationalErrors.resize(oi);
+	if(oi)
+	{
+		float total = float(oi);
+		translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
+		translational_mean = sumTranslationalErrors/total;
+		translational_median = translationalErrors[oi/2];
+		translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
+
+		rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
+		rotational_mean = sumRotationalErrors/total;
+		rotational_median = rotationalErrors[oi/2];
+		rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
+	}
+	return t;
+}
+
 
 ////////////////////////////////////////////
 // Graph utilities
@@ -674,6 +949,20 @@ std::multimap<int, int>::const_iterator findLink(
 	return links.end();
 }
 
+std::multimap<int, Link> filterDuplicateLinks(
+		const std::multimap<int, Link> & links)
+{
+	std::multimap<int, Link> output;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(graph::findLink(output, iter->second.from(), iter->second.to(), true) == output.end())
+		{
+			output.insert(*iter);
+		}
+	}
+	return output;
+}
+
 std::multimap<int, Link> filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType)
@@ -789,7 +1078,7 @@ std::map<int, Transform> radiusPosesFiltering(
 
 				std::set<int> cloudIndices;
 				const Transform & currentT = transforms.at(i);
-				Eigen::Vector3f vA = currentT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+				Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 				for(unsigned int j=0; j<kIndices.size(); ++j)
 				{
 					if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
@@ -798,7 +1087,7 @@ std::map<int, Transform> radiusPosesFiltering(
 						{
 							const Transform & checkT = transforms.at(kIndices[j]);
 							// same orientation?
-							Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+							Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 							double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 							if(a <= angle)
 							{
@@ -894,7 +1183,7 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
 
 			std::set<int> cloudIndices;
 			const Transform & currentT = transforms.at(i);
-			Eigen::Vector3f vA = currentT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+			Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 			for(unsigned int j=0; j<kIndices.size(); ++j)
 			{
 				if((int)i != kIndices[j])
@@ -903,7 +1192,7 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
 					{
 						const Transform & checkT = transforms.at(kIndices[j]);
 						// same orientation?
-						Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+						Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 						if(a <= angle)
 						{
@@ -1545,7 +1834,7 @@ std::list<std::pair<int, Transform> > computePath(
 					{
 						//Transform nextPose = iter->second;
 						//Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-previousIter->second.x(), nextPose.y()-previousIter->second.y(), nextPose.z()-previousIter->second.z(), 1.0f);
-						//Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
+						//Eigen::Vector4f v2 = nextPose.linear().toEigen4f()*Eigen::Vector4f(1,0,0,1);
 						//float angle = pcl::getAngle3D(v1, v2);
 						//float cost = angle ;
 						//UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost);
@@ -1583,6 +1872,20 @@ int findNearestNode(
 		const rtabmap::Transform & targetPose)
 {
 	int id = 0;
+	std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	if(nearestNodes.size())
+	{
+		id = nearestNodes[0];
+	}
+	return id;
+}
+
+std::vector<int> findNearestNodes(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose,
+		int k)
+{
+	std::vector<int> nearestIds;
 	if(nodes.size() && !targetPose.isNull())
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -1600,14 +1903,15 @@ int findNearestNode(
 		std::vector<int> ind;
 		std::vector<float> dist;
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->nearestKSearch(pt, 1, ind, dist);
-		if(ind.size() && dist.size() && ind[0] >= 0)
+		kdTree->nearestKSearch(pt, k, ind, dist);
+
+		nearestIds.resize(ind.size());
+		for(unsigned int i=0; i<ind.size(); ++i)
 		{
-			UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
-			id = ids[ind[0]];
+			nearestIds[i] = ids[ind[i]];
 		}
 	}
-	return id;
+	return nearestIds;
 }
 
 // return <id, sqrd distance>, excluding query
@@ -1704,7 +2008,7 @@ std::map<int, Transform> getPosesInRadius(
 		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
 
-		Eigen::Vector3f vA = fromT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+		Eigen::Vector3f vA = fromT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
@@ -1714,7 +2018,7 @@ std::map<int, Transform> getPosesInRadius(
 				{
 					const Transform & checkT = nodes.at(ids[ind[i]]);
 					// same orientation?
-					Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 					if(a <= angle)
 					{

corelib/src/IMUThread.cpp

@@ -0,0 +1,154 @@
+/*
+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/IMUThread.h"
+#include "rtabmap/core/IMU.h"
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap
+{
+
+IMUThread::IMUThread(int rate, const Transform & localTransform) :
+		rate_(rate),
+		localTransform_(localTransform),
+		captureDelay_(0.0),
+		previousStamp_(0.0)
+{
+}
+
+IMUThread::~IMUThread()
+{
+	imuFile_.close();
+}
+
+bool IMUThread::init(const std::string & path)
+{
+	imuFile_.close();
+	captureDelay_ = 0.0;
+	previousStamp_ = 0.0;
+
+	// open the IMU file
+	std::string line;
+	imuFile_.open(path.c_str());
+	if (!imuFile_.good()) {
+		UERROR("no imu file found at %s",path.c_str());
+		return false;
+	}
+	int number_of_lines = 0;
+	while (std::getline(imuFile_, line))
+		++number_of_lines;
+	printf("No. IMU measurements: %d\n", number_of_lines-1);
+	if (number_of_lines - 1 <= 0) {
+		UERROR("no imu messages present in %s", path.c_str());
+		return false;
+	}
+	// set reading position to second line
+	imuFile_.clear();
+	imuFile_.seekg(0, std::ios::beg);
+	std::getline(imuFile_, line);
+
+	return true;
+}
+
+void IMUThread::setRate(int rate)
+{
+	rate_ = rate;
+}
+
+void IMUThread::mainLoopBegin()
+{
+	ULogger::registerCurrentThread("IMU");
+	frameRateTimer_.start();
+}
+
+void IMUThread::mainLoop()
+{
+	UTimer totalTime;
+	UDEBUG("");
+
+	if(rate_>0 || captureDelay_)
+	{
+		double delay = rate_>0?1000.0/double(rate_):1000.0f*captureDelay_;
+		int sleepTime = delay - 1000.0f*frameRateTimer_.getElapsedTime();
+		if(sleepTime > 2)
+		{
+			uSleep(sleepTime-2);
+		}
+
+		// Add precision at the cost of a small overhead
+		delay/=1000.0;
+		while(frameRateTimer_.getElapsedTime() < delay-0.000001)
+		{
+			//
+		}
+
+		frameRateTimer_.start();
+	}
+	captureDelay_ = 0.0;
+
+	std::string line;
+	if (std::getline(imuFile_, line))
+	{
+		std::stringstream stream(line);
+		std::string s;
+		std::getline(stream, s, ',');
+		std::string nanoseconds = s.substr(s.size() - 9, 9);
+		std::string seconds = s.substr(0, s.size() - 9);
+
+		cv::Vec3d gyr;
+		for (int j = 0; j < 3; ++j) {
+			std::getline(stream, s, ',');
+			gyr[j] = uStr2Double(s);
+		}
+
+		cv::Vec3d acc;
+		for (int j = 0; j < 3; ++j) {
+			std::getline(stream, s, ',');
+			acc[j] = uStr2Double(s);
+		}
+
+		double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
+		if(previousStamp_>0 && stamp > previousStamp_)
+		{
+			captureDelay_ = stamp - previousStamp_;
+		}
+		previousStamp_ = stamp;
+
+		IMU imu(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), localTransform_);
+		this->post(new IMUEvent(imu, stamp));
+	}
+	else if(!this->isKilled())
+	{
+		UWARN("no more imu data...");
+		this->kill();
+		this->post(new IMUEvent());
+	}
+}
+
+} // namespace rtabmap

corelib/src/LaserScan.cpp

@@ -0,0 +1,153 @@
+/*
+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/LaserScan.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap {
+
+int LaserScan::channels(Format format)
+{
+	int channels=0;
+	switch (format) {
+		case kXY:
+			channels = 2;
+			break;
+		case kXYZ:
+		case kXYI:
+			channels = 3;
+			break;
+		case kXYZI:
+		case kXYZRGB:
+			channels = 4;
+			break;
+		case kXYNormal:
+			channels = 5;
+			break;
+		case kXYZNormal:
+		case kXYINormal:
+			channels = 6;
+			break;
+		case kXYZINormal:
+		case kXYZRGBNormal:
+			channels = 7;
+			break;
+		default:
+			UFATAL("Unhandled type %d!", (int)format);
+			break;
+	}
+	return channels;
+}
+
+bool LaserScan::isScan2d(const Format & format)
+{
+	return format==kXY || format==kXYI || format == kXYNormal || format == kXYINormal;
+}
+bool LaserScan::isScanHasNormals(const Format & format)
+{
+	return format==kXYZNormal || format==kXYZINormal || format==kXYZRGBNormal || format == kXYNormal || format == kXYINormal;
+}
+bool LaserScan::isScanHasRGB(const Format & format)
+{
+	return format==kXYZRGB || format==kXYZRGBNormal;
+}
+bool LaserScan::isScanHasIntensity(const Format & format)
+{
+	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
+}
+
+LaserScan LaserScan::backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints, int maxRange, const Transform & localTransform)
+{
+	if(!oldScanFormat.empty())
+	{
+		if(oldScanFormat.channels() == 2)
+		{
+			return LaserScan(oldScanFormat, maxPoints, maxRange, kXY, localTransform);
+		}
+		else if(oldScanFormat.channels() == 3)
+		{
+			return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZ, localTransform);
+		}
+		else if(oldScanFormat.channels() == 4)
+		{
+			return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGB, localTransform);
+		}
+		else if(oldScanFormat.channels() == 5)
+		{
+			return LaserScan(oldScanFormat, maxPoints, maxRange, kXYNormal, localTransform);
+		}
+		else if(oldScanFormat.channels() == 6)
+		{
+			return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZNormal, localTransform);
+		}
+		else if(oldScanFormat.channels() == 7)
+		{
+			return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGBNormal, localTransform);
+		}
+	}
+	return LaserScan();
+}
+
+LaserScan::LaserScan() :
+		maxPoints_(0),
+		maxRange_(0),
+		format_(kUnknown),
+		localTransform_(Transform::getIdentity())
+{
+}
+
+LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform) :
+	data_(data),
+	maxPoints_(maxPoints),
+	maxRange_(maxRange),
+	format_(format),
+	localTransform_(localTransform)
+{
+	UASSERT(data.empty() || data.rows == 1);
+	UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
+	UASSERT(!localTransform.isNull());
+
+	if(!data.empty() && !isCompressed())
+	{
+		if(format == kUnknown)
+		{
+			*this = backwardCompatibility(data_, maxPoints_, maxRange_, localTransform_);
+		}
+		else // verify that format corresponds to expected number of channels
+		{
+			UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%d", format).c_str());
+		}
+	}
+}
+
+}

corelib/src/Link.cpp

@@ -65,13 +65,13 @@ Link::Link(int from,
 
 double Link::rotVariance() const
 {
-	double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
+	double min = uMax3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
 	UASSERT(min > 0.0);
 	return 1.0/min;
 }
 double Link::transVariance() const
 {
-	double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
+	double min = uMax3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
 	UASSERT(min > 0.0);
 	return 1.0/min;
 }
@@ -117,7 +117,8 @@ Link Link::merge(const Link & link, Type outputType) const
 			link.to(),
 			outputType,
 			transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
-			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.at<double>(0,0)<link.infMatrix().at<double>(0,0)?infMatrix_:link.infMatrix()));
+			//transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
 }
 
 Link Link::inverse() const

corelib/src/OccupancyGrid.cpp

@@ -32,6 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
 
+#ifdef RTABMAP_OCTOMAP
+#include <rtabmap/core/OctoMap.h>
+#endif
+
 #include <pcl/io/pcd_io.h>
 
 namespace rtabmap {
@@ -39,15 +43,16 @@ namespace rtabmap {
 OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	parameters_(parameters),
 	cloudDecimation_(Parameters::defaultGridDepthDecimation()),
-	cloudMaxDepth_(Parameters::defaultGridDepthMax()),
-	cloudMinDepth_(Parameters::defaultGridDepthMin()),
+	cloudMaxDepth_(Parameters::defaultGridRangeMax()),
+	cloudMinDepth_(Parameters::defaultGridRangeMin()),
 	//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
 	footprintLength_(Parameters::defaultGridFootprintLength()),
 	footprintWidth_(Parameters::defaultGridFootprintWidth()),
 	footprintHeight_(Parameters::defaultGridFootprintHeight()),
 	scanDecimation_(Parameters::defaultGridScanDecimation()),
 	cellSize_(Parameters::defaultGridCellSize()),
-	occupancyFromCloud_(Parameters::defaultGridFromDepth()),
+	preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
+	occupancyFromDepth_(Parameters::defaultGridFromDepth()),
 	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
 	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
 	normalKSearch_(Parameters::defaultGridNormalK()),
@@ -63,28 +68,32 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
-	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
-	projRayTracing_(Parameters::defaultGridProjRayTracing()),
+	rayTracing_(Parameters::defaultGridRayTracing()),
 	fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
 	minMapSize_(Parameters::defaultGridGlobalMinSize()),
 	erode_(Parameters::defaultGridGlobalEroded()),
 	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
+	updateError_(Parameters::defaultGridGlobalUpdateError()),
 	xMin_(0.0f),
-	yMin_(0.0f)
+	yMin_(0.0f),
+	cloudAssembling_(false),
+	assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZRGB>)
 {
 	this->parseParameters(parameters);
 }
 
 void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromCloud_);
+	Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromDepth_);
 	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
 	if(cloudDecimation_ == 0)
 	{
 		cloudDecimation_ = 1;
 	}
-	Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
-	Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
+	Parameters::parse(parameters, Parameters::kGridRangeMin(), cloudMinDepth_);
+	Parameters::parse(parameters, Parameters::kGridRangeMax(), cloudMaxDepth_);
 	Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
 	Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
 	Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
@@ -94,6 +103,8 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	{
 		this->setCellSize(cellSize);
 	}
+
+	Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_);
 	Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
 	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
 	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
@@ -113,12 +124,12 @@ 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::kGridProjRayTracing(), projRayTracing_);
+	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
 	Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
+	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 
 	UASSERT(minMapSize_ >= 0.0f);
 
@@ -184,6 +195,24 @@ 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_32FC3);
+		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());
@@ -198,53 +227,98 @@ void OccupancyGrid::setCellSize(float cellSize)
 	}
 }
 
+void OccupancyGrid::setCloudAssembling(bool enabled)
+{
+	cloudAssembling_ = enabled;
+	if(!cloudAssembling_)
+	{
+		assembledGround_->clear();
+		assembledObstacles_->clear();
+	}
+}
+
 void OccupancyGrid::createLocalMap(
 		const Signature & node,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
 		cv::Point3f & viewPoint) const
 {
-	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
-			node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
+	UDEBUG("scan format=%d, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
+			node.sensorData().laserScanRaw().isEmpty()?0:node.sensorData().laserScanRaw().format(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
 
-	if(node.sensorData().laserScanRaw().channels() == 2 && !occupancyFromCloud_)
+	if((node.sensorData().laserScanRaw().is2d()) && !occupancyFromDepth_)
 	{
 		UDEBUG("2D laser scan");
 		//2D
 		viewPoint = cv::Point3f(
-				node.sensorData().laserScanInfo().localTransform().x(),
-				node.sensorData().laserScanInfo().localTransform().y(),
-				node.sensorData().laserScanInfo().localTransform().z());
+				node.sensorData().laserScanRaw().localTransform().x(),
+				node.sensorData().laserScanRaw().localTransform().y(),
+				node.sensorData().laserScanRaw().localTransform().z());
 
+		LaserScan scan = node.sensorData().laserScanRaw();
+		if(cloudMinDepth_ > 0.0f)
+		{
+			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(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
+				util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
 				cv::Mat(),
 				viewPoint,
-				ground,
-				obstacles,
+				emptyCells,
+				obstacleCells,
 				cellSize_,
 				scan2dUnknownSpaceFilled_,
-				node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
+				maxRange);
+
+		UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
 	}
 	else
 	{
 		// 3D
-		pcl::IndicesPtr indices(new std::vector<int>);
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		if(!occupancyFromCloud_)
+		if(!occupancyFromDepth_)
 		{
-			UDEBUG("3D laser scan");
-			const Transform & t = node.sensorData().laserScanInfo().localTransform();
-			cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
-			cloud = util3d::laserScanToPointCloudRGB(
-					scan,
-					t);
+			if(!node.sensorData().laserScanRaw().isEmpty())
+			{
+				UDEBUG("3D laser scan");
+				const Transform & t = node.sensorData().laserScanRaw().localTransform();
+				LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
+#ifdef RTABMAP_OCTOMAP
+				// clipping will be done in OctoMap
+				float maxRange = grid3D_&&rayTracing_?0.0f:cloudMaxDepth_;
+#else
+				float maxRange = cloudMaxDepth_;
+#endif
+				if(cloudMinDepth_ > 0.0f || maxRange > 0.0f)
+				{
+					scan = util3d::rangeFiltering(scan, cloudMinDepth_, maxRange);
+				}
 
-			// update viewpoint
-			viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+				// update viewpoint
+				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+
+				UDEBUG("scan format=%d", scan.format());
+				createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+			}
+			else
+			{
+				UWARN("Cannot create local map, scan is empty (node=%d).", node.id());
+			}
 		}
 		else
 		{
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 			UDEBUG("Depth image : decimation=%d max=%f min=%f",
 					cloudDecimation_,
 					cloudMaxDepth_,
@@ -252,7 +326,12 @@ void OccupancyGrid::createLocalMap(
 			cloud = util3d::cloudRGBFromSensorData(
 					node.sensorData(),
 					cloudDecimation_,
+#ifdef RTABMAP_OCTOMAP
+					// clipping will be done in OctoMap
+					grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
+#else
 					cloudMaxDepth_,
+#endif
 					cloudMinDepth_,
 					indices.get(),
 					parameters_,
@@ -286,95 +365,185 @@ void OccupancyGrid::createLocalMap(
 				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 			}
+			createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f, LaserScan::kXYZRGB), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
 		}
+	}
+}
 
-		if(projMapFrame_)
+void OccupancyGrid::createLocalMap(
+		const LaserScan & scan,
+		const Transform & pose,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
+		cv::Point3f & viewPointInOut) const
+{
+	if(projMapFrame_)
+	{
+		//we should rotate viewPoint in /map frame
+		float roll, pitch, yaw;
+		pose.getEulerAngles(roll, pitch, yaw);
+		Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
+		viewPointInOut.x = viewpointRotated.x();
+		viewPointInOut.y = viewpointRotated.y();
+		viewPointInOut.z = viewpointRotated.z();
+	}
+
+	if(scan.size())
+	{
+		pcl::IndicesPtr groundIndices(new std::vector<int>);
+		pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+		cv::Mat groundCloud;
+		cv::Mat obstaclesCloud;
+
+		if(scan.hasRGB() && scan.hasNormals())
 		{
-			//we should rotate viewPoint in /map frame
-			float roll, pitch, yaw;
-			node.getPose().getEulerAngles(roll, pitch, yaw);
-			Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPoint.x, viewPoint.y, viewPoint.z, 0,0,0);
-			viewPoint.x = viewpointRotated.x();
-			viewPoint.y = viewpointRotated.y();
-			viewPoint.z = viewpointRotated.z();
-		}
-
-		if((cloud->is_dense && cloud->size()) ||
-			(!cloud->is_dense && indices->size()))
-		{
-			pcl::IndicesPtr groundIndices(new std::vector<int>);
-			pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
-			cloud = this->segmentCloud<pcl::PointXYZRGB>(
-					cloud,
-					indices,
-					node.getPose(),
-					viewPoint,
-					groundIndices,
-					obstaclesIndices);
-
-			if(!groundIndices->empty() || !obstaclesIndices->empty())
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGBNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
+		else if(scan.hasRGB())
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGB>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
+			{
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
+		else if(scan.hasNormals())
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
+			{
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZ>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
+			{
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices);
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices);
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
 
-				if(groundIndices->size())
+		if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
+		{
+			UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
+			if(groundIsObstacle_ && !groundCloud.empty())
+			{
+				if(obstaclesCloud.empty())
 				{
-					pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
-				}
-
-				if(obstaclesIndices->size())
-				{
-					pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-				}
-
-				if(grid3D_)
-				{
-					UDEBUG("");
-					if(groundIsObstacle_)
-					{
-						*obstaclesCloud += *groundCloud;
-						groundCloud->clear();
-					}
-
-					// transform back in base frame
-					float roll, pitch, yaw;
-					node.getPose().getEulerAngles(roll, pitch, yaw);
-					Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
-					ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
-					obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+					obstaclesCloud = groundCloud;
+					groundCloud = cv::Mat();
 				}
 				else
 				{
-					UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
-					// projection on the xy plane
-					util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
-							groundCloud,
-							obstaclesCloud,
-							ground,
-							obstacles,
-							cellSize_);
+					UASSERT(obstaclesCloud.type() == groundCloud.type());
+					cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type());
+					obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols)));
+					groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols)));
+				}
+			}
 
-					if(projRayTracing_)
+			// transform back in base frame
+			float roll, pitch, yaw;
+			pose.getEulerAngles(roll, pitch, yaw);
+			Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
+
+			if(rayTracing_)
+			{
+#ifdef RTABMAP_OCTOMAP
+				if(!groundCloud.empty() || !obstaclesCloud.empty())
+				{
+					//create local octomap
+					OctoMap octomap(cellSize_);
+					octomap.setMaxRange(cloudMaxDepth_);
+					octomap.addToCache(1, groundCloud, obstaclesCloud, cv::Mat(), cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
+					std::map<int, Transform> poses;
+					poses.insert(std::make_pair(1, Transform::getIdentity()));
+					octomap.update(poses);
+
+					pcl::IndicesPtr groundIndices(new std::vector<int>);
+					pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+					pcl::IndicesPtr emptyIndices(new std::vector<int>);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
+					UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
+					if(scan.hasRGB())
 					{
-						cv::Mat laserScan = obstacles;
-						cv::Mat laserScanNoHit = ground;
-						obstacles = cv::Mat();
-						ground = cv::Mat();
-						util3d::occupancy2DFromLaserScan(
-								laserScan,
-								laserScanNoHit,
-								viewPoint,
-								ground,
-								obstacles,
-								cellSize_,
-								false, // don't fill unknown space
-								0);
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv);
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv);
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv);
 					}
 				}
 			}
+			else
+#else
+					UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
+				}
+#endif
+			{
+				groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
+				obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
+			}
+
+		}
+		else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
+		{
+			cv::Mat laserScan = obstacleCells;
+			cv::Mat laserScanNoHit = groundCells;
+			obstacleCells = cv::Mat();
+			groundCells = cv::Mat();
+			util3d::occupancy2DFromLaserScan(
+					laserScan,
+					laserScanNoHit,
+					viewPointInOut,
+					emptyCells,
+					obstacleCells,
+					cellSize_,
+					false, // don't fill unknown space
+					cloudMaxDepth_);
 		}
 	}
-	UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
+	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
 }
 
 void OccupancyGrid::clear()
@@ -386,9 +555,11 @@ void OccupancyGrid::clear()
 	xMin_ = 0.0f;
 	yMin_ = 0.0f;
 	addedNodes_.clear();
+	assembledGround_->clear();
+	assembledObstacles_->clear();
 }
 
-const cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
+cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 {
 	xMin = xMin_;
 	yMin = yMin_;
@@ -402,10 +573,11 @@ const cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 void OccupancyGrid::addToCache(
 		int nodeId,
 		const cv::Mat & ground,
-		const cv::Mat & obstacles)
+		const cv::Mat & obstacles,
+		const cv::Mat & empty)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
 }
 
 void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
@@ -427,6 +599,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
 	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
 	std::map<int, Transform> transforms;
+	float updateErrorSqrd = updateError_*updateError_;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
@@ -436,7 +609,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 
 			UASSERT(!iter->second.isNull() && !jter->second.isNull());
 			Transform t = Transform::getIdentity();
-			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
 			{
 				t = jter->second * iter->second.inverse();
 				graphOptimized = true;
@@ -470,6 +643,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 		}
 	}
 
+	bool assembledGroundUpdated = false;
+	bool assembledObstaclesUpdated = false;
+	bool assembledEmptyCellsUpdated = false;
+
 	if(graphOptimized || graphChanged)
 	{
 		if(graphChanged)
@@ -481,6 +658,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 			UINFO("Graph optimized!");
 		}
 
+		if(cloudAssembling_)
+		{
+			assembledGround_->clear();
+			assembledObstacles_->clear();
+		}
+
 		if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells
 		{
 			// 1) recreate all local maps
@@ -489,27 +672,32 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 			std::map<int, std::pair<int, int> > tmpIndices;
 			for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
 			{
-				if(iter->second.first)
+				if(!uContains(cache_, iter->first) && transforms.find(iter->first) != transforms.end())
 				{
-					emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
+					if(iter->second.first)
+					{
+						emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
+					}
+					if(iter->second.second)
+					{
+						occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
+					}
+					tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
 				}
-				if(iter->second.second)
-				{
-					occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
-				}
-				tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
 			}
-			for(int y=1; y<map_.rows-1; ++y)
+			for(int y=0; y<map_.rows; ++y)
 			{
-				for(int x=1; x<map_.cols-1; ++x)
+				for(int x=0; x<map_.cols; ++x)
 				{
 					float * info = mapInfo_.ptr<float>(y,x);
 					int nodeId = (int)info[0];
 					if(nodeId > 0 && map_.at<char>(y,x) >= 0)
 					{
-						std::map<int, Transform>::iterator tter = transforms.find(nodeId);
-						if(tter != transforms.end() && !uContains(cache_, nodeId))
+						if(tmpIndices.find(nodeId)!=tmpIndices.end())
 						{
+							std::map<int, Transform>::iterator tter = transforms.find(nodeId);
+							UASSERT(tter != transforms.end());
+
 							cv::Point3f pt(info[1], info[2], 0.0f);
 							pt = util3d::transformPoint(pt, tter->second);
 
@@ -547,9 +735,26 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							}
 						}
 					}
+					else if(nodeId > 0)
+					{
+						UERROR("Cell referred b node %d is unknown!?", nodeId);
+					}
 				}
 			}
 
+			//verify if all cells were added
+			for(std::map<int, std::pair<int, int> >::iterator iter=tmpIndices.begin(); iter!=tmpIndices.end(); ++iter)
+			{
+				std::map<int, cv::Mat>::iterator jter = emptyLocalMaps.find(iter->first);
+				UASSERT_MSG((iter->second.first == 0 && (jter==emptyLocalMaps.end() || jter->second.empty())) ||
+						(iter->second.first != 0 && jter!=emptyLocalMaps.end() && jter->second.cols == iter->second.first),
+						uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
+				jter = occupiedLocalMaps.find(iter->first);
+				UASSERT_MSG((iter->second.second == 0 && (jter==occupiedLocalMaps.end() || jter->second.empty())) ||
+						(iter->second.second != 0 && jter!=occupiedLocalMaps.end() && jter->second.cols == iter->second.second),
+						uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
+			}
+
 			UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
 		}
 
@@ -575,25 +780,30 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	std::list<std::pair<int, Transform> > poses;
 	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
 	UDEBUG("Last id = %d", lastId);
-	if(lastId >= 0)
+
+	// add old poses that were not in the current map (they were just retrieved from LTM)
+	for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(0); iter!=posesIn.end(); ++iter)
 	{
-		for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter)
+		if(addedNodes_.find(iter->first) == addedNodes_.end())
+		{
+			UDEBUG("Pose %d not found in current added poses, it be added to map", iter->first);
+			poses.push_back(*iter);
+		}
+	}
+
+	// insert negative after
+	for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
+	{
+		if(iter->first < 0)
 		{
 			poses.push_back(*iter);
 		}
-		// insert negative after
-		for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
+		else
 		{
-			if(iter->first < 0)
-			{
-				poses.push_back(*iter);
-			}
-			else
-			{
-				break;
-			}
+			break;
 		}
 	}
+
 	for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
 		UASSERT(!iter->second.isNull());
@@ -626,22 +836,24 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 		{
 			if(uContains(cache_, iter->first))
 			{
-				const std::pair<cv::Mat, cv::Mat> & pair = cache_.at(iter->first);
+				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.cols)
+				if(pair.first.first.cols)
 				{
-					if(pair.first.rows > 1 && pair.first.cols == 1)
+					if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
 					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
 					}
-					cv::Mat ground(1, pair.first.cols, CV_32FC2);
+					cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
 					for(int i=0; i<ground.cols; ++i)
 					{
-						const float * vi = pair.first.ptr<float>(0,i);
+						const float * vi = pair.first.first.ptr<float>(0,i);
 						float * vo = ground.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.first.channels() > 2)
+						if(pair.first.first.channels() != 2 && pair.first.first.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -662,22 +874,70 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							maxY = vo[1];
 					}
 					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
+
+					if(cloudAssembling_)
+					{
+						*assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.first), iter->second, 0, 255, 0);
+						assembledGroundUpdated = true;
+					}
 				}
 
-				//obstacles
+				//empty
 				if(pair.second.cols)
 				{
 					if(pair.second.rows > 1 && pair.second.cols == 1)
 					{
 						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
 					}
-					cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
-					for(int i=0; i<obstacles.cols; ++i)
+					cv::Mat ground(1, pair.second.cols, CV_32FC2);
+					for(int i=0; i<ground.cols; ++i)
 					{
 						const float * vi = pair.second.ptr<float>(0,i);
+						float * vo = ground.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(pair.second.channels() != 2 && pair.second.channels() != 5)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						if(minX > vo[0])
+							minX = vo[0];
+						else if(maxX < vo[0])
+							maxX = vo[0];
+
+						if(minY > vo[1])
+							minY = vo[1];
+						else if(maxY < vo[1])
+							maxY = vo[1];
+					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
+
+					if(cloudAssembling_)
+					{
+						*assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.second), iter->second, 0, 255, 0);
+						assembledEmptyCellsUpdated = true;
+					}
+				}
+
+				//obstacles
+				if(pair.first.second.cols)
+				{
+					if(pair.first.second.rows > 1 && pair.first.second.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.second.rows, pair.first.second.cols);
+					}
+					cv::Mat obstacles(1, pair.first.second.cols, CV_32FC2);
+					for(int i=0; i<obstacles.cols; ++i)
+					{
+						const float * vi = pair.first.second.ptr<float>(0,i);
 						float * vo = obstacles.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.second.channels() > 2)
+						if(pair.first.second.channels() != 2 && pair.first.second.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -698,6 +958,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							maxY = vo[1];
 					}
 					uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
+
+					if(cloudAssembling_)
+					{
+						*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.second), iter->second, 255, 0, 0);
+						assembledObstaclesUpdated = true;
+					}
 				}
 			}
 		}
@@ -770,6 +1036,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
 					UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
 					UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
+					UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
+					UASSERT(deltaX>=0 && deltaY>=0);
 					map = cv::Mat::ones(newMapSize, CV_8S)*-1;
 					mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
 					map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
@@ -801,9 +1069,9 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					{
 						float * ptf = iter->second.ptr<float>(0,i);
 						cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
-								uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
-										kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
+						UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
+								uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
+										kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
 						char & value = map.at<char>(pt.y, pt.x);
 						if(value != -2 && (!incrementalGraphUpdate || value==-1))
 						{
@@ -911,9 +1179,9 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 					{
 						float * ptf = jter->second.ptr<float>(0,i);
 						cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
-						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
-									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
-											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
+						UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
+											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
 						char & value = map.at<char>(pt.y, pt.x);
 						if(value != -2)
 						{
@@ -1091,10 +1359,41 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 		}
 	}
 
-	if(!fullUpdate_)
+	if(cloudAssembling_)
+	{
+		if(assembledGroundUpdated && assembledGround_->size() > 1)
+		{
+			assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
+		}
+		if(assembledObstaclesUpdated && assembledGround_->size() > 1)
+		{
+			assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
+		}
+		if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
+		{
+			assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
+		}
+	}
+
+	if(!fullUpdate_ && !cloudAssembling_)
 	{
 		cache_.clear();
 	}
+	else
+	{
+		//clear only negative ids
+		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
+		{
+			if(iter->first < 0)
+			{
+				cache_.erase(iter++);
+			}
+			else
+			{
+				break;
+			}
+		}
+	}
 
 	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
 }

corelib/src/Odometry.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryFovis.h"
 #include "rtabmap/core/OdometryViso2.h"
 #include "rtabmap/core/OdometryDVO.h"
+#include "rtabmap/core/OdometryOkvis.h"
 #include "rtabmap/core/OdometryORBSLAM2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
@@ -39,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UProcessInfo.h"
 #include "rtabmap/core/ParticleFilter.h"
 #include "rtabmap/core/util2d.h"
 
@@ -75,6 +77,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeF2F:
 		odometry = new OdometryF2F(parameters);
 		break;
+	case Odometry::kTypeOkvis:
+		odometry = new OdometryOkvis(parameters);
+		break;
 	default:
 		odometry = new OdometryF2M(parameters);
 		type = Odometry::kTypeF2M;
@@ -99,10 +104,13 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
 		_imageDecimation(Parameters::defaultOdomImageDecimation()),
 		_alignWithGround(Parameters::defaultOdomAlignWithGround()),
+		_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
+		_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
-		distanceTravelled_(0)
+		distanceTravelled_(0),
+		framesProcessed_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 
@@ -124,6 +132,9 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
 	Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
 	Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
+	Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
+	Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
+
 	if(_imageDecimation == 0)
 	{
 		_imageDecimation = 1;
@@ -168,6 +179,7 @@ void Odometry::reset(const Transform & initialPose)
 	_resetCurrentCount = 0;
 	previousStamp_ = 0;
 	distanceTravelled_ = 0;
+	framesProcessed_ = 0;
 	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
@@ -221,6 +233,13 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 {
 	UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
 
+	if(!_imagesAlreadyRectified && !this->canProcessRawImages())
+	{
+		UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
+				"are rectified, and set %s parameter back to true.",
+				Parameters::kRtabmapImagesAlreadyRectified().c_str());
+	}
+
 	// Ground alignment
 	if(_pose.isIdentity() && _alignWithGround)
 	{
@@ -233,11 +252,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			UTimer alignTimer;
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pcl::IndicesPtr ground, obstacles;
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
-			cloud = util3d::voxelize(cloud, indices, 0.01);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 10, 0, indices.get());
 			bool success = false;
-			if(cloud->size())
+			if(indices->size())
 			{
+				cloud = util3d::voxelize(cloud, indices, 0.01);
 				util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
 				if(ground->size())
 				{
@@ -284,7 +303,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
-	double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
+	// KITTI datasets start with stamp=0
+	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
 	Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
 	if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
 	{
@@ -330,7 +350,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 
 	UTimer time;
 	Transform t;
-	if(_imageDecimation > 1)
+	if(_imageDecimation > 1 && !data.imageRaw().empty())
 	{
 		// Decimation of images with calibrations
 		SensorData decimatedData = data;
@@ -395,6 +415,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		info->stamp = data.stamp();
 		info->interval = dt;
 		info->transform = t;
+		if(_publishRAMUsage)
+		{
+			info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);
+		}
 
 		if(!data.groundTruth().isNull())
 		{
@@ -526,7 +550,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 
-		if(data.stamp() == 0)
+		if(data.stamp() == 0 && framesProcessed_ != 0)
 		{
 			UWARN("Null stamp detected");
 		}
@@ -544,6 +568,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			distanceTravelled_ += t.getNorm();
 			info->distanceTravelled = distanceTravelled_;
 		}
+		++framesProcessed_;
 
 		return _pose *= t; // update
 	}