OccupancyGrid: updated parameters check

Depth calibration: using appropriate bin_width and bin_height for all cameras

.gitignore

@@ -1,6 +1,8 @@
 /lib
 .DS_Store
 .settings/language.settings.xml
+.idea/
+cmake-build-debug/
 app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml

.travis.yml

@@ -1,6 +1,7 @@
 sudo: true
 dist: trusty
 language: cpp
+group: deprecated-2017Q3
 
 compiler:
   - gcc

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 12)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_MINOR_VERSION 13)
+SET(RTABMAP_PATCH_VERSION 2)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -46,25 +46,25 @@ IF(NOT ANDROID)
 ENDIF(NOT ANDROID)
 
 IF(WIN32 AND NOT MINGW)
-  	ADD_DEFINITIONS("-DNOMINMAX")
+    ADD_DEFINITIONS("-DNOMINMAX")
     ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
 ELSE ()
-	ADD_DEFINITIONS( "-Wall" )
-	ADD_DEFINITIONS("-Wno-unknown-pragmas")
+    ADD_DEFINITIONS( "-Wall" )
+    ADD_DEFINITIONS("-Wno-unknown-pragmas")
 ENDIF()
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 OLD)
+    cmake_policy(SET CMP0020 OLD)
 endif()
 if(POLICY CMP0043)
-	cmake_policy(SET CMP0043 OLD)
+    cmake_policy(SET CMP0043 OLD)
 endif()
 
 IF(MINGW)
     # Hide the --enable-auto-import warning
-	SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--enable-auto-import")  
-	SET(CMAKE_MODULE_LINKER_FLAGS "-Wl,--enable-auto-import") 
-	SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import") 
+    SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--enable-auto-import")
+    SET(CMAKE_MODULE_LINKER_FLAGS "-Wl,--enable-auto-import")
+    SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import")
 ENDIF(MINGW)
 
 # GCC 4 required
@@ -124,8 +124,17 @@ set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
   "Installation directory for CMake files")
   
 ####### BUILD OPTIONS #######
+
+# ANDROID_PREBUILD (early exit if true)
+OPTION( ANDROID_PREBUILD "Set to ON to build rtabmap resource build tool (required for android build)" OFF )
+IF(ANDROID_PREBUILD)
+   MESSAGE("Option ANDROID_PREBUILD is set, only rtabmap resource tool will be built. You can use android toolchain after that.")
+   ADD_SUBDIRECTORY( utilite )
+   return()
+ENDIF(ANDROID_PREBUILD)
+
 IF(APPLE)
-	OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
 ENDIF(APPLE)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
@@ -149,16 +158,24 @@ option(WITH_CVSBA         "Include cvsba 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_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(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
-# fix libproj.so not found on Xenial
 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)
@@ -199,12 +216,12 @@ IF(WITH_QT)
     IF("${VTK_MAJOR_VERSION}" GREATER 5)
         FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
         IF(Qt5_FOUND)
-             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
         ENDIF(Qt5_FOUND)
     ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
 
     IF(NOT Qt5_FOUND)
-        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
     ENDIF(NOT Qt5_FOUND)
         
     IF(QT4_FOUND OR Qt5_FOUND)
@@ -230,18 +247,18 @@ ENDIF(WITH_FREENECT)
 IF(WITH_FREENECT2)
     FIND_PACKAGE(freenect2 QUIET)
     IF(freenect2_FOUND)
-	   IF(NOT freenect2_INCLUDE_DIRS)
-	      SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
-	   ENDIF(NOT freenect2_INCLUDE_DIRS)
+       IF(NOT freenect2_INCLUDE_DIRS)
+          SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
+       ENDIF(NOT freenect2_INCLUDE_DIRS)
        MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}")
        # Explicitly link to OpenCL (in case of CUDA installed)
-	   FIND_PACKAGE(OpenCL QUIET)
-	   IF(OpenCL_FOUND)
-		  SET(freenect2_LIBRARIES
-			${OpenCL_LIBRARIES}
-			${freenect2_LIBRARIES}
-		  )
-	   ENDIF(OpenCL_FOUND)
+       FIND_PACKAGE(OpenCL QUIET)
+       IF(OpenCL_FOUND)
+          SET(freenect2_LIBRARIES
+            ${OpenCL_LIBRARIES}
+            ${freenect2_LIBRARIES}
+          )
+       ENDIF(OpenCL_FOUND)
     ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)
 
@@ -279,70 +296,122 @@ IF(WITH_FLYCAPTURE2)
 ENDIF(WITH_FLYCAPTURE2)
 
 IF(WITH_CVSBA)
-	FIND_PACKAGE(cvsba QUIET)
+    FIND_PACKAGE(cvsba QUIET)
     IF(cvsba_FOUND)
        MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
     ENDIF(cvsba_FOUND)
 ENDIF(WITH_CVSBA)
 
 IF(WITH_ZED)
-	IF(WIN32) # Windows
-		SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
-		if (CMAKE_CL_64) # 64 bits
-			SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
-		else(CMAKE_CL_64) # 32 bits
-			message("32bits compilation is no more available with CUDA7.0")
-		endif(CMAKE_CL_64)
-		SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
-		IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
-			SET(ZED_FOUND TRUE)
-			LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
-		ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
-	ELSE() # Linux
-		find_package(ZED 1 QUIET)
-	ENDIF(WIN32)
+    IF(WIN32) # Windows
+        SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
+        if (CMAKE_CL_64) # 64 bits
+            SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
+        else(CMAKE_CL_64) # 32 bits
+            message("32bits compilation is no more available with CUDA7.0")
+        endif(CMAKE_CL_64)
+        SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
+        IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
+            SET(ZED_FOUND TRUE)
+            LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
+        ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
+    ELSE() # Linux
+        find_package(ZED 1 QUIET)
+    ENDIF(WIN32)
     
-	IF(ZED_FOUND)
+    IF(ZED_FOUND)
        MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}")
-	   ## look for CUDA
-	   find_package(CUDA)
-	   IF(CUDA_FOUND)
-		MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
-	   ELSE()
-	    MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
-	   ENDIF()
+       ## look for CUDA
+       find_package(CUDA)
+       IF(CUDA_FOUND)
+        MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
+       ELSE()
+        MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
+       ENDIF()
     ENDIF(ZED_FOUND)
 ENDIF(WITH_ZED)
 
 IF(WITH_REALSENSE)
-	FIND_PACKAGE(RealSense QUIET)
+    IF(WITH_REALSENSE_SLAM)
+       FIND_PACKAGE(RealSense QUIET COMPONENTS slam)
+    ELSE()
+       FIND_PACKAGE(RealSense QUIET)
+    ENDIF()
     IF(RealSense_FOUND)
        MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}")
     ENDIF(RealSense_FOUND)
+    IF(RealSenseSlam_FOUND)
+       MESSAGE(STATUS "Found RealSenseSlam: ${RealSense_INCLUDE_DIRS}")
+    ENDIF(RealSenseSlam_FOUND)
 ENDIF(WITH_REALSENSE)
 
 IF(WITH_OCTOMAP)
-	FIND_PACKAGE(OCTOMAP QUIET)
+    FIND_PACKAGE(OCTOMAP QUIET)
     IF(OCTOMAP_FOUND)
        MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
     ENDIF(OCTOMAP_FOUND)
 ENDIF(WITH_OCTOMAP)
 
-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
+IF(WITH_CPUTSDF)
+    FIND_PACKAGE(CPUTSDF QUIET)
+    IF(CPUTSDF_FOUND)
+       MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}")
+    ENDIF(CPUTSDF_FOUND)
+ENDIF(WITH_CPUTSDF)
+
+IF(WITH_FOVIS)
+    FIND_PACKAGE(libfovis QUIET)
+    IF(libfovis_FOUND)
+       MESSAGE(STATUS "Found libfovis: ${libfovis_INCLUDE_DIRS}")
+    ENDIF(libfovis_FOUND)
+ENDIF(WITH_FOVIS)
+
+IF(WITH_VISO2)
+    FIND_PACKAGE(libviso2 QUIET)
+    IF(libviso2_FOUND)
+       MESSAGE(STATUS "Found libviso2: ${libviso2_INCLUDE_DIRS}")
+    ENDIF(libviso2_FOUND)
+ENDIF(WITH_VISO2)
+
+IF(WITH_DVO)
+    FIND_PACKAGE(dvo_core QUIET)
+    IF(dvo_core_FOUND)
+       MESSAGE(STATUS "Found dvo_core: ${dvo_core_INCLUDE_DIRS}")
+    ENDIF(dvo_core_FOUND)
+ENDIF(WITH_DVO)
+
+IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
+    FIND_PACKAGE(ORB_SLAM2 QUIET)
+    IF(ORB_SLAM2_FOUND)
+       MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
+       FIND_PACKAGE(Pangolin QUIET)
+       IF(NOT Pangolin_FOUND)
+          SET(ORB_SLAM2_FOUND FALSE)
+          MESSAGE(STATUS "Found ORB_SLAM2 but not Pangolin, disabling ORB_SLAM2.")
+       ELSE()
+          MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
+          SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
+          SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
+          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)
    #Newest versions require std11
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-	  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-	  IF(COMPILER_SUPPORTS_CXX11)
-		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-	  ELSEIF(COMPILER_SUPPORTS_CXX0X)
-		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-	  ELSE()
-		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
-	  ENDIF()
+      CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+      IF(COMPILER_SUPPORTS_CXX11)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+      ELSEIF(COMPILER_SUPPORTS_CXX0X)
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+      ELSE()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
+      ENDIF()
    ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -376,7 +445,7 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 
 # CONF_DEPENDENCIES contains only dependencies not required by the headers
 SET(CONF_DEPENDENCIES 
-	${ZLIB_LIBRARIES}
+    ${ZLIB_LIBRARIES}
 )
 IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
    SET(NONFREE "//")
@@ -437,11 +506,39 @@ IF(NOT RealSense_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) 
 ENDIF()
+IF(NOT RealSenseSlam_FOUND)
+   SET(REALSENSESLAM "//")
+ENDIF(NOT RealSenseSlam_FOUND)
 IF(NOT OCTOMAP_FOUND)
    SET(OCTOMAP "//")
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
 ENDIF()
+IF(NOT CPUTSDF_FOUND)
+   SET(CPUTSDF "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) 
+ENDIF()
+IF(NOT libfovis_FOUND)
+   SET(FOVIS "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libfovis_LIBRARIES}) 
+ENDIF()
+IF(NOT libviso2_FOUND)
+   SET(VISO2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${libviso2_LIBRARIES}) 
+ENDIF()
+IF(NOT dvo_core_FOUND)
+   SET(DVO "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${dvo_core_LIBRARIES}) 
+ENDIF()
+IF(NOT ORB_SLAM2_FOUND)
+   SET(ORB_SLAM2 "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ORB_SLAM2_LIBRARIES}) 
+ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
@@ -496,8 +593,8 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$
    
 # ... for the build tree
 set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
-					  "${PROJECT_SOURCE_DIR}/guilib/include" 
-					  "${PROJECT_SOURCE_DIR}/utilite/include")
+                      "${PROJECT_SOURCE_DIR}/guilib/include" 
+                      "${PROJECT_SOURCE_DIR}/utilite/include")
 set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
 IF(QT4_FOUND OR Qt5_FOUND)
    set(CONF_WITH_GUI ON)
@@ -622,8 +719,8 @@ MESSAGE(STATUS "  BUILD_APP =            ${BUILD_APP}")
 MESSAGE(STATUS "  BUILD_TOOLS =          ${BUILD_TOOLS}")
 MESSAGE(STATUS "  BUILD_EXAMPLES =       ${BUILD_EXAMPLES}")
 IF(NOT WIN32)
-	# see comment above for the BUILD_SHARED_LIBS option on Windows
-	MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
+    # see comment above for the BUILD_SHARED_LIBS option on Windows
+    MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
 ENDIF(NOT WIN32)
 IF(APPLE)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
@@ -708,11 +805,15 @@ ELSE()
 MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
 ENDIF()
 
+IF(G2O_FOUND OR GTSAM_FOUND)
 IF(WITH_VERTIGO)
 MESSAGE(STATUS "  With VERTIGO              = YES (License: GPLv3)")
 ELSE()
 MESSAGE(STATUS "  With VERTIGO              = NO (WITH_VERTIGO=OFF)")
 ENDIF()
+ELSE()
+MESSAGE(STATUS "  With VERTIGO              = NO (GTSAM or g2o required)")
+ENDIF()
 
 IF(cvsba_FOUND)
 MESSAGE(STATUS "  With cvsba                = YES (License: GPLv2)")
@@ -736,6 +837,13 @@ ENDIF()
 
 IF(RealSense_FOUND)
 MESSAGE(STATUS "  With RealSense            = YES (License: Apache-2)")
+IF(RealSenseSlam_FOUND)
+MESSAGE(STATUS "  With RealSenseSlam        = YES")
+ELSEIF(NOT WITH_REALSENSE)
+MESSAGE(STATUS "  With RealSenseSlam        = NO (librealsense_slam not found)")
+ELSE()
+MESSAGE(STATUS "  With RealSenseSlam        = NO (WITH_REALSENSE_SLAM=OFF)")
+ENDIF()
 ELSEIF(NOT WITH_REALSENSE)
 MESSAGE(STATUS "  With RealSense            = NO (WITH_REALSENSE=OFF)")
 ELSE()
@@ -750,6 +858,48 @@ ELSE()
 MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
 ENDIF()
 
+IF(CPUTSDF_FOUND)
+MESSAGE(STATUS "  With CPUTSDF              = YES (License: BSD)")
+ELSEIF(NOT WITH_CPUTSDF)
+MESSAGE(STATUS "  With CPUTSDF              = NO (WITH_CPUTSDF=OFF)")
+ELSE()
+MESSAGE(STATUS "  With CPUTSDF              = NO (CPUTSDF not found)")
+ENDIF()
+
+IF(libfovis_FOUND)
+MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
+ELSEIF(NOT WITH_FOVIS)
+MESSAGE(STATUS "  With libfovis             = NO (WITH_FOVIS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libfovis             = NO (libfovis not found)")
+ENDIF()
+
+IF(libviso2_FOUND)
+MESSAGE(STATUS "  With libviso2             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_VISO2)
+MESSAGE(STATUS "  With libviso2             = NO (WITH_VISO2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libviso2             = NO (libviso2 not found)")
+ENDIF()
+
+IF(dvo_core_FOUND)
+MESSAGE(STATUS "  With dvo_core             = YES (License: GPLv3)")
+ELSEIF(NOT WITH_DVO)
+MESSAGE(STATUS "  With dvo_core             = NO (WITH_DVO=OFF)")
+ELSE()
+MESSAGE(STATUS "  With dvo_core             = NO (dvo_core not found)")
+ENDIF()
+
+IF(ORB_SLAM2_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
+ELSEIF(NOT WITH_ORB_SLAM2)
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_ORB_SLAM2=OFF)")
+ELSEIF(G2O_FOUND)
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
+ELSE()
+MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)
@@ -768,3 +918,5 @@ MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of th
        GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
        set -DWITH_TORO=ON")
 ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
+
+# vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :

RTABMapConfig.cmake.in

@@ -1,4 +1,8 @@
 # - Config file for the RTABMap package
+# Components:
+#  core    (required)
+#  gui     (optional)
+#  utilite (required)
 # It defines the following variables
 #  RTABMap_INCLUDE_DIRS - include directories for RTABMap
 #  RTABMap_LIBRARIES    - libraries to link against
@@ -42,8 +46,18 @@ ENDIF()
 
 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
 
+list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
+set(WITH_GUI ON)
+if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
+   if (${_index} EQUAL -1)
+      set(WITH_GUI OFF)
+   endif()
+endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+
+
 #gui lib (OFF if RTAB-Map is not built with Qt)
-if(@CONF_WITH_GUI@)
+if(@CONF_WITH_GUI@ AND ${WITH_GUI})
    find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    
@@ -59,13 +73,15 @@ if(@CONF_WITH_GUI@)
    ENDIF()
    
    set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
-endif(@CONF_WITH_GUI@)
+elseif(${WITH_GUI})
+   MESSAGE(ERROR "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
+endif()
 
 # Dependencies
-if(@CONF_VTK_QT@)
+if(@CONF_VTK_QT@ AND ${WITH_GUI})
    find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
-endif(@CONF_VTK_QT@)
-set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
+endif(@CONF_VTK_QT@ AND ${WITH_GUI})
+SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
 
 #backward compatibilities
 set(RTABMAP_CORE ${RTABMap_CORE})
@@ -74,3 +90,7 @@ if(RTABMap_GUI)
    set(RTABMAP_GUI ${RTABMap_GUI})
    set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
 endif(RTABMap_GUI)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
+mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)

Version.h.in

@@ -51,7 +51,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
 @REALSENSE@#define RTABMAP_REALSENSE
+@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
 @OCTOMAP@#define RTABMAP_OCTOMAP
+@CPUTSDF@#define RTABMAP_CPUTSDF
+@FOVIS@#define RTABMAP_FOVIS
+@VISO2@#define RTABMAP_VISO2
+@DVO@#define RTABMAP_DVO
+@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="48"
+        android:versionCode="55"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -31,7 +31,8 @@
                 android:label="@string/app_name"
                 android:launchMode="singleTask"
 		android:screenOrientation="fullSensor"
-		android:configChanges="orientation|screenSize|keyboardHidden">
+		android:configChanges="orientation|screenSize|keyboardHidden"
+		android:theme="@style/ThemeApp">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
                     android:value="NativeRTABMap" />

app/android/info.txt.in

@@ -1,7 +1,7 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
 Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
-Copyright 2016<br>
+Copyright 2016-2017<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
 <b>http://introlab.github.io/rtabmap</b><br><br>
 

app/android/jni/CameraTango.cpp

@@ -104,14 +104,13 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 const float CameraTango::bilateralFilteringSigmaS = 2.0f;
 const float CameraTango::bilateralFilteringSigmaR = 0.075f;
 
-CameraTango::CameraTango(bool colorCamera, int decimation, bool autoExposure, bool publishRawScan, bool smoothing) :
+CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
 		Camera(0),
 		tango_config_(0),
-		firstFrame_(true),
+		previousStamp_(0.0),
 		stampEpochOffset_(0.0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),
-		autoExposure_(autoExposure),
 		rawScanPublished_(publishRawScan),
 		smoothing_(smoothing),
 		cloudStamp_(0),
@@ -190,7 +189,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
-	TangoSupport_initializeLibrary();
+	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
 
 	// Connect to Tango
 	LOGI("NativeRTABMap: Setup tango config");
@@ -219,31 +218,6 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
 			return false;
 		}
-		// disable auto exposure (disabled, seems broken on latest Tango releases)
-		ret = TangoConfig_setBool(tango_config_, "config_color_mode_auto", autoExposure_);
-		if (ret != TANGO_SUCCESS)
-		{
-			LOGE("NativeRTABMap: config_color_mode_auto() failed with error code: %d", ret);
-			//return false;
-		}
-		else
-		{
-			if(!autoExposure_)
-			{
-				ret = TangoConfig_setInt32(tango_config_, "config_color_iso", 800);
-				if (ret != TANGO_SUCCESS)
-				{
-					LOGE("NativeRTABMap: config_color_iso() failed with error code: %d", ret);
-					return false;
-				}
-			}
-			bool verifyAutoExposureState;
-			int32_t verifyIso, verifyExp;
-			TangoConfig_getBool( tango_config_, "config_color_mode_auto", &verifyAutoExposureState );
-			TangoConfig_getInt32( tango_config_, "config_color_iso", &verifyIso );
-			TangoConfig_getInt32( tango_config_, "config_color_exp", &verifyExp );
-			LOGI( "NativeRTABMap: config_color autoExposure=%s %d %d", verifyAutoExposureState?"On" : "Off", verifyIso, verifyExp );
-		}
 	}
 
 	// Enable depth.
@@ -382,6 +356,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}
 
+	LOGD("Calibration: fx=%f fy=%f cx=%f cy=%f width=%d height=%d",
+			color_camera_intrinsics.fx,
+			color_camera_intrinsics.fy,
+			color_camera_intrinsics.cx,
+			color_camera_intrinsics.cy,
+			color_camera_intrinsics.width,
+			color_camera_intrinsics.height);
+
 	cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
 	K.at<double>(0,0) = color_camera_intrinsics.fx;
 	K.at<double>(1,1) = color_camera_intrinsics.fy;
@@ -445,7 +427,8 @@ void CameraTango::close()
 		tango_config_ = nullptr;
 		TangoService_disconnect();
 	}
-	firstFrame_ = true;
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
 }
@@ -884,14 +867,23 @@ void CameraTango::mainLoop()
 			rtabmap::Transform pose = data.groundTruth();
 			data.setGroundTruth(Transform());
 			// convert stamp to epoch
-			if(firstFrame_)
+			bool firstFrame = previousPose_.isNull();
+			if(firstFrame)
 			{
 				stampEpochOffset_ = UTimer::now()-data.stamp();
 			}
 			data.setStamp(stampEpochOffset_ + data.stamp());
-			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.000001);
-			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.000001, firstFrame_?9999:0.000001));
-			firstFrame_ = false;
+			OdometryInfo info;
+			if(!firstFrame)
+			{
+				info.interval = data.stamp()-previousStamp_;
+				info.transform = previousPose_.inverse() * pose;
+			}
+			info.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.000001);
+			LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.000001);
+			this->post(new OdometryEvent(data, pose, info));
+			previousPose_ = pose;
+			previousStamp_ = data.stamp();
 		}
 		else if(!this->isKilled())
 		{

app/android/jni/CameraTango.h

@@ -75,7 +75,7 @@ public:
 	static const float bilateralFilteringSigmaR;
 
 public:
-	CameraTango(bool colorCamera, int decimation, bool autoExposure, bool publishRawScan, bool smoothing);
+	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -87,7 +87,6 @@ public:
 	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
-	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 
@@ -107,12 +106,12 @@ private:
 
 private:
 	void * tango_config_;
-	bool firstFrame_;
+	Transform previousPose_;
+	double previousStamp_;
 	UTimer cameraStartedTime_;
 	double stampEpochOffset_;
 	bool colorCamera_;
 	int decimation_;
-	bool autoExposure_;
 	bool rawScanPublished_;
 	bool smoothing_;
 	cv::Mat cloud_;

app/android/jni/ProgressionStatus.h

@@ -70,7 +70,7 @@ public:
 			increment();
 		}
 
-		return !isCanceled();
+		return ProgressState::callback(msg);
 	}
 	virtual ~ProgressionStatus(){}
 

app/android/jni/RTABMapApp.h

@@ -56,7 +56,7 @@ class RTABMapApp : public UEventsHandler {
 
   void setScreenRotation(int displayRotation, int cameraRotation);
 
-  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
 
   bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
@@ -114,19 +114,23 @@ class RTABMapApp : public UEventsHandler {
                     float x0, float y0, float x1, float y1);
 
   void setPausedMapping(bool paused);
+  void setOnlineBlending(bool enabled);
   void setMapCloudShown(bool shown);
   void setOdomCloudShown(bool shown);
   void setMeshRendering(bool enabled, bool withTexture);
   void setPointSize(float value);
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float value);
   void setLighting(bool enabled);
   void setBackfaceCulling(bool enabled);
+  void setWireframe(bool enabled);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
   void setNodesFiltering(bool enabled);
   void setGraphVisible(bool visible);
   void setGridVisible(bool visible);
-  void setAutoExposure(bool enabled);
   void setRawScanSaved(bool enabled);
   void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
@@ -141,18 +145,18 @@ class RTABMapApp : public UEventsHandler {
   void setClusterRatio(float value);
   void setMaxGainRadius(float value);
   void setRenderingTextureDecimation(int value);
+  void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
   void save(const std::string & databasePath);
-  cv::Mat mergeTextures(pcl::TextureMesh & mesh, int textureSize) const;
   void cancelProcessing();
   bool exportMesh(
-		  const std::string & filePath,
 		  float cloudVoxelSize,
 		  bool regenerateCloud,
 		  bool meshing,
 		  int textureSize,
+		  int textureCount,
 		  int normalK,
 		  bool optimized,
 		  float optimizedVoxelSize,
@@ -160,11 +164,12 @@ class RTABMapApp : public UEventsHandler {
 		  int optimizedMaxPolygons,
 		  float optimizedColorRadius,
 		  bool optimizedCleanWhitePolygons,
-		  bool optimizedColorWhitePolygons,
+		  int optimizedMinClusterSize,
 		  float optimizedMaxTextureDistance,
 		  int optimizedMinTextureClusterSize,
 		  bool blockRendering);
   bool postExportation(bool visualize);
+  bool writeExportedMesh(const std::string & directory, const std::string & name);
   int postProcessing(int approach);
 
  protected:
@@ -188,7 +193,6 @@ class RTABMapApp : public UEventsHandler {
   bool nodesFiltering_;
   bool localizationMode_;
   bool trajectoryMode_;
-  bool autoExposure_;
   bool rawScanSaved_;
   bool smoothing_;
   bool cameraColor_;
@@ -202,6 +206,7 @@ class RTABMapApp : public UEventsHandler {
   float clusterRatio_;
   float maxGainRadius_;
   int renderingTextureDecimation_;
+  float backgroundColor_;
 
   rtabmap::ParametersMap mappingParameters_;
 
@@ -214,6 +219,7 @@ class RTABMapApp : public UEventsHandler {
   bool filterPolygonsOnNextRender_;
   int gainCompensationOnNextRender_;
   bool bilateralFilteringOnNextRender_;
+  bool takeScreenshotOnNextRender_;
   bool cameraJustInitialized_;
   int meshDecimation_;
   int totalPoints_;
@@ -227,8 +233,8 @@ class RTABMapApp : public UEventsHandler {
 
   bool visualizingMesh_;
   bool exportedMeshUpdated_;
-  pcl::TextureMesh::Ptr exportedMesh_;
-  cv::Mat exportedTexture_;
+  pcl::TextureMesh::Ptr optMesh_;
+  cv::Mat optTexture_;
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
@@ -246,6 +252,8 @@ class RTABMapApp : public UEventsHandler {
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;
 
+	USemaphore screenshotReady_;
+
 	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
 

app/android/jni/jni_interface.cpp

@@ -71,6 +71,17 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	return app.openDatabase(databasePathC, databaseInMemory, optimize);
 }
 
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
+    JNIEnv* env, jobject, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+{
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	std::string databaseSourceC;
+	GetJStringContent(env,databaseSource,databaseSourceC);
+	return app.openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+}
+
 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
 		JNIEnv* env, jobject, jobject iBinder) {
@@ -127,6 +138,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setPausedMapping(
 	return app.setPausedMapping(paused);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOnlineBlending(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setOnlineBlending(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMapCloudShown(
 		JNIEnv*, jobject, bool shown)
 {
@@ -151,6 +168,24 @@ Java_com_introlab_rtabmap_RTABMapLib_setPointSize(
 	return app.setPointSize(value);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setFOV(
+		JNIEnv*, jobject, float fov)
+{
+	return app.setFOV(fov);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOrthoCropFactor(
+		JNIEnv*, jobject, float value)
+{
+	return app.setOrthoCropFactor(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridRotation(
+		JNIEnv*, jobject, float value)
+{
+	return app.setGridRotation(value);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLighting(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -163,6 +198,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setBackfaceCulling(
 	return app.setBackfaceCulling(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setWireframe(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setWireframe(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -199,12 +240,6 @@ Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
 	return app.setGridVisible(visible);
 }
 JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
-		JNIEnv*, jobject, bool enabled)
-{
-	return app.setAutoExposure(enabled);
-}
-JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setRawScanSaved(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -288,6 +323,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setRenderingTextureDecimation(
 {
 	return app.setRenderingTextureDecimation(value);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setBackgroundColor(
+		JNIEnv*, jobject, float value)
+{
+	return app.setBackgroundColor(value);
+}
 JNIEXPORT jint JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
     JNIEnv* env, jobject, jstring key, jstring value)
@@ -324,11 +365,11 @@ Java_com_introlab_rtabmap_RTABMapLib_cancelProcessing(
 JNIEXPORT bool JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		JNIEnv* env, jobject,
-		jstring filePath,
 		float cloudVoxelSize,
 		bool regenerateCloud,
 		bool meshing,
 		int textureSize,
+		int textureCount,
 		int normalK,
 		bool optimized,
 		float optimizedVoxelSize,
@@ -336,19 +377,17 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		int optimizedMaxPolygons,
 		float optimizedColorRadius,
 		bool optimizedCleanWhitePolygons,
-		bool optimizedColorWhitePolygons,
+		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
 		bool blockRendering)
 {
-	std::string filePathC;
-	GetJStringContent(env,filePath,filePathC);
 	return app.exportMesh(
-			filePathC,
 			cloudVoxelSize,
 			regenerateCloud,
 			meshing,
 			textureSize,
+			textureCount,
 			normalK,
 			optimized,
 			optimizedVoxelSize,
@@ -356,7 +395,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 			optimizedMaxPolygons,
 			optimizedColorRadius,
 			optimizedCleanWhitePolygons,
-			optimizedColorWhitePolygons,
+			optimizedMinClusterSize,
 			optimizedMaxTextureDistance,
 			optimizedMinTextureClusterSize,
 			blockRendering);
@@ -369,6 +408,18 @@ Java_com_introlab_rtabmap_RTABMapLib_postExportation(
 	return app.postExportation(visualize);
 }
 
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_writeExportedMesh(
+		JNIEnv* env, jobject, jstring directory, jstring name)
+{
+	std::string directoryC;
+	GetJStringContent(env,directory,directoryC);
+	std::string nameC;
+	GetJStringContent(env,name,nameC);
+	return app.writeExportedMesh(directoryC, nameC);
+}
+
+
 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
 		JNIEnv* env, jobject, int approach)

app/android/jni/point_cloud_drawable.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "point_cloud_drawable.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/imgproc/imgproc.hpp>
 #include "util.h"
@@ -39,9 +40,264 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define LOW_DEC 2
 #define LOWLOW_DEC 4
 
+enum PointCloudShaders
+{
+	kPointCloud = 0,
+	kPointCloudBlending = 1,
+	kPointCloudLighting = 2,
+	kPointCloudLightingBlending = 3,
+
+	kTexture = 4,
+	kTextureBlending = 5,
+	kTextureLighting = 6,
+	kTextureLightingBlending = 7,
+
+	kDepthPacking = 8
+};
+
+// PointCloud shaders
+const std::string kPointCloudVertexShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+    "attribute vec3 aVertex;\n"
+    "attribute vec3 aColor;\n"
+
+    "uniform mat4 uMVP;\n"
+    "uniform float uPointSize;\n"
+
+    "varying vec3 vColor;\n"
+    "varying float vLightWeighting;\n"
+
+    "void main() {\n"
+    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
+    "  gl_PointSize = uPointSize;\n"
+	"  vLightWeighting = 1.0;\n"
+    "  vColor = aColor;\n"
+    "}\n";
+const std::string kPointCloudLightingVertexShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+    "attribute vec3 aVertex;\n"
+	"attribute vec3 aNormal;\n"
+    "attribute vec3 aColor;\n"
+
+    "uniform mat4 uMVP;\n"
+	"uniform mat3 uN;\n"
+	"uniform vec3 uLightingDirection;\n"
+    "uniform float uPointSize;\n"
+
+    "varying vec3 vColor;\n"
+    "varying float vLightWeighting;\n"
+
+    "void main() {\n"
+    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
+    "  gl_PointSize = uPointSize;\n"
+	"  vec3 transformedNormal = uN * aNormal;\n"
+	"  vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
+	"  if(vLightWeighting<0.5)"
+	"    vLightWeighting=0.5;\n"
+    "  vColor = aColor;\n"
+    "}\n";
+
+const std::string kPointCloudFragmentShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+	"uniform float uGainR;\n"
+	"uniform float uGainG;\n"
+	"uniform float uGainB;\n"
+    "varying vec3 vColor;\n"
+	"varying float vLightWeighting;\n"
+    "void main() {\n"
+    "  vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
+	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
+    "}\n";
+const std::string kPointCloudBlendingFragmentShader =
+    "precision highp float;\n"
+    "precision mediump int;\n"
+	"uniform float uGainR;\n"
+	"uniform float uGainG;\n"
+	"uniform float uGainB;\n"
+	"uniform float uNearZ;\n"
+	"uniform float uFarZ;\n"
+	"uniform sampler2D uDepthTexture;\n"
+	"uniform vec2 uScreenScale;\n"
+    "varying vec3 vColor;\n"
+	"varying float vLightWeighting;\n"
+    "void main() {\n"
+    "  vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
+	"  float alpha = 1.0;\n"
+	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+	"  float depth = texture2D(uDepthTexture, coord).r;\n"
+	"  float num =  (2.0 * uNearZ * uFarZ);\n"
+	"  float diff = (uFarZ - uNearZ);\n"
+	"  float add = (uFarZ + uNearZ);\n"
+	"  float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
+	"  float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
+	"  float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
+	"  float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
+	"  if(linearFragz > linearDepth + 0.05)\n"
+	"    alpha=0.0;\n"
+	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
+    "}\n";
+
+const std::string kPointCloudDepthPackingVertexShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+    "attribute vec3 aVertex;\n"
+    "uniform mat4 uMVP;\n"
+    "uniform float uPointSize;\n"
+    "void main() {\n"
+    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
+    "  gl_PointSize = uPointSize;\n"
+    "}\n";
+const std::string kPointCloudDepthPackingFragmentShader =
+    "precision highp float;\n"
+    "precision mediump int;\n"
+    "void main() {\n"
+	"  float toFixed = 255.0/256.0;\n"
+	"  vec4 enc = vec4(1.0, 255.0, 65025.0, 160581375.0) * toFixed * gl_FragCoord.z;\n"
+	"  enc = fract(enc);\n"
+	"  gl_FragColor = enc;\n"
+    "}\n";
+
+// Texture shaders
+const std::string kTextureMeshVertexShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+    "attribute vec3 aVertex;\n"
+    "attribute vec2 aTexCoord;\n"
+
+    "uniform mat4 uMVP;\n"
+
+    "varying vec2 vTexCoord;\n"
+    "varying float vLightWeighting;\n"
+
+    "void main() {\n"
+    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
+
+	"  if(aTexCoord.x < 0.0) {\n"
+	"    vTexCoord.x = 1.0;\n"
+	"    vTexCoord.y = 1.0;\n" // bottom right corner
+	"  } else {\n"
+    "    vTexCoord = aTexCoord;\n"
+    "  }\n"
+
+    "  vLightWeighting = 1.0;\n"
+    "}\n";
+const std::string kTextureMeshLightingVertexShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+    "attribute vec3 aVertex;\n"
+	"attribute vec3 aNormal;\n"
+    "attribute vec2 aTexCoord;\n"
+
+    "uniform mat4 uMVP;\n"
+    "uniform mat3 uN;\n"
+    "uniform vec3 uLightingDirection;\n"
+
+    "varying vec2 vTexCoord;\n"
+    "varying float vLightWeighting;\n"
+
+    "void main() {\n"
+    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
+
+	"  if(aTexCoord.x < 0.0) {\n"
+	"    vTexCoord.x = 1.0;\n"
+	"    vTexCoord.y = 1.0;\n" // bottom right corner
+	"  } else {\n"
+    "    vTexCoord = aTexCoord;\n"
+    "  }\n"
+
+    "  vec3 transformedNormal = uN * aNormal;\n"
+    "  vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
+    "  if(vLightWeighting<0.5) \n"
+    "    vLightWeighting=0.5;\n"
+    "}\n";
+const std::string kTextureMeshFragmentShader =
+    "precision mediump float;\n"
+    "precision mediump int;\n"
+	"uniform sampler2D uTexture;\n"
+	"uniform float uGainR;\n"
+	"uniform float uGainG;\n"
+	"uniform float uGainB;\n"
+    "varying vec2 vTexCoord;\n"
+	"varying float vLightWeighting;\n"
+	""
+    "void main() {\n"
+    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
+    "}\n";
+const std::string kTextureMeshBlendingFragmentShader =
+    "precision highp float;\n"
+    "precision mediump int;\n"
+	"uniform sampler2D uTexture;\n"
+    "uniform sampler2D uDepthTexture;\n"
+	"uniform float uGainR;\n"
+	"uniform float uGainG;\n"
+	"uniform float uGainB;\n"
+	"uniform vec2 uScreenScale;\n"
+	"uniform float uNearZ;\n"
+	"uniform float uFarZ;\n"
+    "varying vec2 vTexCoord;\n"
+	"varying float vLightWeighting;\n"
+	""
+    "void main() {\n"
+    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+    "  float alpha = 1.0;\n"
+	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+	"  float depth = texture2D(uDepthTexture, coord).r;\n"
+	"  float num =  (2.0 * uNearZ * uFarZ);\n"
+	"  float diff = (uFarZ - uNearZ);\n"
+	"  float add = (uFarZ + uNearZ);\n"
+	"  float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
+	"  float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
+	"  float ndcFragz = gl_FragCoord.z * 2.0 - 1.0;\n" // Back to NDC
+	"  float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
+	"  if(linearFragz > linearDepth + 0.05)\n"
+	"    alpha=0.0;\n"
+	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
+    "}\n";
+
+std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
+
+void PointCloudDrawable::createShaderPrograms()
+{
+	if(shaderPrograms_.empty())
+	{
+		shaderPrograms_.resize(9);
+
+		shaderPrograms_[kPointCloud] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kPointCloud] != 0);
+		shaderPrograms_[kPointCloudBlending] = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kPointCloudBlending] != 0);
+		shaderPrograms_[kPointCloudLighting] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kPointCloudLighting] != 0);
+		shaderPrograms_[kPointCloudLightingBlending] = tango_gl::util::CreateProgram(kPointCloudLightingVertexShader.c_str(), kPointCloudBlendingFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kPointCloudLightingBlending] != 0);
+
+		shaderPrograms_[kTexture] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kTexture] != 0);
+		shaderPrograms_[kTextureBlending] = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kTextureBlending] != 0);
+		shaderPrograms_[kTextureLighting] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kTextureLighting] != 0);
+		shaderPrograms_[kTextureLightingBlending] = tango_gl::util::CreateProgram(kTextureMeshLightingVertexShader.c_str(), kTextureMeshBlendingFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kTextureLightingBlending] != 0);
+
+		shaderPrograms_[kDepthPacking] = tango_gl::util::CreateProgram(kPointCloudDepthPackingVertexShader.c_str(), kPointCloudDepthPackingFragmentShader.c_str());
+		UASSERT(shaderPrograms_[kDepthPacking] != 0);
+	}
+}
+void PointCloudDrawable::releaseShaderPrograms()
+{
+	for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
+	{
+		glDeleteShader(shaderPrograms_[i]);
+	}
+	shaderPrograms_.clear();
+}
+
 PointCloudDrawable::PointCloudDrawable(
-		GLuint cloudShaderProgram,
-		GLuint textureShaderProgram,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float gainR,
@@ -54,8 +310,6 @@ PointCloudDrawable::PointCloudDrawable(
 				poseGl_(1.0f),
 				visible_(true),
 				hasNormals_(false),
-				cloud_shader_program_(cloudShaderProgram),
-				texture_shader_program_(textureShaderProgram),
 				gainR_(gainR),
 				gainG_(gainG),
 				gainB_(gainB)
@@ -64,9 +318,8 @@ PointCloudDrawable::PointCloudDrawable(
 }
 
 PointCloudDrawable::PointCloudDrawable(
-		GLuint cloudShaderProgram,
-		GLuint textureShaderProgram,
-		const Mesh & mesh) :
+		const Mesh & mesh,
+		bool createWireframe) :
 				vertex_buffers_(0),
 				textures_(0),
 				nPoints_(0),
@@ -74,13 +327,11 @@ PointCloudDrawable::PointCloudDrawable(
 				poseGl_(1.0f),
 				visible_(true),
 				hasNormals_(false),
-				cloud_shader_program_(cloudShaderProgram),
-				texture_shader_program_(textureShaderProgram),
 				gainR_(1.0f),
 				gainG_(1.0f),
 				gainB_(1.0f)
 {
-	updateMesh(mesh);
+	updateMesh(mesh, createWireframe);
 }
 
 PointCloudDrawable::~PointCloudDrawable()
@@ -101,22 +352,33 @@ PointCloudDrawable::~PointCloudDrawable()
 	}
 }
 
-void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes)
+void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
 {
 	LOGD("Update polygons");
 	polygons_.clear();
+	polygonLines_.clear();
+	polygonsLowRes_.clear();
+	polygonLinesLowRes_.clear();
 	if(polygons.size() && organizedToDenseIndices_.size())
 	{
 		unsigned int polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
 		polygons_.resize(polygons.size() * polygonSize);
+		if(createWireframe)
+			polygonLines_.resize(polygons_.size()*2);
 		int oi = 0;
+		int li = 0;
 		for(unsigned int i=0; i<polygons.size(); ++i)
 		{
 			UASSERT(polygons[i].vertices.size() == polygonSize);
 			for(unsigned int j=0; j<polygonSize; ++j)
 			{
 				polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+				if(createWireframe)
+				{
+					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
+				}
 			}
 		}
 
@@ -125,13 +387,21 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 			unsigned int polygonSize = polygonsLowRes[0].vertices.size();
 			UASSERT(polygonSize == 3);
 			polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
+			if(createWireframe)
+				polygonLinesLowRes_.resize(polygonsLowRes_.size()*2);
 			int oi = 0;
+			int li = 0;
 			for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
 			{
 				UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
 				for(unsigned int j=0; j<polygonSize; ++j)
 				{
 					polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+					if(createWireframe)
+					{
+						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
+					}
 				}
 			}
 		}
@@ -255,7 +525,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	nPoints_ = totalPoints;
 }
 
-void PointCloudDrawable::updateMesh(const Mesh & mesh)
+void PointCloudDrawable::updateMesh(const Mesh & mesh, bool createWireframe)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;
@@ -321,9 +591,8 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 		int oi_lowlow = 0;
 		if(textures_ && polygons.size())
 		{
-			//LOGD("Organized mesh with texture");
 			int items = hasNormals_?9:6;
-			vertices = std::vector<float>(mesh.indices->size()*9);
+			vertices = std::vector<float>(mesh.indices->size()*items);
 			for(unsigned int i=0; i<mesh.indices->size(); ++i)
 			{
 				const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
@@ -401,7 +670,6 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 	}
 	else // assume dense mesh with texCoords set to polygons
 	{
-		totalPoints = mesh.cloud->size();
 		if(textures_ && polygons.size() && mesh.normals->size())
 		{
 			//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
@@ -411,8 +679,9 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 			//  tex_coordinates should be linked to points, not
 			//  polygon vertices. Points linked to multiple different texCoords (different textures) should
 			//  be duplicated.
+			totalPoints = mesh.texCoords.size();
 			vertices = std::vector<float>(mesh.texCoords.size()*9);
-			organizedToDenseIndices_ = std::vector<unsigned int>(mesh.texCoords.size(), -1);
+			organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
 
 			UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
 					uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
@@ -468,9 +737,10 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 		}
 		else
 		{
+			totalPoints = mesh.cloud->size();
 			//LOGD("Dense mesh");
 			int items = hasNormals_?7:4;
-			organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->size(), -1);
+			organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
 			vertices = std::vector<float>(mesh.cloud->size()*items);
 			for(unsigned int i=0; i<mesh.cloud->size(); ++i)
 			{
@@ -547,7 +817,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 
 	if(polygons_.size() != polygons.size())
 	{
-		updatePolygons(polygons, polygonsLowRes);
+		updatePolygons(polygons, polygonsLowRes, createWireframe);
 	}
 
 	if(!pose_.isNull())
@@ -594,206 +864,245 @@ void PointCloudDrawable::updateAABBWorld(const rtabmap::Transform & pose)
 }
 
 
-void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
+void PointCloudDrawable::Render(
+		const glm::mat4 & projectionMatrix,
 		const glm::mat4 & viewMatrix,
 		bool meshRendering,
 		float pointSize,
 		bool textureRendering,
 		bool lighting,
-		float distanceToCameraSqr) {
-
-	if(vertex_buffers_ && nPoints_ && visible_)
+		float distanceToCameraSqr,
+		const GLuint & depthTexture,
+		int screenWidth,
+		int screenHeight,
+		float nearClipPlane,
+	    float farClipPlane,
+		bool packDepthToColorChannel,
+		bool wireFrame) const
+{
+	if(vertex_buffers_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
-		if(meshRendering && textureRendering && textures_ && (verticesLowRes_.empty() || distanceToCameraSqr<50.0f))
+		if(packDepthToColorChannel || !hasNormals_)
 		{
-			glUseProgram(texture_shader_program_);
+			lighting = false;
+		}
 
-			GLuint mvp_handle = glGetUniformLocation(texture_shader_program_, "uMVP");
-			glm::mat4 mv_mat = viewMatrix * poseGl_;
-			glm::mat4 mvp_mat = projectionMatrix * mv_mat;
-			glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
-
-			GLuint n_handle = glGetUniformLocation(texture_shader_program_, "uN");
-			glm::mat3 normalMatrix(mv_mat);
-			normalMatrix = glm::inverse(normalMatrix);
-			normalMatrix = glm::transpose(normalMatrix);
-			glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
-
-			if(!hasNormals_)
-			{
-				lighting = false;
-			}
-
-			//lighting
-			GLuint lighting_handle = glGetUniformLocation(texture_shader_program_, "uUseLighting");
-			glUniform1i(lighting_handle, lighting?1:0);
+		if(packDepthToColorChannel || !(meshRendering && textureRendering && textures_))
+		{
+			textureRendering = false;
+		}
 
+		GLuint program;
+		if(packDepthToColorChannel)
+		{
+			program = shaderPrograms_[kDepthPacking];
+		}
+		else if(textureRendering)
+		{
 			if(lighting)
 			{
-				GLuint ambiant_handle = glGetUniformLocation(texture_shader_program_, "uAmbientColor");
-				glUniform3f(ambiant_handle,0.6,0.6,0.6);
-
-				GLuint lightingDirection_handle = glGetUniformLocation(texture_shader_program_, "uLightingDirection");
-				glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
+				program = shaderPrograms_[depthTexture>0?kTextureLightingBlending:kTextureLighting];
 			}
+			else
+			{
+				program = shaderPrograms_[depthTexture>0?kTextureBlending:kTexture];
+			}
+		}
+		else
+		{
+			if(lighting)
+			{
+				program = shaderPrograms_[depthTexture>0?kPointCloudLightingBlending:kPointCloudLighting];
+			}
+			else
+			{
+				program = shaderPrograms_[depthTexture>0?kPointCloudBlending:kPointCloud];
+			}
+		}
 
-			// Texture activate unit 0
-			glActiveTexture(GL_TEXTURE0);
-			// Bind the texture to this unit.
-			glBindTexture(GL_TEXTURE_2D, textures_);
-			// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
-			GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "uTexture");
-			glUniform1i(texture_handle, 0);
+		glUseProgram(program);
+		tango_gl::util::CheckGlError("Pointcloud::Render() set program");
 
-			GLuint gainR_handle = glGetUniformLocation(texture_shader_program_, "uGainR");
-			GLuint gainG_handle = glGetUniformLocation(texture_shader_program_, "uGainG");
-			GLuint gainB_handle = glGetUniformLocation(texture_shader_program_, "uGainB");
+		GLuint mvp_handle = glGetUniformLocation(program, "uMVP");
+		glm::mat4 mv_mat = viewMatrix * poseGl_;
+		glm::mat4 mvp_mat = projectionMatrix * mv_mat;
+		glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+		GLint attribute_vertex = glGetAttribLocation(program, "aVertex");
+		glEnableVertexAttribArray(attribute_vertex);
+		GLint attribute_color = 0;
+		GLint attribute_texture = 0;
+		GLint attribute_normal = 0;
+
+		if(packDepthToColorChannel || !textureRendering)
+		{
+			GLuint point_size_handle_ = glGetUniformLocation(program, "uPointSize");
+			glUniform1f(point_size_handle_, pointSize);
+		}
+		tango_gl::util::CheckGlError("Pointcloud::Render() vertex");
+
+		if(!packDepthToColorChannel)
+		{
+			GLuint gainR_handle = glGetUniformLocation(program, "uGainR");
+			GLuint gainG_handle = glGetUniformLocation(program, "uGainG");
+			GLuint gainB_handle = glGetUniformLocation(program, "uGainB");
 			glUniform1f(gainR_handle, gainR_);
 			glUniform1f(gainG_handle, gainG_);
 			glUniform1f(gainB_handle, gainB_);
 
-			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "aVertex");
-			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "aTexCoord");
-			GLint attribute_normal=0;
-			if(hasNormals_)
+			// blending
+			if(depthTexture > 0)
 			{
-				attribute_normal = glGetAttribLocation(texture_shader_program_, "aNormal");
+				// Texture activate unit 1
+				glActiveTexture(GL_TEXTURE1);
+				// Bind the texture to this unit.
+				glBindTexture(GL_TEXTURE_2D, depthTexture);
+				// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
+				GLuint depth_texture_handle = glGetUniformLocation(program, "uDepthTexture");
+				glUniform1i(depth_texture_handle, 1);
+
+				GLuint zNear_handle = glGetUniformLocation(program, "uNearZ");
+				GLuint zFar_handle = glGetUniformLocation(program, "uFarZ");
+				glUniform1f(zNear_handle, nearClipPlane);
+				glUniform1f(zFar_handle, farClipPlane);
+
+				GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
+				glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
 			}
 
-			glEnableVertexAttribArray(attribute_vertex);
-			glEnableVertexAttribArray(attribute_texture);
-			if(hasNormals_)
+			if(lighting)
 			{
+				GLuint n_handle = glGetUniformLocation(program, "uN");
+				glm::mat3 normalMatrix(mv_mat);
+				normalMatrix = glm::inverse(normalMatrix);
+				normalMatrix = glm::transpose(normalMatrix);
+				glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
+
+				GLuint lightingDirection_handle = glGetUniformLocation(program, "uLightingDirection");
+				glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
+
+				attribute_normal = glGetAttribLocation(program, "aNormal");
 				glEnableVertexAttribArray(attribute_normal);
 			}
-			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+
+			if(textureRendering)
+			{
+				// Texture activate unit 0
+				glActiveTexture(GL_TEXTURE0);
+				// Bind the texture to this unit.
+				glBindTexture(GL_TEXTURE_2D, textures_);
+				// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
+				GLuint texture_handle = glGetUniformLocation(program, "uTexture");
+				glUniform1i(texture_handle, 0);
+
+				attribute_texture = glGetAttribLocation(program, "aTexCoord");
+				glEnableVertexAttribArray(attribute_texture);
+			}
+			else
+			{
+				attribute_color = glGetAttribLocation(program, "aColor");
+				glEnableVertexAttribArray(attribute_color);
+			}
+		}
+		tango_gl::util::CheckGlError("Pointcloud::Render() common");
+
+		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+		if(textures_)
+		{
 			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
-			glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
-			if(hasNormals_)
+			if(textureRendering)
+			{
+				glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
+			}
+			else if(!packDepthToColorChannel)
+			{
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE,  (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			}
+			if(lighting && hasNormals_)
 			{
 				glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
 			}
-			if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
+		}
+		else
+		{
+			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
+			if(!packDepthToColorChannel)
 			{
-				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
 			}
-			else
+			if(lighting && hasNormals_)
 			{
-				glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+				glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
 			}
 		}
-		else // point cloud or colored mesh
+		tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");
+
+		UTimer drawTime;
+		if(textureRendering)
 		{
-			glUseProgram(cloud_shader_program_);
-
-			GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "uMVP");
-			glm::mat4 mv_mat = viewMatrix * poseGl_;
-			glm::mat4 mvp_mat = projectionMatrix * mv_mat;
-			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
-
-			GLuint n_handle = glGetUniformLocation(texture_shader_program_, "uN");
-			glm::mat3 normalMatrix(mv_mat);
-			normalMatrix = glm::inverse(normalMatrix);
-			normalMatrix = glm::transpose(normalMatrix);
-			glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
-
-			if(!hasNormals_)
+			if(distanceToCameraSqr<16.0f || polygonsLowRes_.empty())
 			{
-				lighting = false;
-			}
-
-			//lighting
-			GLuint lighting_handle = glGetUniformLocation(texture_shader_program_, "uUseLighting");
-			glUniform1i(lighting_handle, lighting?1:0);
-
-			if(lighting)
-			{
-				GLuint ambiant_handle = glGetUniformLocation(texture_shader_program_, "uAmbientColor");
-				glUniform3f(ambiant_handle,0.6,0.6,0.6);
-
-				GLuint lightingDirection_handle = glGetUniformLocation(texture_shader_program_, "uLightingDirection");
-				glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
-			}
-
-			GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "uPointSize");
-			glUniform1f(point_size_handle_, pointSize);
-
-			GLuint gainR_handle = glGetUniformLocation(cloud_shader_program_, "uGainR");
-			GLuint gainG_handle = glGetUniformLocation(cloud_shader_program_, "uGainG");
-			GLuint gainB_handle = glGetUniformLocation(cloud_shader_program_, "uGainB");
-			glUniform1f(gainR_handle, gainR_);
-			glUniform1f(gainG_handle, gainG_);
-			glUniform1f(gainB_handle, gainB_);
-
-			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "aVertex");
-			GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "aColor");
-			GLint attribute_normal=0;
-			if(hasNormals_)
-			{
-				attribute_normal = glGetAttribLocation(cloud_shader_program_, "aNormal");
-			}
-
-			glEnableVertexAttribArray(attribute_vertex);
-			glEnableVertexAttribArray(attribute_color);
-			if(hasNormals_)
-			{
-				glEnableVertexAttribArray(attribute_normal);
-			}
-			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-			if(textures_)
-			{
-				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
-				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE,  (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
-				if(hasNormals_)
-				{
-					glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
-				}
+				wireFrame = wireFrame && polygonLines_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+				else
+					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
 			}
 			else
 			{
-				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
-				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
-				if(hasNormals_)
-				{
-					glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
-				}
-			}
-			if(meshRendering && polygons_.size())
-			{
-				if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
-				{
-					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
-				}
+				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
 				else
-				{
 					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
-				}
 			}
-			else if(!verticesLowRes_.empty())
+		}
+		else if(meshRendering && polygons_.size())
+		{
+			if(distanceToCameraSqr<50.0f || polygonsLowRes_.empty())
 			{
-				if(distanceToCameraSqr>600.0f)
-				{
-					glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
-				}
-				else if(distanceToCameraSqr>150.0f)
-				{
-					glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
-				}
+				wireFrame = wireFrame && polygonLines_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
 				else
-				{
-					glDrawArrays(GL_POINTS, 0, nPoints_);
-				}
+					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+			}
+			else
+			{
+				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+				else
+					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+			}
+		}
+		else if(!verticesLowRes_.empty())
+		{
+			if(distanceToCameraSqr>600.0f)
+			{
+				glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
+			}
+			else if(distanceToCameraSqr>150.0f)
+			{
+				glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
 			}
 			else
 			{
 				glDrawArrays(GL_POINTS, 0, nPoints_);
 			}
 		}
+		else
+		{
+			glDrawArrays(GL_POINTS, 0, nPoints_);
+		}
+		//UERROR("drawTime=%fs", drawTime.ticks());
+		tango_gl::util::CheckGlError("Pointcloud::Render() draw");
+
 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
 
 		glUseProgram(0);
-		tango_gl::util::CheckGlError("Pointcloud::Render()");
+		tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");
 	}
 }
 

app/android/jni/point_cloud_drawable.h

@@ -40,24 +40,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 // PointCloudDrawable is responsible for the point cloud rendering.
 class PointCloudDrawable {
+public:
+	static void createShaderPrograms();
+	static void releaseShaderPrograms();
+
+private:
+	static std::vector<GLuint> shaderPrograms_;
+
  public:
   PointCloudDrawable(
-		  GLuint cloudShaderProgram,
-		  GLuint textureShaderProgram,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		  const pcl::IndicesPtr & indices,
 		  float gainR = 1.0f,
 		  float gainG = 1.0f,
 		  float gainB = 1.0f);
   PointCloudDrawable(
-  		  GLuint cloudShaderProgram,
-  		  GLuint textureShaderProgram,
-    	  const Mesh & mesh);
+    	  const Mesh & mesh,
+		  bool createWireframe = false);
   virtual ~PointCloudDrawable();
 
-  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>());
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
   void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
-  void updateMesh(const Mesh & mesh);
+  void updateMesh(const Mesh & mesh, bool createWireframe = false);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
@@ -66,6 +70,7 @@ class PointCloudDrawable {
   bool isVisible() const {return visible_;}
   bool hasMesh() const {return polygons_.size()!=0;}
   bool hasTexture() const {return textures_ != 0;}
+  float getMinHeight() const {return minHeight_;}
   const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
   const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
   const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
@@ -77,13 +82,21 @@ class PointCloudDrawable {
   // @param view_mat: view matrix from current render camera.
   // @param model_mat: model matrix for this point cloud frame.
   // @param vertices: all vertices in this point cloud frame.
-  void Render(const glm::mat4 & projectionMatrix,
+  void Render(
+		  const glm::mat4 & projectionMatrix,
 		  const glm::mat4 & viewMatrix,
 		  bool meshRendering = true,
 		  float pointSize = 3.0f,
 		  bool textureRendering = false,
 		  bool lighting = true,
-		  float distanceToCamSqr = 0.0f);
+		  float distanceToCamSqr = 0.0f,
+		  const GLuint & depthTexture = 0,
+		  int screenWidth = 0,     // nonnull if depthTexture>0
+		  int screenHeight = 0,    // nonnull if depthTexture>0
+		  float nearClipPlane = 0, // nonnull if depthTexture>0
+		  float farClipPlane = 0,  // nonnull if depthTexture>0
+		  bool packDepthToColorChannel = false,
+		  bool wireFrame = false) const;
 
  private:
   template<class PointT>
@@ -104,6 +117,8 @@ class PointCloudDrawable {
   GLuint textures_;
   std::vector<GLuint> polygons_;
   std::vector<GLuint> polygonsLowRes_;
+  std::vector<GLuint> polygonLines_;
+  std::vector<GLuint> polygonLinesLowRes_;
   std::vector<GLuint> verticesLowRes_;
   std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
@@ -112,9 +127,7 @@ class PointCloudDrawable {
   bool visible_;
   bool hasNormals_;
   std::vector<unsigned int> organizedToDenseIndices_;
-
-  GLuint cloud_shader_program_;
-  GLuint texture_shader_program_;
+  float minHeight_; // odom frame
 
   float gainR_;
   float gainG_;

app/android/jni/scene.cpp

@@ -16,11 +16,16 @@
 
 #include <tango-gl/conversions.h>
 #include <tango-gl/gesture_camera.h>
+#include <tango-gl/util.h>
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/common.h>
 
 #include <glm/gtx/transform.hpp>
 
@@ -31,7 +36,7 @@
 // add an offset in z to our origin. We'll set this offset to 1.3 meters based
 // on the average height of a human standing with a Tango device. This allows us
 // to place a grid roughly on the ground for most users.
-const glm::vec3 kHeightOffset = glm::vec3(0.0f, 1.3f, 0.0f);
+const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
 
 // Color of the motion tracking trajectory.
 const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
@@ -42,96 +47,6 @@ const tango_gl::Color kGridColor(0.85f, 0.85f, 0.85f);
 // Frustum scale.
 const glm::vec3 kFrustumScale = glm::vec3(0.4f, 0.3f, 0.5f);
 
-const std::string kPointCloudVertexShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-    "attribute vec3 aVertex;\n"
-	"attribute vec3 aNormal;\n"
-    "attribute vec3 aColor;\n"
-
-    "uniform mat4 uMVP;\n"
-	"uniform mat3 uN;\n"
-	"uniform vec3 uAmbientColor;\n"
-	"uniform vec3 uLightingDirection;\n"
-	"uniform bool uUseLighting;\n"
-
-    "uniform float uPointSize;\n"
-    "varying vec3 vColor;\n"
-    "varying float vLightWeighting;\n"
-
-    "void main() {\n"
-    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
-    "  gl_PointSize = uPointSize;\n"
-	"  if (!uUseLighting) {\n"
-	"    vLightWeighting = 1.0;\n"
-	"  } else {\n"
-	"    vec3 transformedNormal = uN * aNormal;\n"
-	"    vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
-	"    if(vLightWeighting<0.5) vLightWeighting=0.5;\n"
-	"  }\n"
-    "  vColor = aColor;\n"
-    "}\n";
-const std::string kPointCloudFragmentShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-	"uniform float uGainR;\n"
-	"uniform float uGainG;\n"
-	"uniform float uGainB;\n"
-    "varying vec3 vColor;\n"
-	"varying float vLightWeighting;\n"
-    "void main() {\n"
-    "  vec4 textureColor = vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
-	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
-    "}\n";
-
-const std::string kTextureMeshVertexShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-    "attribute vec3 aVertex;\n"
-	"attribute vec3 aNormal;\n"
-    "attribute vec2 aTexCoord;\n"
-
-    "uniform mat4 uMVP;\n"
-    "uniform mat3 uN;\n"
-	"uniform vec3 uAmbientColor;\n"
-    "uniform vec3 uLightingDirection;\n"
-    "uniform bool uUseLighting;\n"
-
-    "varying vec2 vTexCoord;\n"
-    "varying float vLightWeighting;\n"
-
-    "void main() {\n"
-    "  gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
-
-	"  if(aTexCoord.x < 0.0) {\n"
-	"    vTexCoord.x = 1.0;\n"
-	"    vTexCoord.y = 1.0;\n" // bottom right corner
-	"  } else {\n"
-    "    vTexCoord = aTexCoord;\n"
-    "  }\n"
-
-	"  if (!uUseLighting) {\n"
-    "    vLightWeighting = 1.0;\n"
-    "  } else {\n"
-    "    vec3 transformedNormal = uN * aNormal;\n"
-    "    vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
-    "    if(vLightWeighting<0.5) vLightWeighting=0.5;\n"
-    "  }\n"
-    "}\n";
-const std::string kTextureMeshFragmentShader =
-    "precision mediump float;\n"
-    "precision mediump int;\n"
-	"uniform sampler2D uTexture;\n"
-	"uniform float uGainR;\n"
-	"uniform float uGainG;\n"
-	"uniform float uGainB;\n"
-    "varying vec2 vTexCoord;\n"
-	"varying float vLightWeighting;\n"
-    "void main() {\n"
-    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
-	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
-    "}\n";
-
 const std::string kGraphVertexShader =
     "precision mediump float;\n"
     "precision mediump int;\n"
@@ -166,20 +81,24 @@ Scene::Scene() :
 		traceVisible_(true),
 		color_camera_to_display_rotation_(ROTATION_0),
 		currentPose_(0),
-		cloud_shader_program_(0),
-		texture_mesh_shader_program_(0),
 		graph_shader_program_(0),
+		blending_(true),
 		mapRendering_(true),
 		meshRendering_(true),
 		meshRenderingTexture_(true),
 		pointSize_(5.0f),
-		frustumCulling_(true),
 		boundingBoxRendering_(false),
-		lighting_(true),
+		lighting_(false),
 		backfaceCulling_(true),
+		wireFrame_(false),
 		r_(0.0f),
 		g_(0.0f),
-		b_(0.0f)
+		b_(0.0f),
+		fboId_(0),
+		depthTexture_(0),
+		screenWidth_(0),
+		screenHeight_(0),
+		doubleTapOn_(false)
 {
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
@@ -202,38 +121,30 @@ void Scene::InitGLContent()
 	UASSERT(axis_ == 0);
 
 
-  axis_ = new tango_gl::Axis();
-  frustum_ = new tango_gl::Frustum();
-  trace_ = new tango_gl::Trace();
-  grid_ = new tango_gl::Grid();
-  box_ = new BoundingBoxDrawable();
-  currentPose_ = new rtabmap::Transform();
+	axis_ = new tango_gl::Axis();
+	frustum_ = new tango_gl::Frustum();
+	trace_ = new tango_gl::Trace();
+	grid_ = new tango_gl::Grid();
+	box_ = new BoundingBoxDrawable();
+	currentPose_ = new rtabmap::Transform();
 
 
-  axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
-  frustum_->SetColor(kTraceColor);
-  trace_->ClearVertexArray();
-  trace_->SetColor(kTraceColor);
-  grid_->SetColor(kGridColor);
-  grid_->SetPosition(-kHeightOffset);
-  box_->SetShader();
-  box_->SetColor(1,0,0);
+	axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
+	frustum_->SetColor(kTraceColor);
+	trace_->ClearVertexArray();
+	trace_->SetColor(kTraceColor);
+	grid_->SetColor(kGridColor);
+	grid_->SetPosition(kHeightOffset);
+	box_->SetShader();
+	box_->SetColor(1,0,0);
 
-  if(cloud_shader_program_ == 0)
-  {
-	  cloud_shader_program_ = tango_gl::util::CreateProgram(kPointCloudVertexShader.c_str(), kPointCloudFragmentShader.c_str());
-	  UASSERT(cloud_shader_program_ != 0);
-  }
-  if(texture_mesh_shader_program_ == 0)
-  {
-	  texture_mesh_shader_program_ = tango_gl::util::CreateProgram(kTextureMeshVertexShader.c_str(), kTextureMeshFragmentShader.c_str());
-	  UASSERT(texture_mesh_shader_program_ != 0);
-  }
-  if(graph_shader_program_ == 0)
-  {
-	  graph_shader_program_ = tango_gl::util::CreateProgram(kGraphVertexShader.c_str(), kGraphFragmentShader.c_str());
-	  UASSERT(graph_shader_program_ != 0);
-  }
+	PointCloudDrawable::createShaderPrograms();
+
+	if(graph_shader_program_ == 0)
+	{
+		graph_shader_program_ = tango_gl::util::CreateProgram(kGraphVertexShader.c_str(), kGraphFragmentShader.c_str());
+		UASSERT(graph_shader_program_ != 0);
+	}
 }
 
 //Should only be called in OpenGL thread!
@@ -242,27 +153,29 @@ void Scene::DeleteResources() {
 	LOGI("Scene::DeleteResources()");
 	if(axis_)
 	{
-	  delete axis_;
-	  axis_ = 0;
-	  delete frustum_;
-	  delete trace_;
-	  delete grid_;
-	  delete currentPose_;
-	  delete box_;
+		delete axis_;
+		axis_ = 0;
+		delete frustum_;
+		delete trace_;
+		delete grid_;
+		delete currentPose_;
+		delete box_;
 	}
 
-	if (cloud_shader_program_) {
-		glDeleteShader(cloud_shader_program_);
-		cloud_shader_program_ = 0;
-	  }
-	if (texture_mesh_shader_program_) {
-		glDeleteShader(texture_mesh_shader_program_);
-		texture_mesh_shader_program_ = 0;
-	  }
+	PointCloudDrawable::releaseShaderPrograms();
+
 	if (graph_shader_program_) {
 		glDeleteShader(graph_shader_program_);
 		graph_shader_program_ = 0;
-	  }
+	}
+
+	if(fboId_>0)
+	{
+		glDeleteFramebuffers(1, &fboId_);
+		fboId_ = 0;
+		glDeleteTextures(1, &depthTexture_);
+		depthTexture_ = 0;
+	}
 
 	clear();
 }
@@ -285,17 +198,57 @@ void Scene::clear()
 		graph_ = 0;
 	}
 	pointClouds_.clear();
+	if(grid_)
+	{
+		grid_->SetPosition(kHeightOffset);
+	}
 }
 
 //Should only be called in OpenGL thread!
 void Scene::SetupViewPort(int w, int h) {
-  if (h == 0) {
-    LOGE("Setup graphic height not valid");
-  }
-  UASSERT(gesture_camera_ != 0);
-  gesture_camera_->SetAspectRatio(static_cast<float>(w) /
-                                  static_cast<float>(h));
-  glViewport(0, 0, w, h);
+	if (h == 0) {
+		LOGE("Setup graphic height not valid");
+	}
+	UASSERT(gesture_camera_ != 0);
+	gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
+	glViewport(0, 0, w, h);
+	if(screenWidth_ != w || fboId_ == 0)
+	{
+		if(fboId_>0)
+		{
+			glDeleteFramebuffers(1, &fboId_);
+			fboId_ = 0;
+			glDeleteTextures(1, &depthTexture_);
+			depthTexture_ = 0;
+		}
+
+		// Create depth texture
+		glGenTextures(1, &depthTexture_);
+		glBindTexture(GL_TEXTURE_2D, depthTexture_);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, w, h, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_INT, NULL);
+		glBindTexture(GL_TEXTURE_2D, 0);
+
+		// regenerate fbo texture
+		// create a framebuffer object, you need to delete them when program exits.
+		glGenFramebuffers(1, &fboId_);
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
+		// Set the texture to be at the depth attachment point of the FBO
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depthTexture_, 0);
+
+		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
+		if ( status != GL_FRAMEBUFFER_COMPLETE)
+		{
+			LOGE("Frame buffer cannot be generated! Status: %in", status);
+		}
+		glBindFramebuffer(GL_FRAMEBUFFER,0);
+	}
+	screenWidth_ = w;
+	screenHeight_ = h;
 }
 
 std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normalize = true)
@@ -409,26 +362,12 @@ bool intersectFrustumAABB(
 int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
 
-	glEnable(GL_DEPTH_TEST);
-	if(backfaceCulling_)
-	{
-		glEnable(GL_CULL_FACE);
-	}
-	else
-	{
-		glDisable(GL_CULL_FACE);
-	}
-
-	glClearColor(r_, g_, b_, 1.0f);
-	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
+	glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
+	Eigen::Quaternionf quat = currentPose_->getQuaternionf();
+	glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
+	glm::mat4 rotateM;
 	if(!currentPose_->isNull())
 	{
-		glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
-		Eigen::Quaternionf quat = currentPose_->getQuaternionf();
-		glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
-
-		glm::mat4 rotateM;
 		rotateM = glm::rotate<float>(float(color_camera_to_display_rotation_)*-1.57079632679489661923132169163975144, glm::vec3(0.0f, 0.0f, 1.0f));
 
 		if (gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
@@ -441,108 +380,178 @@ int Scene::Render() {
 		{
 			// In third person or top down mode, we follow the camera movement.
 			gesture_camera_->SetAnchorPosition(position, rotation*glm::quat(rotateM));
-
-			frustum_->SetPosition(position);
-			frustum_->SetRotation(rotation);
-			// Set the frustum scale to 4:3, this doesn't necessarily match the physical
-			// camera's aspect ratio, this is just for visualization purposes.
-			frustum_->SetScale(kFrustumScale);
-			frustum_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
-
-			axis_->SetPosition(position);
-			axis_->SetRotation(rotation);
-			axis_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
-		}
-
-		trace_->UpdateVertexArray(position);
-		if(traceVisible_)
-		{
-			trace_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
-		}
-
-		if(gridVisible_)
-		{
-			grid_->Render(gesture_camera_->GetProjectionMatrix(),
-					gesture_camera_->GetViewMatrix());
 		}
 	}
 
-	float fov = 45.0f;
-	rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov);//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
+	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
+	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
+
+	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
 		 0.0f,  0.0f,  1.0f, 0.0f,
 		 0.0f,  1.0f,  0.0f, 0.0f,
 		-1.0f,  0.0f,  0.0f, 0.0f);
 
-	int cloudDrawn=0;
-	if(mapRendering_ && frustumCulling_)
+	//Culling
+	std::vector<glm::vec4> planes = computeFrustumPlanes(projectionMatrix*viewMatrix, true);
+	std::vector<PointCloudDrawable*> cloudsToDraw(pointClouds_.size());
+	int oi=0;
+	for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 	{
-		std::vector<glm::vec4> planes = computeFrustumPlanes(gesture_camera_->GetProjectionMatrix()*gesture_camera_->GetViewMatrix(), true);
-		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		if(!mapRendering_ && iter->first > 0)
 		{
-			if(iter->second->isVisible())
-			{
-				if(intersectFrustumAABB(planes,
-						iter->second->aabbMinWorld(),
-						iter->second->aabbMaxWorld()))
-				{
-					if(boundingBoxRendering_)
-					{
-						box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
-						box_->Render(gesture_camera_->GetProjectionMatrix(),
-											gesture_camera_->GetViewMatrix());
-					}
+			break;
+		}
 
-					++cloudDrawn;
-					Eigen::Vector3f cloudToCamera(
-							iter->second->getPose().x() - openglCamera.x(),
-							iter->second->getPose().y() - openglCamera.y(),
-							iter->second->getPose().z() - openglCamera.z());
-					float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
-					iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
-				}
+		if(iter->second->isVisible())
+		{
+			if(intersectFrustumAABB(planes,
+					iter->second->aabbMinWorld(),
+					iter->second->aabbMaxWorld()))
+			{
+				cloudsToDraw[oi++] = iter->second;
 			}
 		}
 	}
+	cloudsToDraw.resize(oi);
+
+	// First rendering to get depth texture
+	glEnable(GL_DEPTH_TEST);
+	glDepthFunc(GL_LESS);
+	glDepthMask(GL_TRUE);
+	glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
+	glDisable (GL_BLEND);
+	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+	if(backfaceCulling_)
+	{
+		glEnable(GL_CULL_FACE);
+	}
 	else
 	{
-		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
+		glDisable(GL_CULL_FACE);
+	}
+
+	UTimer timer;
+
+	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	if(onlineBlending && fboId_)
+	{
+		// set the rendering destination to FBO
+		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
+
+		glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
+		glClearColor(1, 1, 1, 1);
+		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+		// Draw scene
+		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
 		{
-			if(!mapRendering_ && iter->first > 0)
-			{
-				break;
-			}
+			// set large distance to cam to use low res polygons for fast processing
+			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+		}
 
-			if(iter->second->isVisible())
-			{
-				if(boundingBoxRendering_)
-				{
-					box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
-					box_->Render(gesture_camera_->GetProjectionMatrix(),
-										gesture_camera_->GetViewMatrix());
-				}
+		// back to normal window-system-provided framebuffer
+		glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
+		glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
+	}
 
-				++cloudDrawn;
-				Eigen::Vector3f cloudToCamera(
-						iter->second->getPose().x() - openglCamera.x(),
-						iter->second->getPose().y() - openglCamera.y(),
-						iter->second->getPose().z() - openglCamera.z());
-				float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
-				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
-			}
+	if(doubleTapOn_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+	{
+		glClearColor(0, 0, 0, 0);
+		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+		{
+			// set large distance to cam to use low res polygons for fast processing
+			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+		}
+
+		GLubyte zValue[4];
+		glReadPixels(doubleTapPos_.x*screenWidth_, screenHeight_-doubleTapPos_.y*screenHeight_, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, zValue);
+		float fromFixed = 256.0f/255.0f;
+		float zValueF = float(zValue[0]/255.0f)*fromFixed + float(zValue[1]/255.0f)*fromFixed/255.0f + float(zValue[2]/255.0f)*fromFixed/65025.0f + float(zValue[3]/255.0f)*fromFixed/160581375.0f;
+
+		if(zValueF != 0.0f)
+		{
+			zValueF = zValueF*2.0-1.0;//NDC
+			glm::vec4 point = glm::inverse(projectionMatrix*viewMatrix)*glm::vec4(doubleTapPos_.x*2.0f-1.0f, (1.0f-doubleTapPos_.y)*2.0f-1.0f, zValueF, 1.0f);
+			point /= point.w;
+			gesture_camera_->SetAnchorOffset(glm::vec3(point.x, point.y, point.z) - position);
+		}
+	}
+	doubleTapOn_ = false;
+
+	glClearColor(r_, g_, b_, 1.0f);
+	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+
+	if(!currentPose_->isNull())
+	{
+		if (gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+		{
+			frustum_->SetPosition(position);
+			frustum_->SetRotation(rotation);
+			// Set the frustum scale to 4:3, this doesn't necessarily match the physical
+			// camera's aspect ratio, this is just for visualization purposes.
+			frustum_->SetScale(kFrustumScale);
+			frustum_->Render(projectionMatrix, viewMatrix);
+
+			axis_->SetPosition(position);
+			axis_->SetRotation(rotation);
+			axis_->Render(projectionMatrix, viewMatrix);
+		}
+
+		trace_->UpdateVertexArray(position);
+		if(traceVisible_)
+		{
+			trace_->Render(projectionMatrix, viewMatrix);
+		}
+
+		if(gridVisible_)
+		{
+			grid_->Render(projectionMatrix, viewMatrix);
 		}
 	}
 
 	if(graphVisible_ && graph_)
 	{
-		graph_->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix());
+		graph_->Render(projectionMatrix, viewMatrix);
 	}
 
-	return cloudDrawn;
+
+	if(onlineBlending)
+	{
+		glEnable (GL_BLEND);
+		glDepthMask(GL_FALSE);
+	}
+
+	for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+	{
+		PointCloudDrawable * cloud = *iter;
+
+		if(boundingBoxRendering_)
+		{
+			box_->updateVertices(cloud->aabbMinWorld(), cloud->aabbMaxWorld());
+			box_->Render(projectionMatrix, viewMatrix);
+		}
+
+		Eigen::Vector3f cloudToCamera(
+				cloud->getPose().x() - openglCamera.x(),
+				cloud->getPose().y() - openglCamera.y(),
+				cloud->getPose().z() - openglCamera.z());
+		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
+
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
+	}
+
+	if(onlineBlending)
+	{
+		glDisable (GL_BLEND);
+		glDepthMask(GL_TRUE);
+	}
+
+	return (int)cloudsToDraw.size();
 }
 
 void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {
@@ -556,6 +565,24 @@ void Scene::SetCameraPose(const rtabmap::Transform & pose)
 	*currentPose_ = pose;
 }
 
+void Scene::setFOV(float angle)
+{
+	gesture_camera_->SetFieldOfView(angle);
+}
+void Scene::setOrthoCropFactor(float value)
+{
+	gesture_camera_->SetOrthoCropFactor(value);
+}
+void Scene::setGridRotation(float angleDeg)
+{
+	float angleRad = angleDeg * DEGREE_2_RADIANS;
+	if(grid_)
+	{
+		glm::quat rot = glm::rotate(glm::quat(1,0,0,0), angleRad, glm::vec3(0, 1, 0));
+		grid_->SetRotation(rot);
+	}
+}
+
 rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 {
 	if(fov)
@@ -563,14 +590,27 @@ rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 		*fov = gesture_camera_->getFOV();
 	}
 	return glmToTransform(gesture_camera_->GetTransformationMatrix());
-
 }
 
 void Scene::OnTouchEvent(int touch_count,
                          tango_gl::GestureCamera::TouchEvent event, float x0,
                          float y0, float x1, float y1) {
 	UASSERT(gesture_camera_ != 0);
-  gesture_camera_->OnTouchEvent(touch_count, event, x0, y0, x1, y1);
+	if(touch_count == 3)
+	{
+		//doubletap
+		if(!doubleTapOn_)
+		{
+			doubleTapPos_.x = x0;
+			doubleTapPos_.y = y0;
+			doubleTapOn_ = true;
+		}
+	}
+	else
+	{
+		// rotate/translate/zoom
+		gesture_camera_->OnTouchEvent(touch_count, event, x0, y0, x1, y1);
+	}
 }
 
 void Scene::updateGraph(
@@ -623,12 +663,7 @@ void Scene::addCloud(
 	}
 
 	//create
-	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(
-			cloud_shader_program_,
-			texture_mesh_shader_program_,
-			cloud,
-			indices);
+	PointCloudDrawable * drawable = new PointCloudDrawable(cloud, indices);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
 }
@@ -636,7 +671,8 @@ void Scene::addCloud(
 void Scene::addMesh(
 		int id,
 		const Mesh & mesh,
-		const rtabmap::Transform & pose)
+		const rtabmap::Transform & pose,
+		bool createWireframe)
 {
 	LOGI("add mesh %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
@@ -647,13 +683,61 @@ void Scene::addMesh(
 	}
 
 	//create
-	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(
-			cloud_shader_program_,
-			texture_mesh_shader_program_,
-			mesh);
+	PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
+
+	if(!mesh.pose.isNull() && mesh.cloud->size() && (!mesh.cloud->isOrganized() || mesh.indices->size()))
+	{
+		UTimer time;
+		float height = 0.0f;
+		Eigen::Affine3f affinePose = mesh.pose.toEigen3f();
+		if(mesh.polygons.size())
+		{
+			for(unsigned int i=0; i<mesh.polygons.size(); ++i)
+			{
+				for(unsigned int j=0; j<mesh.polygons[i].vertices.size(); ++j)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.polygons[i].vertices[j]), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+		else
+		{
+			if(mesh.cloud->isOrganized())
+			{
+				for(unsigned int i=0; i<mesh.indices->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.indices->at(i)), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+			else
+			{
+				for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(i), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+
+		if(grid_->GetPosition().y == kHeightOffset.y || grid_->GetPosition().y > height)
+		{
+			grid_->SetPosition(glm::vec3(0,height,0));
+		}
+		LOGD("compute min height %f s", time.ticks());
+	}
 }
 
 
@@ -722,3 +806,11 @@ void Scene::updateGains(int id, float gainR, float gainG, float gainB)
 		iter->second->setGains(gainR, gainG, gainB);
 	}
 }
+
+void Scene::setGridColor(float r, float g, float b)
+{
+	if(grid_)
+	{
+		grid_->SetColor(r, g, b);
+	}
+}

app/android/jni/scene.h

@@ -57,6 +57,8 @@ class Scene {
 
   // Setup GL view port.
   void SetupViewPort(int w, int h);
+  int getViewPortWidth() const {return screenWidth_;}
+  int getViewPortHeight() const {return screenHeight_;}
 
   void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 
@@ -78,7 +80,7 @@ class Scene {
 
   void SetCameraPose(const rtabmap::Transform & pose);
   rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
-  rtabmap::Transform GetOpenGLCameraPose(float * fov) const;
+  rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;
 
   // Touch event passed from android activity. This function only support two
   // touches.
@@ -108,7 +110,8 @@ class Scene {
   void addMesh(
   		int id,
   		const Mesh & mesh,
-  		const rtabmap::Transform & pose);
+  		const rtabmap::Transform & pose,
+		bool createWireframe = false);
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
@@ -120,19 +123,24 @@ class Scene {
   void updateMesh(int id, const Mesh & mesh);
   void updateGains(int id, float gainR, float gainG, float gainB);
 
+  void setBlending(bool enabled) {blending_ = enabled;}
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
-  void setFrustumCulling(bool enabled) {frustumCulling_ = enabled;}
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float angleDeg);
   void setLighting(bool enabled) {lighting_ = enabled;}
   void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
+  void setWireframe(bool enabled) {wireFrame_ = enabled;}
   void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
+  void setGridColor(float r, float g, float b);
 
+  bool isBlending() const {return blending_;}
   bool isMapRendering() const {return mapRendering_;}
   bool isMeshRendering() const {return meshRendering_;}
   bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
   float getPointSize() const {return pointSize_;}
-  bool isFrustumCulling() const {return frustumCulling_;}
   bool isLighting() const {return lighting_;}
   bool isBackfaceCulling() const {return backfaceCulling_;}
 
@@ -166,21 +174,26 @@ class Scene {
   rtabmap::Transform * currentPose_;
 
   // Shader to display point cloud.
-  GLuint cloud_shader_program_;
-  GLuint texture_mesh_shader_program_;
   GLuint graph_shader_program_;
 
+  bool blending_;
   bool mapRendering_;
   bool meshRendering_;
   bool meshRenderingTexture_;
   float pointSize_;
-  bool frustumCulling_;
   bool boundingBoxRendering_;
   bool lighting_;
   bool backfaceCulling_;
+  bool wireFrame_;
   float r_;
   float g_;
   float b_;
+  GLuint fboId_;
+  GLuint depthTexture_;
+  GLsizei screenWidth_;
+  GLsizei screenHeight_;
+  bool doubleTapOn_;
+  cv::Point2f doubleTapPos_;
 };
 
 #endif  // TANGO_POINT_CLOUD_SCENE_H_

app/android/jni/tango-gl/camera.cpp

@@ -22,8 +22,13 @@ namespace tango_gl {
 Camera::Camera() {
   field_of_view_ = 45.0f * DEGREE_2_RADIANS;
   aspect_ratio_ = 4.0f / 3.0f;
-  near_clip_plane_ = 0.1f;
-  far_clip_plane_ = 100.0f;
+  width_ = 800.0f;
+  height_ = 600.0f;
+  near_clip_plane_ = 0.2f;
+  far_clip_plane_ = 1000.0f;
+  ortho_ = false;
+  orthoScale_ = 2.0f;
+  orthoCropFactor_ = -1.0f;
 }
 
 glm::mat4 Camera::GetViewMatrix() {
@@ -31,18 +36,29 @@ glm::mat4 Camera::GetViewMatrix() {
 }
 
 glm::mat4 Camera::GetProjectionMatrix() {
-  return glm::perspective(field_of_view_, aspect_ratio_,
-                          near_clip_plane_, far_clip_plane_);
+	if(ortho_)
+	{
+		return glm::ortho(-orthoScale_*aspect_ratio_, orthoScale_*aspect_ratio_, -orthoScale_, orthoScale_, orthoScale_ + orthoCropFactor_, far_clip_plane_);
+	}
+  return glm::perspective(field_of_view_, aspect_ratio_, near_clip_plane_, far_clip_plane_);
 }
 
-void Camera::SetAspectRatio(float aspect_ratio) {
-  aspect_ratio_ = aspect_ratio;
+void Camera::SetWindowSize(float width, float height) {
+	width_ = width;
+	height_ = height;
+  aspect_ratio_ = width/height;
 }
 
 void Camera::SetFieldOfView(float fov) {
   field_of_view_ = fov * DEGREE_2_RADIANS;
 }
 
+void Camera::SetNearFarClipPlanes(const float near, const float far)
+{
+	near_clip_plane_ = near;
+	far_clip_plane_ = far;
+}
+
 Camera::~Camera() {
 }
 

app/android/jni/tango-gl/gesture_camera.cpp

@@ -62,68 +62,83 @@ GestureCamera::GestureCamera() :
 GestureCamera::~GestureCamera() { delete cam_parent_transform_; }
 
 void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
-                                 float y0, float x1, float y1) {
-  if (camera_type_ == kFirstPerson) {
-    return;
-  }
+		float y0, float x1, float y1) {
 
-  if (touch_count == 1) {
-    switch (event) {
-      case kTouch0Down: {
-        cam_start_angle_ = cam_cur_angle_;
+	if (camera_type_!=kFirstPerson && touch_count == 1) {
+		switch (event) {
+		case kTouch0Down: {
+			cam_start_angle_ = cam_cur_angle_;
 
-        touch0_start_position_.x = x0;
-        touch0_start_position_.y = y0;
-        break;
-      }
-      case kTouchMove: {
-        float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
-        float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
+			touch0_start_position_.x = x0;
+			touch0_start_position_.y = y0;
+			break;
+		}
+		case kTouchMove: {
+			glm::vec2 offset;
 
-        cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
-        cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
-        StartCameraToCurrentTransform();
-        break;
-      }
-      default: { break; }
-    }
-  }
-  if (touch_count == 2) {
-    switch (event) {
-      case kTouch1Down: {
-        float abs_x = x0 - x1;
-        float abs_y = y0 - y1;
-        start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
-        cam_start_dist_ = GetPosition().z;
+			float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
+			float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
 
-        // center touch
-        touch0_start_position_.x = (x0+x1)/2.0f;
-        touch0_start_position_.y = (y0+y1)/2.0f;
-        break;
-      }
-      case kTouchMove: {
-        float abs_x = x0 - x1;
-        float abs_y = y0 - y1;
-        float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
+			if(camera_type_!=kTopOrtho)
+				cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
+			cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
 
-        cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
-                                  kCamViewMinDist, kCamViewMaxDist);
+			StartCameraToCurrentTransform();
 
-        glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
-        glm::vec2 offset;
-        offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
-        offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
-        touch0_start_position_ = touch_center_position;
+			break;
+		}
+		default: { break; }
+		}
+	}
+	if (touch_count == 2) {
+		switch (event) {
+		case kTouch1Down: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
+			cam_start_dist_ = GetPosition().z;
+			cam_start_fov_ = this->getFOV();
 
-        StartCameraToCurrentTransform();
+			// center touch
+			touch0_start_position_.x = (x0+x1)/2.0f;
+			touch0_start_position_.y = (y0+y1)/2.0f;
+			break;
+		}
+		case kTouchMove: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
 
-        anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			if(camera_type_ == kFirstPerson)
+			{
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
+			}
+			else
+			{
+				cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+								kCamViewMinDist, kCamViewMaxDist);
 
-        break;
-      }
-      default: { break; }
-    }
-  }
+				this->SetOrthoMode(camera_type_ == kTopOrtho);
+				if(camera_type_ == kTopOrtho)
+				{
+					this->SetOrthoScale(cam_cur_dist_);
+				}
+
+				glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
+				glm::vec2 offset;
+				offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
+				offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
+				touch0_start_position_ = touch_center_position;
+
+				StartCameraToCurrentTransform();
+
+				anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			}
+			break;
+		}
+		default: { break; }
+		}
+	}
 }
 
 Segment GestureCamera::GetSegmentFromTouch(float normalized_x,
@@ -164,6 +179,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
   camera_type_ = camera_index;
   switch (camera_index) {
     case kFirstPerson:
+    	SetOrthoMode(false);
       SetFieldOfView(kLowFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
@@ -177,20 +193,22 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       break;
     case kThirdPerson:
     case kThirdPersonFollow:
-      SetFieldOfView(kHighFov);
+    	SetOrthoMode(false);
+    	SetFieldOfView(kHighFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 4.0f;
+      cam_cur_angle_.x = -M_PI / 6.0f;
       cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;
     case kTopDown:
-      SetFieldOfView(kHighFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      SetOrthoMode(false);
+      SetFieldOfView(kHighFov);
       cam_cur_dist_ = kTopDownCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
       cam_cur_angle_.x = -M_PI / 2.0f;
@@ -198,6 +216,19 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;
+    case kTopOrtho:
+	  SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+	  SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	  SetOrthoMode(true);
+	  SetOrthoScale(kTopDownCameraDist);
+	  SetOrthoCropFactor(-1.0f);
+	  cam_cur_dist_ = kTopDownCameraDist;
+	  anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+	  cam_cur_angle_.x = -M_PI / 2.0f;
+	  cam_cur_angle_.y = 0.0f;
+	  cam_cur_target_rot_ = glm::quat(1,0,0,0);
+	  StartCameraToCurrentTransform();
+	  break;
     default:
       break;
   }

app/android/jni/tango-gl/include/tango-gl/camera.h

@@ -27,11 +27,17 @@ class Camera : public Transform {
   Camera& operator=(const Camera&) = delete;
   ~Camera();
 
-  void SetAspectRatio(const float aspect_ratio);
+  void SetWindowSize(const float width, const float height);
   void SetFieldOfView(const float fov);
+  void SetOrthoMode(bool enabled) {ortho_ = enabled;}
+  void SetOrthoScale(float scale) {orthoScale_ = scale;}
+  void SetOrthoCropFactor(float value) {orthoCropFactor_ = value;}
+  void SetNearFarClipPlanes(const float near, const float far);
 
   glm::mat4 GetViewMatrix();
   glm::mat4 GetProjectionMatrix();
+  float getNearClipPlane() const {return near_clip_plane_;}
+  float getFarClipPlane() const {return far_clip_plane_;}
 
   /**
    * Create an OpenGL perspective matrix from window size, camera intrinsics, and clip settings.
@@ -52,7 +58,12 @@ class Camera : public Transform {
  protected:
   float field_of_view_;
   float aspect_ratio_;
+  float width_;
+  float height_;
   float near_clip_plane_, far_clip_plane_;
+  bool ortho_;
+  float orthoScale_;
+  float orthoCropFactor_;
 };
 }  // namespace tango_gl
 #endif  // TANGO_GL_CAMERA_H_

app/android/jni/tango-gl/include/tango-gl/gesture_camera.h

@@ -29,7 +29,8 @@ class GestureCamera : public Camera {
     kFirstPerson = 0,
     kThirdPersonFollow = 1,
     kTopDown = 2,
-    kThirdPerson = 3
+	kTopOrtho = 3,
+    kThirdPerson = 4
   };
 
   enum TouchEvent {
@@ -55,6 +56,11 @@ class GestureCamera : public Camera {
                               float touch_range);
 
   void SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation);
+  void SetAnchorOffset(const glm::vec3& pos) {anchor_offset_ = pos;}
+  const glm::vec3& GetAnchorOffset() const {return anchor_offset_;}
+
+  void SetCameraDistance(float cameraDistance) {cam_cur_dist_ = cameraDistance;}
+  float GetCameraDistance() const {return cam_cur_dist_;}
 
   // Set camera type, set render camera's parent position and rotation.
   void SetCameraType(CameraType camera_index);
@@ -75,6 +81,7 @@ class GestureCamera : public Camera {
   glm::quat cam_cur_target_rot_;
 
   float cam_start_dist_;
+  float cam_start_fov_;
   float cam_cur_dist_;
   glm::vec3 anchor_offset_;
 

app/android/jni/util.h

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

app/android/res/drawable/custom_seekbar.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<layer-list xmlns:android="http://schemas.android.com/apk/res/android" >
+    <item>
+        <shape>
+            <gradient
+                android:endColor="#00ffffff"
+                android:startColor="#ff686868"
+                android:useLevel="false" />
+        </shape>
+    </item>
+
+</layer-list>

app/android/res/layout/activity_rtabmap.xml

@@ -15,16 +15,21 @@
 -->
 <RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
     android:layout_width="wrap_content"
-    android:layout_height="wrap_content" >
+    android:layout_height="wrap_content">
 
     <android.opengl.GLSurfaceView
         android:id="@+id/gl_surface_view"
         android:layout_width="fill_parent"
         android:layout_height="fill_parent"
-        android:layout_gravity="top" />
+        android:layout_gravity="top"/>
+    
+    <RelativeLayout 
+        android:layout_width="wrap_content"
+        android:layout_height="match_parent"
+        android:fitsSystemWindows="true">
     
     <ToggleButton
-        android:id="@+id/backface_button"
+        android:id="@+id/wireframe_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
         android:layout_above="@+id/light_button"
@@ -33,67 +38,56 @@
         android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
-        android:textOff="@string/backface_off"
-        android:textOn="@string/backface_on" />
+        android:textOff="@string/wireframe"
+        android:textOn="@string/wireframe" />
     
     <ToggleButton
         android:id="@+id/light_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/first_person_button"
-        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_above="@+id/backface_button"
+        android:layout_alignLeft="@+id/backface_button"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="15dp"
+        android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
         android:textOff="@string/light_off"
         android:textOn="@string/light_on" />
     
     <ToggleButton
-        android:id="@+id/first_person_button"
+        android:id="@+id/backface_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/third_person_button"
-        android:layout_alignLeft="@+id/third_person_button"
+        android:layout_above="@+id/camera_button"
+        android:layout_alignRight="@+id/camera_button"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
+        android:layout_marginBottom="15dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
-        android:textOff="@string/first_person"
-        android:textOn="@string/first_person" />
+        android:textOff="@string/backface_off"
+        android:textOn="@string/backface_on" />
  
-    <ToggleButton
-        android:id="@+id/third_person_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/top_down_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/third_person"
-        android:textOn="@string/third_person" />
-
-    <ToggleButton
-        android:id="@+id/top_down_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
+    <com.introlab.rtabmap.NDSpinner
+        android:id="@+id/camera_button"
+        android:layout_width="140dp"
+        android:layout_height="40dp"
         android:layout_alignParentBottom="true"
         android:layout_alignParentRight="true"
         android:layout_marginRight="5dp"
+        android:layout_marginBottom="10dp"
         android:paddingRight="5dp"
-        android:textOff="@string/top_down"
-        android:textOn="@string/top_down" />
+        android:paddingBottom="10dp"
+        android:text="@string/camera_button"
+        android:spinnerMode="dropdown"/>
 
     <ToggleButton
         android:id="@+id/pause_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_alignLeft="@+id/first_person_button"
+		android:layout_marginRight="5dp"
         android:layout_alignParentTop="true"
-        android:layout_marginTop="61dp"
+        android:layout_marginTop="10dp"
         android:layout_alignParentRight="true"
-        android:paddingRight="5dp"
         android:textOff="@string/pause"
         android:textOn="@string/resume" />
 
@@ -101,26 +95,50 @@
         android:id="@+id/button_shareToSketchfab"
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
+        android:layout_marginRight="5dp"
         android:layout_alignParentTop="true"
-        android:layout_alignRight="@+id/pause_button"
+        android:layout_marginTop="10dp"
+        android:layout_alignParentRight="true"
         android:text="@string/share_to_sketchfab" />
 
     <Button
         android:id="@+id/button_saveOnDevice"
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
-        android:layout_toLeftOf="@+id/button_shareToSketchfab"
+        android:layout_alignRight="@+id/button_shareToSketchfab"
+        android:layout_below="@+id/button_shareToSketchfab"
         android:text="@string/save_to_file" />
 
     <Button
         android:id="@+id/close_visualization_button"
         android:layout_width="200dp"
         android:layout_height="wrap_content"
-        android:layout_alignBaseline="@+id/top_down_button"
-        android:layout_alignBottom="@+id/top_down_button"
+        android:layout_above="@+id/camera_button"
         android:layout_centerHorizontal="true"
-        android:paddingLeft="5dp"
         android:text="@string/close_visualization" />
 
+    <SeekBar
+        android:id="@+id/seekBar_ortho_cut"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:rotation="270"
+        android:layout_above="@+id/light_button"
+        android:layout_alignParentLeft="true"
+        android:layout_gravity="center"
+        android:layout_marginLeft="-50dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+    
+    <SeekBar
+        android:id="@+id/seekBar_grid"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentLeft="true"
+        android:layout_alignBottom="@+id/camera_button"
+        android:layout_marginLeft="35dp"
+        android:paddingBottom="10dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+
+
+
+</RelativeLayout>        
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -51,6 +51,18 @@
    			android:entries="@array/pref_rendering_texture_decimation_keys"
    			android:entryValues="@array/pref_rendering_texture_decimation_values"
    			android:defaultValue="@string/pref_default_rendering_texture_decimation"/>
+        <ListPreference 
+            android:key="@string/pref_key_background_color"
+    		android:title="@string/pref_title_background_color"
+    		android:summary="@string/pref_summary_background_color"
+   			android:entries="@array/pref_background_color_keys"
+   			android:entryValues="@array/pref_background_color_values"
+   			android:defaultValue="@string/pref_default_background_color"/>
+        <SwitchPreference
+            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
             android:key="@string/pref_key_nodes_filtering"
             android:title="@string/pref_title_nodes_filtering"
@@ -73,11 +85,6 @@
 	            android:summary="@string/pref_summary_append"
 	            android:defaultValue="@string/pref_default_append"/>
 		    <SwitchPreference
-	            android:key="@string/pref_key_auto_exposure"
-	            android:title="@string/pref_title_auto_exposure"
-	            android:summary="@string/pref_summary_auto_exposure"
-	            android:defaultValue="@string/pref_default_auto_exposure"/>
-		    <SwitchPreference
 	            android:key="@string/pref_key_resolution"
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
@@ -220,6 +227,14 @@
 	   			android:entryValues="@array/pref_texture_size_values"
 	   			android:defaultValue="@string/pref_default_texture_size"/>
 	        
+	        <ListPreference 
+	            android:key="@string/pref_key_texture_count"
+	    		android:title="@string/pref_title_texture_count"
+	    		android:summary="@string/pref_summary_texture_count"
+	   			android:entries="@array/pref_texture_count_keys"
+	   			android:entryValues="@array/pref_texture_count_values"
+	   			android:defaultValue="@string/pref_default_texture_count"/>
+	        
 	        <ListPreference 
 	            android:key="@string/pref_key_normal_k"
 	    		android:title="@string/pref_title_normal_k"
@@ -258,7 +273,7 @@
 		            android:key="@string/pref_key_opt_depth"
 		    		android:title="@string/pref_title_opt_depth"
 		    		android:summary="@string/pref_summary_opt_depth"
-		   			android:entries="@array/pref_opt_depth_values"
+		   			android:entries="@array/pref_opt_depth_keys"
 		   			android:entryValues="@array/pref_opt_depth_values"
 		   			android:defaultValue="@string/pref_default_opt_depth"/>
 		        
@@ -277,10 +292,17 @@
 		            android:summary="@string/pref_summary_opt_clean_white"
 		            android:defaultValue="@string/pref_default_opt_clean_white"/>
 		        
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_min_cluster_size"
+		    		android:title="@string/pref_title_opt_min_cluster_size"
+		    		android:summary="@string/pref_summary_opt_min_cluster_size"
+		   			android:entries="@array/pref_opt_min_cluster_size_keys"
+		   			android:entryValues="@array/pref_opt_min_cluster_size_values"
+		   			android:defaultValue="@string/pref_default_opt_min_cluster_size"/>
+		        
 		       </PreferenceCategory>
 	 	</PreferenceScreen>
-        
-        <ListPreference 
+	 	<ListPreference 
             android:key="@string/pref_key_gain_max_radius"
     		android:title="@string/pref_title_gain_max_radius"
     		android:summary="@string/pref_summary_gain_max_radius"
@@ -294,7 +316,20 @@
    			android:entries="@array/pref_cluster_ratio_keys"
    			android:entryValues="@array/pref_cluster_ratio_values"
    			android:defaultValue="@string/pref_default_cluster_ratio"/>
-        
+        <SwitchPreference 
+            android:key="@string/pref_key_notification_sound"
+    		android:title="@string/pref_title_notification_sound"
+    		android:summary="@string/pref_summary_notification_sound"
+   			android:defaultValue="@string/pref_default_notification_sound"/>
+    </PreferenceCategory>
+	<PreferenceCategory
+        android:title="@string/pref_title_presets">
+        <Preference android:title="@string/pref_title_open_button"
+	                android:key="@string/pref_key_open_button"/>
+        <Preference android:title="@string/pref_title_save_button"
+	                android:key="@string/pref_key_save_button"/>
+        <Preference android:title="@string/pref_title_remove_button"
+	                android:key="@string/pref_key_remove_button"/>
 	    <Preference android:title="@string/pref_title_reset_button"
 	                android:key="@string/pref_key_reset_button"/>
     </PreferenceCategory>

app/android/res/layout/database_list.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+android:layout_width="match_parent"
+android:layout_height="wrap_content"
+android:background="#fff">
+
+<ImageView
+android:id="@+id/imageView"
+android:layout_width="@dimen/image_width"
+android:layout_height="@dimen/image_width"
+android:layout_marginLeft="10dp"
+android:padding="5dp"
+android:src="@drawable/ic_launcher" />
+
+<TextView
+android:id="@+id/textView"
+android:layout_width="wrap_content"
+android:layout_height="wrap_content"
+android:layout_gravity="center"
+android:text="Demo"
+android:textColor="#000" />
+</LinearLayout>

app/android/res/menu/optionmenu.xml

@@ -17,12 +17,6 @@
                   	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
                   </menu>
               </item>
-              <item android:id="@+id/export_mesh_menu" android:title="Raw Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_mesh" android:title="Colored Mesh" />
-			          <item android:id="@+id/export_mesh_texture" android:title="Textured Mesh" />
-		          </menu>
-	          </item>
 	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
 	              <menu>
 		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />

app/android/res/values/arrays.xml

@@ -0,0 +1,9 @@
+
+    <resources>
+        <string-array name="camera_view_array">
+        <item>First View</item>
+        <item>Third-P. View</item>
+        <item>Top View</item>
+        <item>Ortho View</item>
+    </string-array>
+    </resources>

app/android/res/values/dimens.xml

@@ -0,0 +1,4 @@
+
+    <resources>
+        <dimen name="image_width">150dp</dimen>
+    </resources>

app/android/res/values/strings.xml

@@ -8,15 +8,14 @@
     <string name="sketchfab">Upload to Sketchfab&#8230;</string>
     <string name="status">"Status: "</string>
     <string name="words">"Words: "</string>
-    <string name="first_person">First</string>
-    <string name="third_person">Third</string>
-    <string name="top_down">Top</string>
+    <string name="camera_button">First View</string>
     <string name="pause">Pause</string>
     <string name="resume">Resume</string>
     <string name="backface_on">Backface</string>
     <string name="backface_off">Backface</string>
     <string name="light_on">Lighting</string>
     <string name="light_off">Lighting</string>
+    <string name="wireframe">Wireframe</string>
     <string name="close_visualization">Close Visualization</string>
     <string name="save_to_file">Export to File&#8230;</string>
     <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
@@ -41,6 +40,9 @@
     <string name="pref_default_tags">rtabmap 3dscan tango</string>
     <string name="pref_key_rendering">pref_key_rendering</string>
     <string name="pref_default_rendering">2</string>
+    <string name="pref_key_open_button">pref_key_open_button</string>
+    <string name="pref_key_save_button">pref_key_save_button</string>
+    <string name="pref_key_remove_button">pref_key_remove_button</string>
     <string name="pref_key_reset_button">pref_key_reset_button</string>
     <string name="pref_key_density">pref_key_density</string>
     <string name="pref_default_density">1</string>
@@ -56,12 +58,14 @@
     <string name="pref_default_triangle">2</string>
     <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
     <string name="pref_default_rendering_texture_decimation">4</string>
+    <string name="pref_key_blending">pref_key_blending</string>
+    <string name="pref_default_blending">true</string>
+    <string name="pref_key_background_color">pref_key_background_color</string>
+    <string name="pref_default_background_color">0.2</string>
     <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>
     <string name="pref_default_nodes_filtering">false</string>
     <string name="pref_key_append">pref_key_append</string>
     <string name="pref_default_append">true</string>
-    <string name="pref_key_auto_exposure">pref_key_auto_exposure</string>
-    <string name="pref_default_auto_exposure">true</string>
     <string name="pref_key_resolution">pref_key_resolution</string>
     <string name="pref_default_resolution">false</string>
     <string name="pref_key_smoothing">pref_key_smoothing</string>
@@ -101,9 +105,11 @@
     <string name="pref_default_db_in_memory">true</string>
     
     <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
-    <string name="pref_default_cloud_voxel">0</string>
+    <string name="pref_default_cloud_voxel">0.01</string>
     <string name="pref_key_texture_size">pref_key_texture_size</string>
     <string name="pref_default_texture_size">4096</string>
+    <string name="pref_key_texture_count">pref_key_texture_count</string>
+    <string name="pref_default_texture_count">1</string>
     <string name="pref_key_normal_k">pref_key_normal_k</string>
     <string name="pref_default_normal_k">18</string>
     <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
@@ -113,15 +119,19 @@
     <string name="pref_key_block_render">pref_key_block_render</string>
     <string name="pref_default_block_render">false</string>
     <string name="pref_key_opt_depth">pref_key_opt_depth</string>
-    <string name="pref_default_opt_depth">8</string>
+    <string name="pref_default_opt_depth">0</string>
     <string name="pref_key_opt_color_radius">pref_key_opt_color_radius</string>
     <string name="pref_default_opt_color_radius">0.05</string>
     <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>
     <string name="pref_default_opt_clean_white">true</string>
+    <string name="pref_key_opt_min_cluster_size">pref_key_opt_min_cluster_size</string>
+    <string name="pref_default_opt_min_cluster_size">0</string>
     <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
     <string name="pref_default_gain_max_radius">0.02</string>
     <string name="pref_key_cluster_ratio">pref_key_cluster_ratio</string>
     <string name="pref_default_cluster_ratio">0.05</string>
+    <string name="pref_key_notification_sound">pref_key_notification_sound</string>
+    <string name="pref_default_notification_sound">true</string>
     <!-- Preference keys: END -->
 
     <string name="pref_title_rendering">Rendering</string>
@@ -139,6 +149,10 @@
     <string name="pref_summary_depth">Points over the maximum depth are not rendered.</string>
     <string name="pref_title_point_size">Point Size</string>
     <string name="pref_summary_point_size">Size of the points when rendering only the point cloud.</string>
+    <string name="pref_title_blending">Blending</string>
+    <string name="pref_summary_blending">Blend close surfaces together to get more smooth colors on overlapping surfaces. May decrease rendering frame rate.</string>
+    <string name="pref_title_background_color">Background Color</string>
+    <string name="pref_summary_background_color"></string>
     <string name="pref_title_nodes_filtering">Nodes Filtering</string>
     <string name="pref_summary_nodes_filtering">Render only the newest point cloud of a loop closure.</string>
 
@@ -257,6 +271,32 @@
         <item>"4"</item>
         <item>"8"</item>
     </string-array>
+    <string-array name="pref_background_color_keys">
+        <item>"White"</item>
+        <item>"0.9"</item>
+        <item>"Light Gray"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"Gray"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"Dark Gray"</item>
+        <item>"0.1"</item>
+        <item>"Black"</item>
+    </string-array>
+    <string-array name="pref_background_color_values">
+        <item>"1.0"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.0"</item>
+    </string-array>
 
     <string name="pref_title_mapping_sub">Mapping&#8230;</string>
     <string name="pref_title_mapping">Mapping</string>
@@ -265,14 +305,12 @@
     <string name="pref_title_mapping_database">Database</string>
     <string name="pref_title_append">Append Mode</string>
     <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
-    <string name="pref_title_auto_exposure">Auto Exposure</string>
-    <string name="pref_summary_auto_exposure">Adjust camera exposure depending on the lighting to get always maximum contrast. This may change texture color between scanned images. Color correction option in Post-Processing can help to uniformize colors. May not work on some devices.</string>
     <string name="pref_title_resolution">HD Mode</string>
     <string name="pref_summary_resolution">Save HD images of the color camera if you want very detailed textures. More memory will be required.</string>
     <string name="pref_title_smoothing">Smoothing</string>
     <string name="pref_summary_smoothing">Smooth the point clouds.</string>
     <string name="pref_title_fisheye">Fish Eye Camera</string>
-    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. Cannot be used on Yellowstone tablet.</string>
+    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
     <string name="pref_title_update_rate">Update Rate</string>
     <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
     <string name="pref_title_time_thr">Time Limit</string>
@@ -532,6 +570,8 @@
     <string name="pref_summary_cloud_voxel">If you don\'t need a very precise point cloud, you can set this to reduce the output point cloud size. This is also used for optimized mesh.</string>
     <string name="pref_title_texture_size">Texture Size</string>
     <string name="pref_summary_texture_size">If the map is large, you may want to increase this to maximize the texture resolution.</string>
+    <string name="pref_title_texture_count">Maximum Output Textures</string>
+    <string name="pref_summary_texture_count">Note that all textures are still merged into one for visualization, but exported in multiple files.</string>
     <string name="pref_title_normal_k">Normal K</string>
     <string name="pref_summary_normal_k">K-nearest neighbors used for normal computation when a mesh is created.</string>
     <string name="pref_title_max_texture_distance">Max Texture Distance</string>
@@ -573,6 +613,26 @@
         <item>"2048"</item>
         <item>"1024"</item>
     </string-array>
+    <string-array name="pref_texture_count_keys">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_texture_count_values">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
     <string-array name="pref_normal_k_values">
         <item>"30"</item>
         <item>"24"</item>
@@ -601,35 +661,48 @@
         <item>"2"</item>
     </string-array>
     <string-array name="pref_min_texture_cluster_size_keys">
-        <item>"Disabled"</item>
         <item>"1000"</item>
         <item>"500"</item>
         <item>"200"</item>
         <item>"100"</item>
         <item>"50"</item>
         <item>"10"</item>
+        <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_min_texture_cluster_size_values">
-        <item>"0"</item>
         <item>"1000"</item>
         <item>"500"</item>
         <item>"200"</item>
         <item>"100"</item>
         <item>"50"</item>
         <item>"10"</item>
+        <item>"0"</item>
     </string-array>
     
     <string name="pref_title_optimized">Optimized</string>
     <string name="pref_title_opt_voxel">Voxel Size</string>
     <string name="pref_summary_opt_voxel">Increasing this can reduce reconstruction time at the cost of less geometry precision.</string>
     <string name="pref_title_opt_depth">Reconstruction Depth</string>
-    <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower.</string>
+    <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth). \"Auto\" means that depth is chosen so that polygon size is just under 3 cm.</string>
     <string name="pref_title_opt_color_radius">Color Radius</string>
     <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
     <string name="pref_title_opt_clean_white">Clean Mesh</string>
     <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
+	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>
+    <string name="pref_title_opt_min_cluster_size">Polygon Filtering</string>
     
+    <string-array name="pref_opt_depth_keys">
+        <item>"Auto"</item>
+        <item>"12"</item>
+        <item>"11"</item>
+        <item>"10"</item>
+        <item>"9"</item>
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+    </string-array>
     <string-array name="pref_opt_depth_values">
+        <item>"0"</item>
         <item>"12"</item>
         <item>"11"</item>
         <item>"10"</item>
@@ -666,12 +739,22 @@
         <item>"0.01"</item>
         <item>"-1"</item>
     </string-array>
+    <string-array name="pref_opt_min_cluster_size_keys">
+        <item>"Only biggest cluster kept"</item>
+        <item>"Keep all polygons"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+    </string-array>
 
     <string name="pref_title_general">General</string>
     <string name="pref_title_gain_max_radius">Color Correction Radius</string>
     <string name="pref_summary_gain_max_radius">Radius used to find pixel correspondences for Adjust Colors optimization.</string>
     <string name="pref_title_cluster_ratio">Noise Filtering Ratio</string>
     <string name="pref_summary_cluster_ratio">Polygon clusters with size smaller than this ratio of the largest cluster are removed by the Noise Filtering optimization.</string>
+    <string name="pref_title_notification_sound">Notification Sound</string>
+    <string name="pref_summary_notification_sound">After saving database or preparing data to export, a notification sound is played.</string>
 
     <string-array name="pref_gain_max_radius_keys">
         <item>"0.3 m"</item>
@@ -722,6 +805,10 @@
         <item>"0.01"</item>
     </string-array>
 
+    <string name="pref_title_presets">Presets</string>
+    <string name="pref_title_open_button">Open</string>
+    <string name="pref_title_save_button">Save</string>
+    <string name="pref_title_remove_button">Remove</string>
     <string name="pref_title_reset_button">Restore All Default Settings</string>
     <string name="title_activity_sketchfab">SketchfabActivity</string>
     <string name="hello_world">Hello world!</string>

app/android/res/values/styles.xml

@@ -0,0 +1,10 @@
+<resources>
+    <style name="ThemeActionBar" parent="@android:style/Widget.DeviceDefault.ActionBar.Solid">
+        <item name="android:background">#20000000</item>
+    </style>
+
+    <style name="ThemeApp" parent="@android:style/Theme.DeviceDefault">
+        <item name="android:actionBarStyle">@style/ThemeActionBar</item>
+        <item name="android:windowActionBarOverlay">true</item>
+    </style>
+</resources>

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

@@ -0,0 +1,102 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.database.Cursor;
+import android.database.sqlite.SQLiteDatabase;
+import android.graphics.Bitmap;
+import android.graphics.BitmapFactory;
+import android.graphics.drawable.Drawable;
+import android.util.Log;
+import android.view.LayoutInflater;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.BaseAdapter;
+import android.widget.ImageView;
+import android.widget.LinearLayout;
+import android.widget.SimpleAdapter;
+
+import java.io.ByteArrayInputStream;
+import java.io.InputStream;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+public class DatabaseListArrayAdapter extends SimpleAdapter {
+	LayoutInflater inflater;
+	Context context;
+	ArrayList<HashMap<String, String>> arrayList;
+	int imageWidth;
+
+	public DatabaseListArrayAdapter(Context context, ArrayList<HashMap<String, String>> data, int resource, String[] from, int[] to) {
+		super(context, data, resource, from, to);
+		this.context = context;
+		this.arrayList = data;
+		this.imageWidth = (int)context.getResources().getDimension(R.dimen.image_width);
+		inflater.from(context);
+	}
+
+	@Override
+	public View getView(final int position, View convertView, ViewGroup parent) {
+		View view = super.getView(position, convertView, parent);
+		ImageView imageView = (ImageView) view.findViewById(R.id.imageView);
+		
+		boolean imageSet = false;
+		String path = this.arrayList.get(position).get("path");
+		if(!path.isEmpty())
+		{
+			SQLiteDatabase db = null;
+			try {
+				db = SQLiteDatabase.openDatabase(path, null, SQLiteDatabase.OPEN_READONLY);
+				
+				// get version
+				Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);    
+		        if(c1.moveToFirst()) {
+		        	String version = c1.getString(c1.getColumnIndex("version"));
+		        	Log.i(RTABMapActivity.TAG, "Version="+version);
+		        	if(Util.versionCompare(version, "0.12.0") >= 0) {
+		        		Cursor c2 = db.rawQuery("SELECT preview_image FROM Admin WHERE preview_image is not null", null); 
+		        		if(c2.moveToFirst()) {
+		        			Log.i(RTABMapActivity.TAG, "Found image preview for db " + path);
+
+		        			byte[] bytes = c2.getBlob(c2.getColumnIndex("preview_image"));
+		        			ByteArrayInputStream inputStream = new ByteArrayInputStream(bytes);
+		        			Bitmap bitmap = BitmapFactory.decodeStream(inputStream);
+		        			imageView.setImageBitmap(bitmap);
+		        			imageSet = true;
+		        		}
+		        		else {
+		        			Log.i(RTABMapActivity.TAG, "Not found image preview for db " + path);
+		        		}
+		        	}
+		        	else {
+		        		Log.i(RTABMapActivity.TAG, "Too old database for preview image, path = " + path);
+		        	}
+		        }
+		        else {
+		        	Log.e(RTABMapActivity.TAG, "Failed getting version from database");
+		        }
+	
+	        } catch (Exception e) {
+	            Log.e(RTABMapActivity.TAG, e.getMessage());
+	        }
+			finally {
+				if(db != null && db.isOpen()) {
+	            	db.close();
+	            }
+			}
+		}
+		else
+		{
+			Log.e(RTABMapActivity.TAG, "Database path empty for item " + position);
+		}
+		
+		if(!imageSet)
+		{
+			Drawable myDrawable = context.getResources().getDrawable(R.drawable.ic_launcher);
+			imageView.setImageDrawable(myDrawable);
+		}
+		LinearLayout.LayoutParams layoutParams = new LinearLayout.LayoutParams(imageWidth,imageWidth/(!imageSet?2:1));
+		imageView.setLayoutParams(layoutParams);
+		return view;
+	}
+
+}

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

@@ -0,0 +1,43 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.util.AttributeSet;
+import android.widget.Spinner;
+
+
+/** Spinner extension that calls onItemSelected even when the selection is the same as its previous value 
+Author: Mattia Ruggiero 
+https://stackoverflow.com/questions/5335306/how-can-i-get-an-event-in-android-spinner-when-the-current-selected-item-is-sele
+*/
+public class NDSpinner extends Spinner {
+
+    public NDSpinner(Context context)
+    { super(context); }
+
+    public NDSpinner(Context context, AttributeSet attrs)
+    { super(context, attrs); }
+
+    public NDSpinner(Context context, AttributeSet attrs, int defStyle)
+    { super(context, attrs, defStyle); }
+
+    @Override 
+    public void setSelection(int position, boolean animate) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position, animate);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            //getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    } 
+
+    @Override
+    public void setSelection(int position) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    }
+
+}

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

@@ -28,6 +28,7 @@ public class RTABMapLib
     public static native void setScreenRotation(int displayRotation, int cameraRotation);
     
     public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase2(String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
     
     /*
      * Called when the Tango service is connected.
@@ -58,6 +59,7 @@ public class RTABMapLib
     
 
     public static native void setPausedMapping(boolean paused);
+    public static native void setOnlineBlending(boolean enabled);
     public static native void setMapCloudShown(boolean shown);
     public static native void setOdomCloudShown(boolean shown);
     public static native void setMeshRendering(boolean enabled, boolean withTexture);
@@ -67,7 +69,6 @@ public class RTABMapLib
     public static native void setNodesFiltering(boolean enabled);
     public static native void setGraphVisible(boolean visible);
     public static native void setGridVisible(boolean visible);
-    public static native void setAutoExposure(boolean enabled);
     public static native void setRawScanSaved(boolean enabled);
     public static native void setFullResolution(boolean enabled);
     public static native void setSmoothing(boolean enabled);
@@ -77,25 +78,30 @@ public class RTABMapLib
     public static native void setMaxCloudDepth(float value);
     public static native void setMinCloudDepth(float value);
     public static native void setPointSize(float value);
+    public static native void setFOV(float value);
+    public static native void setOrthoCropFactor(float value);
+    public static native void setGridRotation(float value);
     public static native void setLighting(boolean enabled);
     public static native void setBackfaceCulling(boolean enabled);
+    public static native void setWireframe(boolean enabled);
     public static native void setCloudDensityLevel(int value);
     public static native void setMeshAngleTolerance(float value);
     public static native void setMeshTriangleSize(int value);
     public static native void setClusterRatio(float value);
     public static native void setMaxGainRadius(float value);
     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 resetMapping();
     public static native void save(String outputDatabasePath);
     public static native void cancelProcessing();
     public static native boolean exportMesh(
-    		String filePath,
     		float cloudVoxelSize,
     		boolean regenerateCloud,
     		boolean meshing,
     		int textureSize,
+    		int textureCount,
     		int normalK,
     		boolean optimized,
     		float optimizedVoxelSize,
@@ -103,10 +109,11 @@ public class RTABMapLib
     		int optimizedMaxPolygons,
     		float optimizedColorRadius,
     		boolean optimizedCleanWhitePolygons,
-    		boolean optimizedColorWhitePolygons,
+    		int optimizedMinClusterSize,
     		float optimizedMaxTextureDistance,
     		int optimizedMinTextureClusterSize,
     		boolean blockRendering);
+    public static native boolean writeExportedMesh(String directory, String name);
     public static native boolean postExportation(boolean visualize);
     public static native int postProcessing(int approach);
     

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

@@ -41,6 +41,8 @@ public class Renderer implements GLSurfaceView.Renderer {
 
 	private TextManager mTextManager = null;
 	private float mSurfaceHeight = 0.0f;
+	private float mTextColor = 1.0f;
+	private int mOffset = 0;
 	
 	private Vector<TextObject> mTexts;
 
@@ -65,6 +67,11 @@ public class Renderer implements GLSurfaceView.Renderer {
 		mToast = toast;
 	}
 	
+	public void setOffset(int offset)
+	{
+		mOffset = offset;
+	}
+
 	// Render loop of the Gl context.
 	public void onDrawFrame(GL10 useGLES20instead) {
 
@@ -93,7 +100,9 @@ public class Renderer implements GLSurfaceView.Renderer {
 					mTextManager.PrepareDraw(txtcollection);
 				}
 				
-				mTextManager.Draw(mtrxProjectionAndView);
+				float[] mvp = new float[16];
+				Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
+				mTextManager.Draw(mvp);
 			}
 
 			if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
@@ -102,6 +111,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 					public void run() {
 						if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
 						mProgressDialog.dismiss();
+						mActivity.resetNoTouchTimer();
 					}
 				});
 			}
@@ -174,9 +184,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 		
 		// Create our text manager
 		mTextManager = new TextManager(mActivity);
-		
-		GLES20.glEnable(GLES20.GL_BLEND);
-	    GLES20.glBlendFunc(GLES20.GL_ONE, GLES20.GL_ONE_MINUS_SRC_ALPHA);
+		mTextManager.setColor(mTextColor);
 	}
 	
 	public void updateTexts(String[] texts)
@@ -208,4 +216,13 @@ public class Renderer implements GLSurfaceView.Renderer {
 			mTextChanged = true;
 		}
 	}
+	
+	public void setTextColor(float color) 
+	{
+		mTextColor = color;
+		if(mTextManager != null)
+		{
+			mTextManager.setColor(mTextColor);
+		}
+	}
 }

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

@@ -1,23 +1,36 @@
 package com.introlab.rtabmap;
 
-import android.app.Activity;
+import java.io.File;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Iterator;
+import java.util.Map.Entry;
+
+import android.app.AlertDialog;
+import android.content.DialogInterface;
 import android.content.SharedPreferences;
 import android.content.SharedPreferences.OnSharedPreferenceChangeListener;
 import android.os.Bundle;
 import android.preference.ListPreference;
 import android.preference.Preference;
 import android.preference.PreferenceActivity;
-import android.preference.PreferenceManager;
+import android.text.InputType;
+import android.view.WindowManager;
+import android.view.inputmethod.EditorInfo;
+import android.widget.EditText;
 
 public class SettingsActivity extends PreferenceActivity implements OnSharedPreferenceChangeListener {
 	
+	private SettingsActivity getActivity() {return this;}
+	
     @Override
     public void onCreate(Bundle savedInstanceState) {
         super.onCreate(savedInstanceState);
         addPreferencesFromResource(R.layout.activity_settings);
         
-        Preference button = findPreference(getString(R.string.pref_key_reset_button));
-        button.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        Preference buttonReset = findPreference(getString(R.string.pref_key_reset_button));
+        buttonReset.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
         	@Override
         	public boolean onPreferenceClick(Preference preference) {   
         		getPreferenceScreen().getSharedPreferences().edit().clear().commit();
@@ -28,12 +41,157 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	}
         });
         
+        Preference buttonOpen = findPreference(getString(R.string.pref_key_open_button));
+        buttonOpen.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Choose Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						//sp1 is the shared pref to copy to
+        						SharedPreferences.Editor ed = getPreferenceScreen().getSharedPreferences().edit(); 
+        						SharedPreferences sp = getActivity().getSharedPreferences(files[which], MODE_PRIVATE); //The shared preferences to copy from
+        						//Cycle through all the entries in the sp
+        						for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+        						 Object v = entry.getValue(); 
+        						 String key = entry.getKey();
+        						 //Now we just figure out what type it is, so we can copy it.
+        						 // Note that i am using Boolean and Integer instead of boolean and int.
+        						 // That's because the Entry class can only hold objects and int and boolean are primatives.
+        						 if(v instanceof Boolean) 
+        						 // Also note that i have to cast the object to a Boolean 
+        						 // and then use .booleanValue to get the boolean
+        						    ed.putBoolean(key, ((Boolean)v).booleanValue());
+        						 else if(v instanceof Float)
+        						    ed.putFloat(key, ((Float)v).floatValue());
+        						 else if(v instanceof Integer)
+        						    ed.putInt(key, ((Integer)v).intValue());
+        						 else if(v instanceof Long)
+        						    ed.putLong(key, ((Long)v).longValue());
+        						 else if(v instanceof String)
+        						    ed.putString(key, ((String)v));         
+        						}
+        						ed.commit(); //save it.
+        						recreate();
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonSave = findPreference(getString(R.string.pref_key_save_button));
+        buttonSave.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+    			builder.setTitle("Save Presets:");
+    			final EditText input = new EditText(getActivity());
+    			input.setInputType(InputType.TYPE_CLASS_TEXT); 
+    			input.setImeOptions(EditorInfo.IME_FLAG_NO_EXTRACT_UI);
+    			builder.setView(input);
+    			builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+    				@Override
+    				public void onClick(DialogInterface dialog, int which)
+    				{
+    					final String fileName = input.getText().toString();  
+    					dialog.dismiss();
+    					if(!fileName.isEmpty())
+    					{
+    						File newFile = new File(getApplicationInfo().dataDir + "/shared_prefs/" + fileName + ".xml");
+    						if(newFile.exists())
+    						{
+    							new AlertDialog.Builder(getActivity())
+    							.setTitle("Presets Already Exist")
+    							.setMessage("Do you want to overwrite the existing file?")
+    							.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									saveConfig(fileName);
+    								}
+    							})
+    							.setNegativeButton("No", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									dialog.dismiss();
+    								}
+    							})
+    							.show();
+    						}
+    						else
+    						{
+    							saveConfig(fileName);
+    						}
+    					}
+    				}
+    			});
+    			AlertDialog alertToShow = builder.create();
+    			alertToShow.getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_VISIBLE);
+    			alertToShow.show();
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonRemove = findPreference(getString(R.string.pref_key_remove_button));
+        buttonRemove.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Remove Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						File file = new File(getApplicationInfo().dataDir + "/shared_prefs/" + files[which] + ".xml");
+        						if(file.exists())
+        						{
+        							file.delete();
+        						}
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        
         ((Preference)findPreference(getString(R.string.pref_key_density))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_density))).getEntry() + ") "+getString(R.string.pref_summary_density));
         ((Preference)findPreference(getString(R.string.pref_key_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth))).getEntry() + ") "+getString(R.string.pref_summary_depth));
         ((Preference)findPreference(getString(R.string.pref_key_min_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getEntry() + ") "+getString(R.string.pref_summary_min_depth));
         ((Preference)findPreference(getString(R.string.pref_key_point_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_point_size))).getEntry() + ") "+getString(R.string.pref_summary_point_size));
         ((Preference)findPreference(getString(R.string.pref_key_angle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_angle))).getEntry() + ") "+getString(R.string.pref_summary_angle));
         ((Preference)findPreference(getString(R.string.pref_key_triangle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_triangle))).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         ((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));
@@ -50,12 +208,14 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         
         ((Preference)findPreference(getString(R.string.pref_key_cloud_voxel))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cloud_voxel))).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         ((Preference)findPreference(getString(R.string.pref_key_texture_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_size))).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
+        ((Preference)findPreference(getString(R.string.pref_key_texture_count))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_count))).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
         ((Preference)findPreference(getString(R.string.pref_key_normal_k))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_normal_k))).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
         ((Preference)findPreference(getString(R.string.pref_key_max_texture_distance))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_texture_distance))).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
         ((Preference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));
 
         ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         ((Preference)findPreference(getString(R.string.pref_key_opt_color_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_color_radius))).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        ((Preference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
 
         ((Preference)findPreference(getString(R.string.pref_key_cluster_ratio))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cluster_ratio))).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         ((Preference)findPreference(getString(R.string.pref_key_gain_max_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_gain_max_radius))).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
@@ -89,6 +249,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_point_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_point_size));
         	if(key.compareTo(getString(R.string.pref_key_angle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_angle));
         	if(key.compareTo(getString(R.string.pref_key_triangle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         	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));
@@ -105,12 +266,14 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	
         	if(key.compareTo(getString(R.string.pref_key_cloud_voxel))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         	if(key.compareTo(getString(R.string.pref_key_texture_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
+        	if(key.compareTo(getString(R.string.pref_key_texture_count))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
         	if(key.compareTo(getString(R.string.pref_key_normal_k))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
         	if(key.compareTo(getString(R.string.pref_key_max_texture_distance))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
         	if(key.compareTo(getString(R.string.pref_key_min_texture_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));
 
         	if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         	if(key.compareTo(getString(R.string.pref_key_opt_color_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        	if(key.compareTo(getString(R.string.pref_key_opt_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
         	
         	if(key.compareTo(getString(R.string.pref_key_cluster_ratio))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         	if(key.compareTo(getString(R.string.pref_key_gain_max_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
@@ -133,4 +296,33 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         getPreferenceScreen().getSharedPreferences()
                 .unregisterOnSharedPreferenceChangeListener(this);
     }
+    
+    private void saveConfig(String fileName)
+    {
+    	//sp1 is the shared pref to copy to
+		SharedPreferences.Editor ed = getActivity().getSharedPreferences(fileName, MODE_PRIVATE).edit(); 
+		SharedPreferences sp = getPreferenceScreen().getSharedPreferences(); //The shared preferences to copy from
+		ed.clear(); // This clears the one we are copying to, but you don't necessarily need to do that.
+		//Cycle through all the entries in the sp
+		for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+		 Object v = entry.getValue(); 
+		 String key = entry.getKey();
+		 //Now we just figure out what type it is, so we can copy it.
+		 // Note that i am using Boolean and Integer instead of boolean and int.
+		 // That's because the Entry class can only hold objects and int and boolean are primatives.
+		 if(v instanceof Boolean) 
+		 // Also note that i have to cast the object to a Boolean 
+		 // and then use .booleanValue to get the boolean
+		    ed.putBoolean(key, ((Boolean)v).booleanValue());
+		 else if(v instanceof Float)
+		    ed.putFloat(key, ((Float)v).floatValue());
+		 else if(v instanceof Integer)
+		    ed.putInt(key, ((Integer)v).intValue());
+		 else if(v instanceof Long)
+		    ed.putLong(key, ((Long)v).longValue());
+		 else if(v instanceof String)
+		    ed.putString(key, ((String)v));         
+		}
+		ed.commit(); //save it.
+    }
 }

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

@@ -44,7 +44,6 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	private Dialog mAuthDialog;
 	
 	private String mAuthToken;
-	private boolean mExportedOBJ;
 	private String mWorkingDirectory;
 	
 	EditText mFilename;
@@ -70,7 +69,6 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		mProgressDialog.setCanceledOnTouchOutside(false);
 		
 		mAuthToken = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY);
-		mExportedOBJ = getIntent().getExtras().getBoolean(RTABMapActivity.RTABMAP_EXPORTED_OBJ_KEY);
 		mFilename.setText(getIntent().getExtras().getString(RTABMapActivity.RTABMAP_FILENAME_KEY));
 		mWorkingDirectory = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_WORKING_DIR_KEY);
 		
@@ -129,51 +127,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 			editor.commit();
 		}
 		
-		final String extension  = mExportedOBJ?".obj":".ply";
-		
-		String[] files = new String[0];
-		// verify if we have all files
-		if(extension.compareTo(".obj") == 0)
-		{
-			File objFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".obj");
-			File mltFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".mtl");
-			File jpgFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".jpg");
-			if(objFile.exists() && mltFile.exists() && jpgFile.exists())
-			{
-				files = new String[3];
-				files[0] = objFile.getAbsolutePath();
-				files[1] = mltFile.getAbsolutePath();
-				files[2] = jpgFile.getAbsolutePath();
-			}
-			else
-			{
-				Toast.makeText(getActivity(), String.format("Missing OBJ files!"), Toast.LENGTH_LONG).show();
-			}
-		}
-		else if(extension.compareTo(".ply") == 0)
-		{
-			File plyFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + extension);
-			if(plyFile.exists())
-			{
-				files = new String[1];
-				files[0] = plyFile.getAbsolutePath();
-			}
-			else
-			{
-				Toast.makeText(getActivity(), String.format("Missing PLY file!"), Toast.LENGTH_LONG).show();
-			}
-		}
-		else
-		{
-			Toast.makeText(getActivity(), String.format("Unknown file extension \"%s\"!", extension), Toast.LENGTH_LONG).show();
-		}
-
-		if(files.length > 0)
-		{
-			final String[] filesToZip = files;
-			authorizeAndPublish(filesToZip, mFilename.getText().toString());
-		}
-	
+		authorizeAndPublish(mFilename.getText().toString());	
 	}
 	
 	private boolean isNetworkAvailable() {
@@ -183,7 +137,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	    return activeNetworkInfo != null && activeNetworkInfo.isConnected();
 	}
 	
-	private void authorizeAndPublish(final String[] filesToZip, final String fileName)
+	private void authorizeAndPublish(final String fileName)
 	{
 		if(!isNetworkAvailable())
 		{
@@ -193,7 +147,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 			.setMessage("Network is not available. Make sure you have internet before continuing.")
 			.setPositiveButton("Try Again", new DialogInterface.OnClickListener() {
 				public void onClick(DialogInterface dialog, int which) {
-					authorizeAndPublish(filesToZip, fileName);
+					authorizeAndPublish(fileName);
 				}
 			})
 			.setNeutralButton("Abort", new DialogInterface.OnClickListener() {
@@ -240,7 +194,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 						
 						mAuthDialog.dismiss();
 
-						zipAndPublish(filesToZip, fileName);
+						zipAndPublish(fileName);
 					}
 				}
 			});
@@ -251,14 +205,12 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		}
 		else
 		{
-			zipAndPublish(filesToZip, fileName);
+			zipAndPublish(fileName);
 		}
 	}
 
-	private void zipAndPublish(final String[] filesToZip, final String fileName)
+	private void zipAndPublish(final String fileName)
 	{		
-		final String zipOutput = mWorkingDirectory+fileName+".zip";
-
 		mProgressDialog.setTitle("Upload to Sketchfab");
 		mProgressDialog.setMessage(String.format("Compressing the files..."));
 		mProgressDialog.show();
@@ -266,56 +218,110 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		Thread workingThread = new Thread(new Runnable() {
 			public void run() {
 				try{
-					Util.zip(filesToZip, zipOutput);
-					runOnUiThread(new Runnable() {
-						public void run() {
-							mProgressDialog.dismiss();
 					
-							File f = new File(zipOutput);
+					File tmpDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR);
+					tmpDir.mkdirs();
+					String[] fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+					if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR));
+					for(int i=0; i<fileNames.length; ++i)
+					{
+						File f = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i]);
+						if(f.delete())
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleted \"%s\"", f.getPath()));
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Failed deleting \"%s\"", f.getPath()));
+						}
+					}
+					File exportDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_EXPORT_DIR);
+					exportDir.mkdirs();
 					
-							// Continue?
-							AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
-							builder.setTitle("File(s) compressed and ready to upload!");
+					if(RTABMapLib.writeExportedMesh(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
+					{
+						String[] files = new String[0];
+						// verify if we have all files
 				
-							final int fileSizeMB = (int)f.length()/(1024 * 1024);
-							final int fileSizeKB = (int)f.length()/(1024);
-							if(fileSizeMB == 0)
+						fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+						if(fileNames.length > 0)
+						{
+							files = new String[fileNames.length];
+							for(int i=0; i<fileNames.length; ++i)
 							{
-								Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d KB", fileSizeKB));
-								builder.setMessage(String.format("Total size to upload = %d KB. Do you want to continue?\n\n", fileSizeKB));
-							}
-							else
-							{
-								Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d MB", fileSizeMB));
-								builder.setMessage(String.format("Total size to upload = %d MB. %sDo you want to continue?\n\n"
-										+ "Tip: To reduce the model size, you can also look at the Settings->Exporting options.", fileSizeMB,
-										fileSizeMB>=50?"Note that for size over 50 MB, a Sketchfab PRO account is required, otherwise the upload may fail. ":""));
+								files[i] = mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i];
 							}
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, "Missing files!");
+						}
 				
+						if(files.length > 0)
+						{
+							final String[] filesToZip = files;
+							final String zipOutput = mWorkingDirectory+fileName+".zip";
+							Util.zip(filesToZip, zipOutput);
+							runOnUiThread(new Runnable() {
+								public void run() {
+									mProgressDialog.dismiss();
 
-							builder.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-								public void onClick(DialogInterface dialog, int which) {
-									mProgressDialog.setTitle("Upload to Sketchfab");
+									File f = new File(zipOutput);
+
+									// Continue?
+									AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+									builder.setTitle("File(s) compressed and ready to upload!");
+									
+									final int fileSizeMB = (int)f.length()/(1024 * 1024);
+									final int fileSizeKB = (int)f.length()/(1024);
 									if(fileSizeMB == 0)
 									{
-										mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d KB) to Sketchfab...", fileName, fileSizeKB));
+										Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d KB", fileSizeKB));
+										builder.setMessage(String.format("Total size to upload = %d KB. Do you want to continue?\n\n", fileSizeKB));
 									}
 									else
 									{
-										mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d MB) to Sketchfab...", fileName, fileSizeMB));
+										Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d MB", fileSizeMB));
+										builder.setMessage(String.format("Total size to upload = %d MB. %sDo you want to continue?\n\n"
+												+ "Tip: To reduce the model size, you can also look at the Settings->Exporting options.", fileSizeMB,
+												fileSizeMB>=50?"Note that for size over 50 MB, a Sketchfab PRO account is required, otherwise the upload may fail. ":""));
 									}
-									mProgressDialog.show();
-									new uploadToSketchfabTask().execute(zipOutput, fileName);
+
+									
+									builder.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+										public void onClick(DialogInterface dialog, int which) {
+											mProgressDialog.setTitle("Upload to Sketchfab");
+											if(fileSizeMB == 0)
+											{
+												mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d KB) to Sketchfab...", fileName, fileSizeKB));
+											}
+											else
+											{
+												mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d MB) to Sketchfab...", fileName, fileSizeMB));
+											}
+											mProgressDialog.show();
+											new uploadToSketchfabTask().execute(zipOutput, fileName);
+										}
+									});
+									builder.setNegativeButton("No", new DialogInterface.OnClickListener() {
+										public void onClick(DialogInterface dialog, int which) {
+											// do nothing...
+										}
+									});
+									builder.show();
 								}
 							});
-							builder.setNegativeButton("No", new DialogInterface.OnClickListener() {
-								public void onClick(DialogInterface dialog, int which) {
-									// do nothing...
-								}
-							});
-							builder.show();
 						}
-					});
+					}
+					else
+					{
+						runOnUiThread(new Runnable() {
+							public void run() {
+								mProgressDialog.dismiss();
+								Toast.makeText(getActivity(), String.format("Failed writing files!"), Toast.LENGTH_LONG).show();
+							}
+						});
+					}
 				}
 				catch(IOException ex) {
 					Log.e(RTABMapActivity.TAG, "Failed to zip", ex);

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

@@ -29,23 +29,20 @@ public class TextManager {
 	public static final String vs_Text =
 		"uniform mat4 uMVPMatrix;" +
 		"attribute vec4 vPosition;" +
-		"attribute vec4 a_Color;" +
 		"attribute vec2 a_texCoord;" +
-		"varying vec4 v_Color;" + 
 		"varying vec2 v_texCoord;" +
 	    "void main() {" +
 	    "  gl_Position = uMVPMatrix * vPosition;" +
 	    "  v_texCoord = a_texCoord;" +
-	    "  v_Color = a_Color;" + 
 	    "}";
 	public static final String fs_Text =
 	    "precision mediump float;" +
-	    "varying vec4 v_Color;" +
+	    "uniform float uColor;" +
 	    "varying vec2 v_texCoord;" +
         "uniform sampler2D s_texture;" +
 	    "void main() {" +
-	    "  gl_FragColor = texture2D( s_texture, v_texCoord ) * v_Color;" +
-	    "  gl_FragColor.rgb *= v_Color.a;" +
+	    "  gl_FragColor = texture2D( s_texture, v_texCoord );" +
+	    "  gl_FragColor.rgb *= uColor;" +
 	    "}";
 	
 	public static int sp_Text;
@@ -60,21 +57,19 @@ public class TextManager {
 	private float mUVWidth;
 	private float mUVHeight;
 	private float mTextHeight;
+	private float mColor;
 	
 	private FloatBuffer vertexBuffer;
 	private FloatBuffer textureBuffer;
-	private FloatBuffer colorBuffer;
 	private ShortBuffer drawListBuffer;
 	
 	private float[] vecs;
 	private float[] uvs;
 	private short[] indices;
-	private float[] colors;
 	
 	private int index_vecs;
 	private int index_indices;
 	private int index_uvs;
-	private int index_colors;
 
 	private int texturenr;
 	private int[] mTextures;
@@ -87,7 +82,6 @@ public class TextManager {
 	{
 		// Create the arrays
 		vecs = new float[3 * 10];
-		colors = new float[4 * 10];
 		uvs = new float[2 * 10];
 		indices = new short[10];
 		
@@ -133,6 +127,7 @@ public class TextManager {
 		}
 		mUVWidth = (float)RI_TEXT_HEIGHT_BASE/(float)RI_TEXT_TEXTURE_SIZE;
 		mUVHeight = mTextHeight/(float)RI_TEXT_TEXTURE_SIZE;
+		mColor = 1.0f;
 		
 		int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
 		mCharacterWidth = new float[RI_TEXT_STOP-RI_TEXT_START];
@@ -190,13 +185,6 @@ public class TextManager {
 			index_vecs++;
 		}
 		
-		// We should add the colors, so we can use the same texture for multiple effects.
-		for(int i=0;i<cs.length;i++)
-		{
-			colors[index_colors] = cs[i];
-			index_colors++;
-		}
-		
 		// We should add the uvs
 		for(int i=0;i<uv.length;i++)
 		{
@@ -218,7 +206,6 @@ public class TextManager {
 		index_vecs = 0;
 		index_indices = 0;
 		index_uvs = 0;
-		index_colors = 0;
 		
 		// Get the total amount of characters
 		int charcount = 0;
@@ -234,12 +221,10 @@ public class TextManager {
 		
 		// Create the arrays we need with the correct size.
 		vecs = null;
-		colors = null;
 		uvs = null;
 		indices = null;
 		
 		vecs = new float[charcount * 12];
-		colors = new float[charcount * 16];
 		uvs = new float[charcount * 8];
 		indices = new short[charcount * 6];
 
@@ -269,6 +254,7 @@ public class TextManager {
 		if(vecs.length > 0)
 		{
 			GLES20.glDisable(GLES20.GL_DEPTH_TEST);
+			GLES20.glEnable(GLES20.GL_BLEND);
 			
 			// Set the correct shader for our grid object.
 			GLES20.glUseProgram(sp_Text);
@@ -280,13 +266,6 @@ public class TextManager {
 			vertexBuffer.put(vecs);
 			vertexBuffer.position(0);
 			
-			// The vertex buffer.
-			ByteBuffer bb3 = ByteBuffer.allocateDirect(colors.length * 4);
-			bb3.order(ByteOrder.nativeOrder());
-			colorBuffer = bb3.asFloatBuffer();
-			colorBuffer.put(colors);
-			colorBuffer.position(0);
-			
 			// The texture buffer
 			ByteBuffer bb2 = ByteBuffer.allocateDirect(uvs.length * 4);
 			bb2.order(ByteOrder.nativeOrder());
@@ -322,22 +301,16 @@ public class TextManager {
 		    GLES20.glEnableVertexAttribArray ( mPositionHandle );
 	        GLES20.glEnableVertexAttribArray ( mTexCoordLoc );
 	
-	        int mColorHandle = GLES20.glGetAttribLocation(sp_Text, "a_Color");
-	
-		    // Enable a handle to the triangle vertices
-		    GLES20.glEnableVertexAttribArray(mColorHandle);
-	
-		    // Prepare the background coordinate data
-		    GLES20.glVertexAttribPointer(mColorHandle, 4,
-		                                 GLES20.GL_FLOAT, false,
-		                                 0, colorBuffer);
-	
 		    // get handle to shape's transformation matrix
 	        int mtrxhandle = GLES20.glGetUniformLocation(sp_Text, "uMVPMatrix");
 	        
 	    	// Apply the projection and view transformation
 	        GLES20.glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
 	        
+	        // get handle to color value
+	        int colorhandle = GLES20.glGetUniformLocation(sp_Text, "uColor");
+	        GLES20.glUniform1f(colorhandle, mColor);
+	        
 	        int mSamplerLoc = GLES20.glGetUniformLocation (sp_Text, "s_texture" );
 	        
 	        // Texture activate unit 0
@@ -353,7 +326,6 @@ public class TextManager {
 	        // Disable vertex array
 	        GLES20.glDisableVertexAttribArray(mPositionHandle);
 	        GLES20.glDisableVertexAttribArray(mTexCoordLoc);
-	        GLES20.glDisableVertexAttribArray(mColorHandle);
 		}
 	}
 	
@@ -440,4 +412,8 @@ public class TextManager {
 	public void setUniformscale(float uniformscale) {
 		this.uniformscale = uniformscale;
 	}
+	
+	public void setColor(float color) {
+		mColor = color;
+	}
 }

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

@@ -2,9 +2,12 @@ package com.introlab.rtabmap;
 
 import java.io.BufferedInputStream;
 import java.io.BufferedOutputStream;
+import java.io.File;
 import java.io.FileInputStream;
 import java.io.FileOutputStream;
+import java.io.FilenameFilter;
 import java.io.IOException;
+import java.util.Arrays;
 import java.util.zip.ZipEntry;
 import java.util.zip.ZipOutputStream;
 
@@ -52,5 +55,72 @@ public class Util {
 		}
 	}
 
+	public static String[] loadFileList(String directory, final boolean databasesOnly) {
+		File path = new File(directory); 
+		String fileList[];
+		try {
+			path.mkdirs();
+		}
+		catch(SecurityException e) {
+			Log.e(RTABMapActivity.TAG, "unable to write on the sd card " + e.toString());
+		}
+		if(path.exists()) {
+			FilenameFilter filter = new FilenameFilter() {
+
+				@Override
+				public boolean accept(File dir, String filename) {
+					File sel = new File(dir, filename);
+					if(databasesOnly)
+					{
+						return filename.compareTo(RTABMapActivity.RTABMAP_TMP_DB) != 0 && filename.endsWith(".db");
+					}
+					else
+					{
+						return sel.isFile();
+					}
+				}
+
+			};
+			fileList = path.list(filter);
+			Arrays.sort(fileList);
+		}
+		else {
+			fileList = new String[0];
+		}
+		return fileList;
+	}
+	
+	/**
+	 * https://stackoverflow.com/questions/6701948/efficient-way-to-compare-version-strings-in-java
+	 * Compares two version strings. 
+	 * 
+	 * Use this instead of String.compareTo() for a non-lexicographical 
+	 * comparison that works for version strings. e.g. "1.10".compareTo("1.6").
+	 * 
+	 * @note It does not work if "1.10" is supposed to be equal to "1.10.0".
+	 * 
+	 * @param str1 a string of ordinal numbers separated by decimal points. 
+	 * @param str2 a string of ordinal numbers separated by decimal points.
+	 * @return The result is a negative integer if str1 is _numerically_ less than str2. 
+	 *         The result is a positive integer if str1 is _numerically_ greater than str2. 
+	 *         The result is zero if the strings are _numerically_ equal.
+	 */
+	public static int versionCompare(String str1, String str2) {
+	    String[] vals1 = str1.split("\\.");
+	    String[] vals2 = str2.split("\\.");
+	    int i = 0;
+	    // set index to first non-equal ordinal or length of shortest version string
+	    while (i < vals1.length && i < vals2.length && vals1[i].equals(vals2[i])) {
+	      i++;
+	    }
+	    // compare first non-equal ordinal number
+	    if (i < vals1.length && i < vals2.length) {
+	        int diff = Integer.valueOf(vals1[i]).compareTo(Integer.valueOf(vals2[i]));
+	        return Integer.signum(diff);
+	    }
+	    // the strings are equal or one string is a substring of the other
+	    // e.g. "1.2.3" = "1.2.3" or "1.2.3" < "1.2.3.4"
+	    return Integer.signum(vals1.length - vals2.length);
+	}
 	
 }

app/src/CMakeLists.txt

@@ -74,7 +74,11 @@ ELSE()
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
 IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    IF(Qt5Svg_FOUND)
+        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    ELSE()
+        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
+    ENDIF()
 ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)

cmake_modules/FindORB_SLAM2.cmake

@@ -0,0 +1,31 @@
+# - Find ORB_SLAM2 
+#
+# It sets the following variables:
+#  ORB_SLAM2_FOUND         - Set to false, or undefined, if ORB_SLAM2 isn't found.
+#  ORB_SLAM2_INCLUDE_DIRS  - The ORB_SLAM2 include directory.
+#  ORB_SLAM2_LIBRARIES     - The ORB_SLAM2 library to link against.
+#
+#  Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder.
+
+find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
+find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib)
+
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY 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)
+
+IF (ORB_SLAM2_FOUND)
+   # show which ORB_SLAM2 was found only if not quiet
+   IF (NOT ORB_SLAM2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}")
+   ENDIF (NOT ORB_SLAM2_FIND_QUIETLY)
+ELSE (ORB_SLAM2_FOUND)
+   # fatal error if ORB_SLAM2 is required but not found
+   IF (ORB_SLAM2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2")
+   ENDIF (ORB_SLAM2_FIND_REQUIRED)
+ENDIF (ORB_SLAM2_FOUND)
+

cmake_modules/FindRealSense.cmake

@@ -4,9 +4,18 @@
 #
 # It sets the following variables:
 #  RealSense_FOUND       - Set to false, or undefined, if RealSense isn't found.
+#  RealSenseSlam_FOUND    - Set to false, or undefined, if RealSense slam module isn't found.
 #  RealSense_INCLUDE_DIRS - The RealSense include directory.
 #  RealSense_LIBRARIES     - The RealSense library to link against.
 
+# Use find_package( RealSense COMPONENTS slam ) to search for realsense slam library
+if( RealSense_FIND_COMPONENTS )
+   foreach( component ${RealSense_FIND_COMPONENTS} )
+     string( TOUPPER ${component} _COMPONENT )
+     set( REALSENSE_USE_${_COMPONENT} 1 )
+   endforeach()
+endif()
+
 #RealSense library
 find_path(RealSense_INCLUDE_DIRS NAMES librealsense/rs.hpp PATHS $ENV{RealSense_ROOT_DIR}/include)
 if(CMAKE_CL_64)
@@ -19,9 +28,44 @@ IF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
    SET(RealSense_FOUND TRUE)
 ENDIF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY)
 
+#SLAM
+if(REALSENSE_USE_SLAM)
+   find_path(RealSenseSlam_INCLUDE_DIRS NAMES librealsense/slam/slam.h PATHS $ENV{RealSense_ROOT_DIR}/include)
+   if(CMAKE_CL_64)
+      find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+      find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+      find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+      find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64)
+   else()
+      find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+      find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+      find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+      find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib  $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32)
+   endif()
+else()
+   set(RealSenseSlam_INCLUDE_DIRS "")
+endif()
+
 IF (RealSense_FOUND)
-   # show which RealSense was found only if not quiet
+
+   IF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+      SET(RealSenseSlam_FOUND TRUE)
+   ENDIF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+
    SET(RealSense_LIBRARIES ${RealSense_LIBRARY})
+   IF (RealSenseSlam_FOUND)
+      IF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+         SET(RealSenseSlam_FOUND TRUE)
+      ENDIF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY)
+      SET(RealSense_LIBRARIES 
+            ${RealSense_LIBRARIES}
+            ${RealSenseSlam_LIBRARY}
+            ${RealSenseImage_LIBRARY}
+            ${RealSenseSP_Core_LIBRARY}
+            ${RealSenseTracker_LIBRARY})
+   ENDIF(RealSenseSlam_FOUND)
+
+   # show which RealSense was found only if not quiet
    IF (NOT RealSense_FIND_QUIETLY)
       MESSAGE(STATUS "Found RealSense: ${RealSense_LIBRARIES}")
    ENDIF (NOT RealSense_FIND_QUIETLY)

corelib/include/rtabmap/core/CameraRGB.h

@@ -107,12 +107,14 @@ public:
 		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
 	}
 
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 	void setOdometryPath(const std::string & filePath, int format = 0)
 	{
 		_odometryPath = filePath;
 		_odometryFormat = format;
 	}
 
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
 		_groundTruthPath = filePath;
@@ -127,7 +129,11 @@ public:
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
-	bool readPoses(std::list<Transform> & outputPoses, std::list<double> & stamps, const std::string & filePath, int format) const;
+	bool readPoses(
+			std::list<Transform> & outputPoses,
+			std::list<double> & stamps,
+			const std::string & filePath,
+			int format) const;
 
 private:
 	std::string _path;

corelib/include/rtabmap/core/CameraRGBD.h

@@ -69,6 +69,9 @@ namespace rs
 {
 	class context;
 	class device;
+	namespace slam {
+	class slam;
+	}
 }
 
 typedef struct _freenect_context freenect_context;
@@ -290,6 +293,7 @@ private:
 /////////////////////////
 // CameraRealSense
 /////////////////////////
+class slam_event_handler;
 class RTABMAP_EXP CameraRealSense :
 	public Camera
 {
@@ -302,6 +306,7 @@ public:
 		int deviceId = 0,
 		int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
 		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
+		bool computeOdometry = false,
 		float imageRate = 0,
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();
@@ -309,6 +314,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -320,6 +326,16 @@ private:
 	int deviceId_;
 	int presetRGB_;
 	int presetDepth_;
+	bool computeOdometry_;
+
+	int motionSeq_[2];
+	rs::slam::slam * slam_;
+	UMutex slamLock_;
+
+	std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
+	std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
+	UMutex dataMutex_;
+	USemaphore dataReady_;
 #endif
 };
 

corelib/include/rtabmap/core/CameraStereo.h

@@ -42,11 +42,8 @@ class Camera;
 
 namespace sl
 {
-namespace zed
-{
 class Camera;
 }
-}
 
 namespace rtabmap
 {
@@ -121,7 +118,7 @@ public:
 			int deviceId,
 			int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 1,// 0=FULL, 1=RAW
+			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
@@ -130,7 +127,7 @@ public:
 	CameraStereoZed(
 			const std::string & svoFilePath,
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 1,// 0=FULL, 1=RAW
+			int sensingMode = 0,// 0=STANDARD, 1=FILL
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
@@ -148,7 +145,7 @@ protected:
 
 private:
 #ifdef RTABMAP_ZED
-	sl::zed::Camera * zed_;
+	sl::Camera * zed_;
 	StereoCameraModel stereoModel_;
 	CameraVideo::Source src_;
 	int usbDevice_;

corelib/include/rtabmap/core/CameraThread.h

@@ -42,6 +42,8 @@ namespace rtabmap
 {
 
 class Camera;
+class CameraInfo;
+class SensorData;
 class StereoDense;
 
 /**
@@ -80,6 +82,8 @@ public:
 		_scanNormalsK = normalsK;
 	}
 
+	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
+
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}

corelib/include/rtabmap/core/DBDriver.h

@@ -98,6 +98,27 @@ public:
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
 	void addStatistics(const Statistics & statistics) const;
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void 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
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
+	cv::Mat loadOptimizedMesh(
+			std::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,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
@@ -107,11 +128,17 @@ public:
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
+	long getNodesMemoryUsed() const;
+	long getLinksMemoryUsed() const;
 	long getImagesMemoryUsed() const;
 	long getDepthImagesMemoryUsed() const;
+	long getCalibrationsMemoryUsed() const;
+	long getGridsMemoryUsed() const;
 	long getLaserScansMemoryUsed() const;
 	long getUserDataMemoryUsed() const;
 	long getWordsMemoryUsed() const;
+	long getFeaturesMemoryUsed() const;
+	long getStatisticsMemoryUsed() const;
 	int getLastNodesSize() const; // working memory
 	int getLastDictionarySize() const; // working memory
 	int getTotalNodesSize() const;
@@ -131,7 +158,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 getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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;
 	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;
@@ -151,11 +179,17 @@ private:
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
+	virtual long getNodesMemoryUsedQuery() const = 0;
+	virtual long getLinksMemoryUsedQuery() const = 0;
 	virtual long getImagesMemoryUsedQuery() const = 0;
 	virtual long getDepthImagesMemoryUsedQuery() const = 0;
+	virtual long getCalibrationsMemoryUsedQuery() const = 0;
+	virtual long getGridsMemoryUsedQuery() const = 0;
 	virtual long getLaserScansMemoryUsedQuery() const = 0;
 	virtual long getUserDataMemoryUsedQuery() const = 0;
 	virtual long getWordsMemoryUsedQuery() const = 0;
+	virtual long getFeaturesMemoryUsedQuery() const = 0;
+	virtual long getStatisticsMemoryUsedQuery() const = 0;
 	virtual int getLastNodesSizeQuery() const = 0;
 	virtual int getLastDictionarySizeQuery() const = 0;
 	virtual int getTotalNodesSizeQuery() const = 0;
@@ -187,6 +221,27 @@ private:
 					const cv::Mat & image) const = 0;
 
 	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
+	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void 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,
+#else
+				const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#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,
+#else
+				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+				cv::Mat * textures) const = 0;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;
@@ -197,7 +252,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 getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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 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;

corelib/include/rtabmap/core/Features2d.h

@@ -105,7 +105,7 @@ public:
 		kFeatureGfttBrief=6,
 		kFeatureBrisk=7,
 		kFeatureGfttOrb=8,  //new 0.10.11
-		kFeatureFreak=9};   //new 0.11.14
+		kFeatureKaze=9};    //new 0.13.2
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -433,27 +433,31 @@ private:
 	cv::Ptr<CV_BRISK> brisk_;
 };
 
-//FREAK
-class RTABMAP_EXP FREAK : public Feature2D
+//KAZE
+class RTABMAP_EXP KAZE : public Feature2D
 {
 public:
-	FREAK(const ParametersMap & parameters = ParametersMap());
-	virtual ~FREAK();
+	KAZE(const ParametersMap & parameters = ParametersMap());
+	virtual ~KAZE();
 
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const { return kFeatureFreak; }
+	virtual Feature2D::Type getType() const { return kFeatureKaze; }
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	bool orientationNormalized_;
-	bool scaleNormalized_;
-	float patternScale_;
+	bool extended_;
+	bool upright_;
+	float threshold_;
 	int nOctaves_;
+	int nOctaveLayers_;
+	int diffusivity_;
 
-	cv::Ptr<CV_FREAK> _freak;
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<cv::KAZE> kaze_;
+#endif
 };
 
 

corelib/include/rtabmap/core/GainCompensator.h

@@ -73,18 +73,22 @@ public:
 
 	void apply(
 			int id,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) const;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			bool rgb = true) const;
 	void apply(
 			int id,
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-			const pcl::IndicesPtr & indices) const;
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
 	void apply(
 			int id,
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-			const pcl::IndicesPtr & indices) const;
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
 	void apply(
 			int id,
-			cv::Mat & image) const;
+			cv::Mat & image,
+			bool rgb = true) const;
 
 	double getGain(int id, double * r=0, double * g=0, double * b=0) const;
 	int getIndex(int id) const;

corelib/include/rtabmap/core/Graph.h

@@ -53,11 +53,25 @@ bool RTABMAP_EXP exportPoses(
 
 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, GPS (t,x,y)
+		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
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1
 
+/**
+ * Compute translation and rotation errors for KITTI datasets.
+ * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
+ * @param poses_gt, Ground Truth poses
+ * @param poses_result, Estimated poses
+ * @param t_err, Output translation error (%)
+ * @param r_err, Output rotation error (deg/m)
+ */
+void RTABMAP_EXP calcKittiSequenceErrors(
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err);
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
@@ -205,6 +219,10 @@ float RTABMAP_EXP computePathLength(
 		unsigned int fromIndex = 0,
 		unsigned int toIndex = 0);
 
+// assuming they are all linked in map order
+float RTABMAP_EXP computePathLength(
+		const std::map<int, Transform> & path);
+
 std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);

corelib/include/rtabmap/core/Link.h

@@ -46,20 +46,14 @@ public:
 		kUserClosure,
 		kVirtualClosure,
 		kNeighborMerged,
-		kUndef};
+		kPosePrior,
+		kUndef = 99};
 	Link();
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
-			const cv::Mat & userData = cv::Mat());
-	Link(int from,
-			int to,
-			Type type,
-			const Transform & transform,
-			double rotVariance,
-			double transVariance,
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
 			const cv::Mat & userData = cv::Mat());
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -87,7 +81,6 @@ public:
 
 private:
 	void setInfMatrix(const cv::Mat & infMatrix);
-	void setVariance(double rotVariance, double transVariance);
 
 private:
 	int from_;

corelib/include/rtabmap/core/Memory.h

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <pcl/pcl_config.h>
 
 namespace rtabmap {
 
@@ -75,6 +76,7 @@ public:
 	bool update(const SensorData & data,
 			const Transform & pose,
 			const cv::Mat & covariance,
+			const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
 			Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
@@ -91,6 +93,27 @@ public:
 
 	int cleanup();
 	void saveStatistics(const Statistics & statistics);
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() 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
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
+	cv::Mat loadOptimizedMesh(
+			std::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,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
 	void emptyTrash();
 	void joinTrashThread();
 	bool addLink(const Link & link, bool addInDatabase = false);
@@ -153,6 +176,7 @@ public:
 			std::string & label,
 			double & stamp,
 			Transform & groundTruth,
+			std::vector<float> & velocity,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
@@ -244,6 +268,7 @@ private:
 	bool _rawDescriptorsKept;
 	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
+	bool _saveIntermediateNodeData;
 	bool _incrementalMemory;
 	bool _reduceGraph;
 	int _maxStMemSize;

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -44,7 +44,12 @@ public:
 	void parseParameters(const ParametersMap & parameters);
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
+	float getMinMapSize() const {return minMapSize_;}
 	bool isGridFromDepth() const {return occupancyFromCloud_;}
+	bool isFullUpdate() const {return fullUpdate_;}
+	bool isMapFrameProjection() const {return projMapFrame_;}
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	int cacheSize() const {return (int)cache_.size();}
 
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -67,13 +72,8 @@ public:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles);
-	void update(const std::map<int, Transform> & poses, float minMapSize = 0.0f, float footprintRadius = 0.0f);
-	const cv::Mat & getMap(float & xMin, float & yMin) const
-	{
-		xMin = xMin_;
-		yMin = yMin_;
-		return map_;
-	}
+	void update(const std::map<int, Transform> & poses);
+	const cv::Mat getMap(float & xMin, float & yMin) const;
 
 private:
 	ParametersMap parameters_;
@@ -104,6 +104,10 @@ private:
 	bool scan2dUnknownSpaceFilled_;
 	double scan2dMaxUnknownSpaceFilledRange_;
 	bool projRayTracing_;
+	bool fullUpdate_;
+	float minMapSize_;
+	bool erode_;
+	float footprintRadius_;
 
 	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
 	cv::Mat map_;

corelib/include/rtabmap/core/OctoMap.h

@@ -57,7 +57,7 @@ public:
 
 class RTABMAP_EXP OctoMap {
 public:
-	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f);
+	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
 
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	void addToCache(int nodeId,
@@ -81,7 +81,8 @@ public:
 			float & xMin,
 			float & yMin,
 			float & gridCellSize,
-			float minGridSize);
+			float minGridSize = 0.0f,
+			unsigned int treeDepth = 0);
 
 	bool writeBinary(const std::string & path);
 
@@ -97,6 +98,7 @@ private:
 	std::map<int, Transform> addedNodes_;
 	octomap::KeyRay keyRay_;
 	bool hasColor_;
+	bool fullUpdate_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Odometry.h

@@ -45,7 +45,11 @@ public:
 	enum Type {
 		kTypeUndef = -1,
 		kTypeF2M = 0,
-		kTypeF2F = 1
+		kTypeF2F = 1,
+		kTypeFovis = 2,
+		kTypeViso2 = 3,
+		kTypeDVO = 4,
+		kTypeORBSLAM2 = 5
 	};
 
 public:

corelib/include/rtabmap/core/OdometryDVO.h

@@ -0,0 +1,67 @@
+/*
+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 ODOMETRYDVO_H_
+#define ODOMETRYDVO_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace dvo {
+class DenseTracker;
+namespace core {
+class RgbdImagePyramid;
+class RgbdCameraPyramid;
+}
+}
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryDVO : public Odometry
+{
+public:
+	OdometryDVO(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryDVO();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeDVO;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+	dvo::DenseTracker * dvo_;
+	dvo::core::RgbdImagePyramid * reference_;
+	dvo::core::RgbdCameraPyramid * camera_;
+	bool lost_;
+	Transform motionFromKeyFrame_;
+	Transform previousLocalTransform_;
+
+};
+
+}
+
+#endif /* ODOMETRYDVO_H_ */

corelib/include/rtabmap/core/OdometryEvent.h

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

corelib/include/rtabmap/core/OdometryFovis.h

@@ -0,0 +1,68 @@
+/*
+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 ODOMETRYFOVIS_H_
+#define ODOMETRYFOVIS_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace fovis {
+class VisualOdometry;
+class Rectification;
+class StereoCalibration;
+class DepthImage;
+class StereoDepth;
+}
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryFovis : public Odometry
+{
+public:
+	OdometryFovis(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryFovis();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeFovis;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+	fovis::VisualOdometry * fovis_;
+	fovis::Rectification * rect_;
+	fovis::StereoCalibration * stereoCalib_;
+	fovis::DepthImage * depthImage_;
+	fovis::StereoDepth * stereoDepth_;
+	ParametersMap fovisParameters_;
+	bool lost_;
+	Transform previousLocalTransform_;
+};
+
+}
+
+#endif /* ODOMETRYFOVIS_H_ */

corelib/include/rtabmap/core/OdometryInfo.h

@@ -42,8 +42,6 @@ public:
 		matches(0),
 		inliers(0),
 		icpInliersRatio(0.0f),
-		varianceLin(0.0f),
-		varianceAng(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
@@ -67,8 +65,7 @@ public:
 		output.matches = matches;
 		output.inliers = inliers;
 		output.icpInliersRatio = icpInliersRatio;
-		output.varianceLin = varianceLin;
-		output.varianceAng = varianceAng;
+		output.covariance = covariance.clone();
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
@@ -80,6 +77,7 @@ public:
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
+		output.interval = interval;
 		output.transform = transform;
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
@@ -92,8 +90,7 @@ public:
 	int matches;
 	int inliers;
 	float icpInliersRatio;
-	float varianceLin;
-	float varianceAng;
+	cv::Mat covariance;
 	int features;
 	int localMapSize;
 	int localScanMapSize;

corelib/include/rtabmap/core/OdometryORBSLAM2.h

@@ -0,0 +1,63 @@
+/*
+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 ODOMETRYORBSLAM2_H_
+#define ODOMETRYORBSLAM2_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace ORB_SLAM2 {
+class System;
+}
+
+class ORBSLAM2System;
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryORBSLAM2 : public Odometry
+{
+public:
+	OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryORBSLAM2();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM2;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+	ORBSLAM2System * orbslam2_;
+	ORB_SLAM2::System * system_;
+	bool firstFrame_;
+	Transform originLocalTransform_;
+
+};
+
+}
+
+#endif /* ODOMETRYORBSLAM2_H_ */

corelib/include/rtabmap/core/OdometryViso2.h

@@ -0,0 +1,63 @@
+/*
+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 ODOMETRYVISO2_H_
+#define ODOMETRYVISO2_H_
+
+#include <rtabmap/core/Odometry.h>
+
+class VisualOdometryStereo;
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryViso2 : public Odometry
+{
+public:
+	OdometryViso2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryViso2();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeViso2;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+	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_;
+	Transform reference_motion_;
+	Transform previousLocalTransform_;
+	ParametersMap viso2Parameters_;
+};
+
+}
+
+#endif /* ODOMETRYVISO2_H_ */

corelib/include/rtabmap/core/ProgressState.h

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
 
+#include <rtabmap/utilite/ULogger.h>
 
 class ProgressState
 {
@@ -35,6 +36,8 @@ public:
 	ProgressState():canceled_(false){}
 	virtual bool callback(const std::string & msg) const
 	{
+		if(!msg.empty())
+			UDEBUG("msg=%s", msg.c_str());
 		return true;
 	}
 	virtual ~ProgressState(){}

corelib/include/rtabmap/core/Registration.h

@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef REGISTRATION_H_
-#define REGISTRATION_H_
+#ifndef RTABMAP_REGISTRATION_H_
+#define RTABMAP_REGISTRATION_H_
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
@@ -63,6 +63,7 @@ public:
 
 	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
 	bool force3DoF() const {return force3DoF_;}
+	bool covarianceNormalized() const {return covarianceNormalized_;}
 
 	// take ownership!
 	void setChildRegistration(Registration * child);
@@ -75,7 +76,7 @@ public:
 	Transform computeTransformation(
 			const SensorData & from,
 			const SensorData & to,
-			Transform SensorData = Transform::getIdentity(),
+			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;
 
 	Transform computeTransformationMod(
@@ -84,6 +85,8 @@ 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);
@@ -104,6 +107,7 @@ protected:
 
 private:
 	bool varianceFromInliersCount_;
+	bool covarianceNormalized_;
 	bool force3DoF_;
 	Registration * child_;
 
@@ -111,4 +115,4 @@ private:
 
 }
 
-#endif /* REGISTRATION_H_ */
+#endif /* RTABMAP_REGISTRATION_H_ */

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -35,8 +35,6 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
-		varianceLin(0),
-		varianceAng(0),
 		inliers(0),
 		matches(0),
 		icpInliersRatio(0),
@@ -45,8 +43,7 @@ public:
 	{
 	}
 
-	float varianceLin;
-	float varianceAng;
+	cv::Mat covariance;
 	std::string rejectedMsg;
 
 	// RegistrationVis

corelib/include/rtabmap/core/Rtabmap.h

@@ -59,11 +59,32 @@ public:
 	Rtabmap();
 	virtual ~Rtabmap();
 
-	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
+	/**
+	 * @brief Main loop of rtabmap.
+	 * @param data Sensor data to process.
+	 * @param odomPose Odometry pose, should be non-null for RGB-D SLAM mode.
+	 * @param covariance Odometry covariance.
+	 * @param externalStats External statistics to be saved in the database for convenience
+	 * @return true if data has been added to map.
+	 */
 	bool process(
 			const SensorData & data,
 			Transform odomPose,
-			const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience
+			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
+			const std::vector<float> & odomVelocity = std::vector<float>(),
+			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
+	// for convenience
+	bool process(
+			const SensorData & data,
+			Transform odomPose,
+			float odomLinearVariance,
+			float odomAngularVariance,
+			const std::vector<float> & odomVelocity = std::vector<float>(),
+			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
+	// for convenience, loop closure detection only
+	bool process(
+			const cv::Mat & image,
+			int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
 
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");
@@ -194,6 +215,7 @@ private:
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
 	float _loopRatio;
+	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
 	bool _rawDataKept;

corelib/include/rtabmap/core/RtabmapThread.h

@@ -125,8 +125,7 @@ private:
 	Rtabmap * _rtabmap;
 	bool _paused;
 	Transform lastPose_;
-	double _rotVariance;
-	double _transVariance;
+	cv::Mat covariance_;
 
 	cv::Mat _userData;
 	UMutex _userDataMutex;

corelib/include/rtabmap/core/SensorData.h

@@ -221,6 +221,10 @@ public:
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}
 
+	void setGlobalPose(const Transform & pose, const cv::Mat & covariance) {globalPose_ = pose; globalPoseCovariance_ = covariance;}
+	const Transform & globalPose() const {return globalPose_;}
+	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
+
 	long getMemoryUsed() const; // Return memory usage in Bytes
 
 private:
@@ -258,6 +262,9 @@ private:
 	cv::Mat _descriptors;
 
 	Transform groundTruth_;
+
+	Transform globalPose_;
+	cv::Mat globalPoseCovariance_; // 6x6 double
 };
 
 }

corelib/include/rtabmap/core/Signature.h

@@ -115,11 +115,21 @@ public:
 	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
+	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
+		_velocity = std::vector<float>(6,0);
+		_velocity[0]=vx;
+		_velocity[1]=vy;
+		_velocity[2]=vz;
+		_velocity[3]=vroll;
+		_velocity[4]=vpitch;
+		_velocity[5]=vyaw;
+	}
 
 	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
+	const std::vector<float> & getVelocity() const {return _velocity;}
 
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}
@@ -147,6 +157,7 @@ private:
 
 	Transform _pose;
 	Transform _groundTruthPose;
+	std::vector<float> _velocity;
 
 	SensorData _sensorData;
 };

corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h

@@ -32,6 +32,7 @@ RTAB-Map integration: Mathieu Labbe
 
 #include <assert.h>
 #include <vector>
+#include <set>
 #include <Eigen/Core>
 #include <opencv2/opencv.hpp>
 #include <rtabmap/utilite/UMutex.h>
@@ -42,7 +43,7 @@ namespace clams
   class RTABMAP_EXP DiscreteFrustum
   {
   public:
-    DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0);
+    DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0, double max_dist = 10.0);
     //! z value, not distance to origin.
     //! thread-safe.
     void addExample(double ground_truth, double measurement);
@@ -66,6 +67,16 @@ namespace clams
 
   class RTABMAP_EXP DiscreteDepthDistortionModel
   {
+  public:
+    // returns all divisors of num
+    static std::set<size_t> getDivisors(const size_t &num);
+
+    // returns divisor from divisors closest to ref
+    static size_t getClosestToRef(const std::set<size_t> &divisors, const double &ref);
+
+    // sets bin_width and bin_height to appropriate values
+    static void getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height);
+
   public:
     DiscreteDepthDistortionModel() :
     	width_(0),
@@ -78,7 +89,7 @@ namespace clams
 		training_samples_(0)
     {}
     virtual ~DiscreteDepthDistortionModel();
-    DiscreteDepthDistortionModel(int width, int height, int bin_width = 8, int bin_height = 6, double bin_depth = 2.0, int smoothing = 1);
+    DiscreteDepthDistortionModel(int width, int height, int bin_width = 8, int bin_height = 6, double bin_depth = 2.0, int smoothing = 1, double max_depth = 10.0);
     DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other);
     DiscreteDepthDistortionModel& operator=(const DiscreteDepthDistortionModel& other);
     void undistort(cv::Mat & depth) const;

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

@@ -82,7 +82,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 	}
 
 	// filter ground/obstacles zone
-	if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
+	if(minGroundHeight_ != 0.0f || maxObstacleHeight_ != 0.0f)
 	{
 		indices = util3d::passThrough(cloud, indices, "z",
 				minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
@@ -98,7 +98,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
 			UDEBUG("Cluster radius=%f", clusterRadius_);
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
-			UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
+			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -121,8 +121,17 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		{
 			UDEBUG("");
 			// passthrough filter
-			groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
-			obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
+			groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z",
+					minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
+					maxGroundHeight_!=0.0f?maxGroundHeight_:std::numeric_limits<int>::max());
+
+			pcl::IndicesPtr notObstacles = groundIndices;
+			if(indices->size())
+			{
+				notObstacles = util3d::extractIndices(cloud, indices, true);
+				notObstacles = util3d::concatenate(notObstacles, groundIndices);
+			}
+			obstaclesIndices = rtabmap::util3d::extractIndices(cloud, notObstacles, true);
 		}
 
 		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());

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

@@ -104,7 +104,7 @@ void segmentObstaclesFromGround(
 				Eigen::Vector4f min,max;
 				pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
 
-				if(maxGroundHeight <= 0 || min[2] < maxGroundHeight)
+				if(maxGroundHeight == 0.0f || min[2] < maxGroundHeight)
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
@@ -113,7 +113,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 && centroid[2] <= maxGroundHeight+0.01))) // epsilon
+							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight!=0.0f && centroid[2] <= maxGroundHeight+0.01))) // epsilon
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -154,7 +154,7 @@ void segmentObstaclesFromGround(
 			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
 
 			// If ground height is set, remove obstacles under it
-			if(maxGroundHeight > 0.0f)
+			if(maxGroundHeight != 0.0f)
 			{
 				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
 			}

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

@@ -8,6 +8,9 @@
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
 #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
 
+#include <pcl/search/kdtree.h>
+#include <rtabmap/utilite/UConversion.h>
+
 namespace rtabmap {
 
 namespace util3d {
@@ -42,6 +45,278 @@ std::vector<pcl::Vertices> normalizePolygonsSide(
 	return output;
 }
 
+template<typename pointRGBT>
+void denseMeshPostProcessing(
+		pcl::PolygonMeshPtr & mesh,
+		float meshDecimationFactor,
+		int maximumPolygons,
+		const typename pcl::PointCloud<pointRGBT>::Ptr & cloud,
+		float transferColorRadius,
+		bool coloredOutput,
+		bool cleanMesh,
+		int minClusterSize,
+		ProgressState * progressState)
+{
+	// compute normals for the mesh if not already here
+	bool hasNormals = false;
+	bool hasColors = false;
+	for(unsigned int i=0; i<mesh->cloud.fields.size(); ++i)
+	{
+		if(mesh->cloud.fields[i].name.compare("normal_x") == 0)
+		{
+			hasNormals = true;
+		}
+		else if(mesh->cloud.fields[i].name.compare("rgb") == 0)
+		{
+			hasColors = true;
+		}
+	}
+
+	if(maximumPolygons > 0)
+	{
+		double factor = 1.0-double(maximumPolygons)/double(mesh->polygons.size());
+		if(factor > meshDecimationFactor)
+		{
+			meshDecimationFactor = factor;
+		}
+	}
+	if(meshDecimationFactor > 0.0)
+	{
+		unsigned int count = mesh->polygons.size();
+		if(progressState) progressState->callback(uFormat("Mesh decimation (factor=%f) from %d polygons...",meshDecimationFactor, (int)count));
+
+		mesh = util3d::meshDecimation(mesh, (float)meshDecimationFactor);
+		if(progressState) progressState->callback(uFormat("Mesh decimated (factor=%f) from %d to %d polygons", meshDecimationFactor, (int)count, (int)mesh->polygons.size()));
+		if(count < mesh->polygons.size())
+		{
+			if(progressState) progressState->callback(uFormat("Decimated mesh has more polygons than before!"));
+		}
+		hasNormals = false;
+		hasColors = false;
+	}
+
+	if(cloud.get()!=0 &&
+		!hasColors &&
+		transferColorRadius >= 0.0)
+	{
+		if(progressState) progressState->callback(uFormat("Transferring color from point cloud to mesh..."));
+
+		// transfer color from point cloud to mesh
+		typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
+		tree->setInputCloud(cloud);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud);
+		std::vector<bool> coloredPts(coloredCloud->size());
+		for(unsigned int i=0; i<coloredCloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			pointRGBT pt;
+			pt.x = coloredCloud->at(i).x;
+			pt.y = coloredCloud->at(i).y;
+			pt.z = coloredCloud->at(i).z;
+			if(transferColorRadius > 0.0)
+			{
+				tree->radiusSearch(pt, transferColorRadius, kIndices, kDistances);
+			}
+			else
+			{
+				tree->nearestKSearch(pt, 1, kIndices, kDistances);
+			}
+			if(kIndices.size())
+			{
+				//compute average color
+				int r=0;
+				int g=0;
+				int b=0;
+				int a=0;
+				for(unsigned int j=0; j<kIndices.size(); ++j)
+				{
+					r+=(int)cloud->at(kIndices[j]).r;
+					g+=(int)cloud->at(kIndices[j]).g;
+					b+=(int)cloud->at(kIndices[j]).b;
+					a+=(int)cloud->at(kIndices[j]).a;
+				}
+				coloredCloud->at(i).r = r/kIndices.size();
+				coloredCloud->at(i).g = g/kIndices.size();
+				coloredCloud->at(i).b = b/kIndices.size();
+				coloredCloud->at(i).a = a/kIndices.size();
+				coloredPts.at(i) = true;
+			}
+			else
+			{
+				//white
+				coloredCloud->at(i).r = coloredCloud->at(i).g = coloredCloud->at(i).b = 255;
+				coloredPts.at(i) = false;
+			}
+		}
+		pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
+
+		// remove polygons with no color
+		if(cleanMesh)
+		{
+			std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
+			int oi=0;
+			for(unsigned int i=0; i<mesh->polygons.size(); ++i)
+			{
+				bool coloredPolygon = true;
+				for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
+				{
+					if(!coloredPts.at(mesh->polygons[i].vertices[j]))
+					{
+						coloredPolygon = false;
+						break;
+					}
+				}
+				if(coloredPolygon)
+				{
+					filteredPolygons[oi++] = mesh->polygons[i];
+				}
+			}
+			filteredPolygons.resize(oi);
+			mesh->polygons = filteredPolygons;
+		}
+		hasColors = true;
+	}
+
+	if(minClusterSize)
+	{
+		if(progressState) progressState->callback(uFormat("Filter small polygon clusters..."));
+
+		// filter polygons
+		std::vector<std::set<int> > neighbors;
+		std::vector<std::set<int> > vertexToPolygons;
+		util3d::createPolygonIndexes(mesh->polygons,
+				mesh->cloud.height*mesh->cloud.width,
+				neighbors,
+				vertexToPolygons);
+		std::list<std::list<int> > clusters = util3d::clusterPolygons(
+				neighbors,
+				minClusterSize<0?0:minClusterSize);
+
+		std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
+		if(minClusterSize < 0)
+		{
+			// only keep the biggest cluster
+			std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
+			unsigned int biggestClusterSize = 0;
+			for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
+			{
+				if(iter->size() > biggestClusterSize)
+				{
+					biggestClusterIndex = iter;
+					biggestClusterSize = iter->size();
+				}
+			}
+			if(biggestClusterIndex != clusters.end())
+			{
+				int oi=0;
+				for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
+				{
+					filteredPolygons[oi++] = mesh->polygons.at(*jter);
+				}
+				filteredPolygons.resize(oi);
+			}
+		}
+		else
+		{
+			int oi=0;
+			for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
+			{
+				for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
+				{
+					filteredPolygons[oi++] = mesh->polygons.at(*jter);
+				}
+			}
+			filteredPolygons.resize(oi);
+		}
+
+		int before = (int)mesh->polygons.size();
+		mesh->polygons = filteredPolygons;
+
+		if(progressState) progressState->callback(uFormat("Filtered %d polygons.", before-(int)mesh->polygons.size()));
+	}
+
+	// compute normals for the mesh if not already here, add also white color if colored output is required
+	if(!hasNormals || (!hasColors && coloredOutput))
+	{
+		// use polygons
+		if(hasColors || coloredOutput)
+		{
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+			pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
+
+			Eigen::Vector3f normal(1,0,0);
+			for(unsigned int i=0; i<mesh->polygons.size(); ++i)
+			{
+				pcl::Vertices & v = mesh->polygons[i];
+				if(!hasNormals)
+				{
+					UASSERT(v.vertices.size()>2);
+					Eigen::Vector3f v0(
+							cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
+							cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
+							cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
+					int last = v.vertices.size()-1;
+					Eigen::Vector3f v1(
+							cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
+							cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
+							cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
+					normal = v0.cross(v1);
+					normal.normalize();
+				}
+				// flat normal (per face)
+				for(unsigned int j=0; j<v.vertices.size(); ++j)
+				{
+					if(!hasNormals)
+					{
+						cloud->at(v.vertices[j]).normal_x = normal[0];
+						cloud->at(v.vertices[j]).normal_y = normal[1];
+						cloud->at(v.vertices[j]).normal_z = normal[2];
+					}
+					if(!hasColors)
+					{
+						cloud->at(v.vertices[j]).r = 255;
+						cloud->at(v.vertices[j]).g = 255;
+						cloud->at(v.vertices[j]).b = 255;
+					}
+				}
+			}
+			pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
+			pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
+
+			for(unsigned int i=0; i<mesh->polygons.size(); ++i)
+			{
+				pcl::Vertices & v = mesh->polygons[i];
+				UASSERT(v.vertices.size()>2);
+				Eigen::Vector3f v0(
+						cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
+						cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
+						cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
+				int last = v.vertices.size()-1;
+				Eigen::Vector3f v1(
+						cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
+						cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
+						cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
+				Eigen::Vector3f normal = v0.cross(v1);
+				normal.normalize();
+				// flat normal (per face)
+				for(unsigned int j=0; j<v.vertices.size(); ++j)
+				{
+					cloud->at(v.vertices[j]).normal_x = normal[0];
+					cloud->at(v.vertices[j]).normal_y = normal[1];
+					cloud->at(v.vertices[j]).normal_z = normal[2];
+				}
+			}
+			pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
+		}
+	}
+}
+
 }
 
 }

corelib/include/rtabmap/core/util3d.h

@@ -190,14 +190,16 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
 		float maxDepth,
 		float minDepth);
 
-// return CV_32FC3
+// return CV_32FC3  (x,y,z)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
-// return CV_32FC6
+// 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());
-// return CV_32FC4
+// return CV_32FC4 (x,y,z,rgb)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
-// return CV_32FC2
+// 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());
+// 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());
@@ -205,6 +207,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::
 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);
 
 // For laserScan of type CV_32FC2, z is set to null.
 pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
@@ -212,7 +217,12 @@ pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index)
 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);
 
+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);
 
 cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
@@ -258,12 +268,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(
 		const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
 
-pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
-		const std::list<pcl::TextureMesh::Ptr> & meshes);
-
-void RTABMAP_EXP concatenateTextureMaterials(
-		pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, float & scale, std::vector<bool> * materialsKept=0);
-
 /**
  * @brief Concatenate a vector of indices to a single vector.
  *

corelib/include/rtabmap/core/util3d_filtering.h

@@ -135,6 +135,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		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::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/include/rtabmap/core/util3d_mapping.h

@@ -51,13 +51,23 @@ RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
 		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.");
 
-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,
 		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.");
+
+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,
+		float cellSize,
+		bool unknownSpaceFilled = false,
 		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
 
 cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
@@ -79,7 +89,7 @@ RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform
 		float minMapSize = 0.0f,
 		float scanMaxRange = 0.0f), "Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base.");
 
-cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
+RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans, // in /base_link frame
 		const std::map<int, cv::Point3f > & viewpoints, // /base_link -> /base_scan
 		float cellSize,
@@ -87,6 +97,16 @@ cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		float & xMin,
 		float & yMin,
 		float minMapSize = 0.0f,
+		float scanMaxRange = 0.0f), "Use interface with cv::Mat scans.");
+
+cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
+		const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >, in /base_link frame
+		const std::map<int, cv::Point3f > & viewpoints, // /base_link -> /base_scan
+		float cellSize,
+		bool unknownSpaceFilled,
+		float & xMin,
+		float & yMin,
+		float minMapSize = 0.0f,
 		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
 
 void RTABMAP_EXP rayTrace(const cv::Point2i & start,
@@ -96,6 +116,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
+cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map);
+
 template<typename PointT>
 typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 		const typename pcl::PointCloud<PointT> & cloud);

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -50,7 +50,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
 			int pnpRefineIterations = 1,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
-			double * varianceOut = 0, // mean reproj error if words3B is not set
+			cv::Mat * covariance = 0, // mean reproj error if words3B is not set
 			std::vector<int> * matchesOut = 0,
 			std::vector<int> * inliersOut = 0);
 
@@ -61,7 +61,7 @@ Transform RTABMAP_EXP estimateMotion3DTo3D(
 			double inliersDistance = 0.1,
 			int iterations = 100,
 			int refineIterations = 5,
-			double * varianceOut = 0,
+			cv::Mat * covariance = 0,
 			std::vector<int> * matchesOut = 0,
 			std::vector<int> * inliersOut = 0);
 

corelib/include/rtabmap/core/util3d_registration.h

@@ -57,7 +57,7 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		int refineModelIterations = 10,
 		double refineModelSigma = 3.0,
 		std::vector<int> * inliers = 0,
-		double * variance = 0);
+		cv::Mat * variance = 0);
 
 void RTABMAP_EXP computeVarianceAndCorrespondences(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,

corelib/include/rtabmap/core/util3d_surface.h

@@ -44,6 +44,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+class Memory;
+class DBDriver;
+
 namespace util3d
 {
 
@@ -137,10 +140,79 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
 		const std::map<int, CameraModel> & cameraModels,
+		const std::map<int, cv::Mat> & cameraDepths,
 		float maxDistance = 0.0f, // max camera distance to polygon to apply texture
+		float maxDepthError = 0.0f, // maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used)
+		float maxAngle = 0.0f, // maximum angle between camera and face (0=disabled)
 		int minClusterSize = 50, // minimum size of polygons clusters textured
+		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
+		const ProgressState * state = 0,
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
+pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
+		const pcl::PolygonMesh::Ptr & mesh,
+		const std::map<int, Transform> & poses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		const std::map<int, cv::Mat> & cameraDepths,
+		float maxDistance = 0.0f, // max camera distance to polygon to apply texture
+		float maxDepthError = 0.0f, // maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used)
+		float maxAngle = 0.0f, // maximum angle between camera and face (0=disabled)
+		int minClusterSize = 50, // minimum size of polygons clusters textured
+		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
+		const ProgressState * state = 0,
+		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0);
+
+/**
+ * Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept.
+ */
+void RTABMAP_EXP cleanTextureMesh(
+		pcl::TextureMesh & textureMesh,
+		int minClusterSize);
+
+pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
+		const std::list<pcl::TextureMesh::Ptr> & meshes);
+
+void RTABMAP_EXP concatenateTextureMaterials(
+		pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);
+
+pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(
+		const cv::Mat & cloudMat,
+		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+		const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+		cv::Mat & textures,
+		bool mergeTextures = false);
+
+pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(
+		const cv::Mat & cloudMat,
+		const std::vector<std::vector<unsigned int> > & polygons);
+
+/**
+ * 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, std::vector<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);
 
+
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		int normalKSearch = 20,
@@ -213,6 +285,18 @@ std::vector<pcl::Vertices> normalizePolygonsSide(
 		const std::vector<pcl::Vertices> & polygons,
 		const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0));
 
+template<typename pointRGBT>
+void denseMeshPostProcessing(
+		pcl::PolygonMeshPtr & mesh,
+		float meshDecimationFactor = 0.0f, // value between 0 and 1, 0=disabled
+		int maximumPolygons = 0,           // 0=disabled
+		const typename pcl::PointCloud<pointRGBT>::Ptr & cloud = pcl::PointCloud<pointRGBT>::Ptr(), // A RGB point cloud used to transfer colors back to mesh (needed for parameters below)
+		float transferColorRadius = 0.05f, // <0=disabled, 0=nearest color
+		bool coloredOutput = true,         // Not used anymore, output is colored if transferColorRadius>=0
+		bool cleanMesh = true,             // Remove polygons not colored (if coloredOutput is disabled, transferColorRadius is still used to clean the mesh)
+		int minClusterSize = 50,           // Remove small polygon clusters after the mesh has been cleaned (0=disabled)
+		ProgressState * progressState = 0);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_transforms.h

@@ -94,6 +94,9 @@ pcl::PointXYZRGB RTABMAP_EXP transformPoint(
 pcl::PointNormal RTABMAP_EXP transformPoint(
 		const pcl::PointNormal & point,
 		const Transform & transform);
+pcl::PointXYZRGBNormal RTABMAP_EXP transformPoint(
+		const pcl::PointXYZRGBNormal & point,
+		const Transform & transform);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -60,6 +60,10 @@ SET(SRC_FILES
 	OdometryF2M.cpp
 	OdometryMono.cpp
 	OdometryF2F.cpp
+	OdometryFovis.cpp
+	OdometryViso2.cpp
+	OdometryDVO.cpp
+	OdometryORBSLAM2.cpp
 	
 	Stereo.cpp
 	StereoDense.cpp
@@ -275,6 +279,50 @@ IF(OCTOMAP_FOUND)
 	)
 ENDIF(OCTOMAP_FOUND)
 
+IF(libfovis_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${libfovis_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${libfovis_LIBRARIES}
+	)
+ENDIF(libfovis_FOUND)
+
+IF(libviso2_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${libviso2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${libviso2_LIBRARIES}
+	)
+ENDIF(libviso2_FOUND)
+
+IF(dvo_core_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${dvo_core_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${dvo_core_LIBRARIES}
+	)
+ENDIF(dvo_core_FOUND)
+
+IF(ORB_SLAM2_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${ORB_SLAM2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${ORB_SLAM2_LIBRARIES}
+	)
+ENDIF(ORB_SLAM2_FOUND)
+
 ####################################
 # Generate resources files
 ####################################
@@ -296,7 +344,7 @@ SET(RESOURCES_HEADERS
 IF(ANDROID)
 
    IF(NOT RTABMAP_RES_TOOL)
-      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool)
+      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
       IF(NOT RTABMAP_RES_TOOL)
          MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
       ENDIF(NOT RTABMAP_RES_TOOL)

corelib/src/CameraRGBD.cpp

@@ -14,7 +14,6 @@ modification, are permitted provided that the following conditions are met:
       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
@@ -66,6 +65,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #ifdef RTABMAP_REALSENSE
 #include <librealsense/rs.hpp>
+#ifdef RTABMAP_REALSENSE_SLAM
+#include <rs_core.h>
+#include <rs_utils.h>
+#include <librealsense/slam/slam.h>
+#endif
 #endif
 
 #ifdef RTABMAP_OPENNI2
@@ -1974,7 +1978,13 @@ bool CameraRealSense::available()
 #endif
 }
 
-CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, float imageRate, const rtabmap::Transform & localTransform) :
+CameraRealSense::CameraRealSense(
+		int device,
+		int presetRGB,
+		int presetDepth,
+		bool computeOdometry,
+		float imageRate,
+		const rtabmap::Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_REALSENSE
     ,
@@ -1982,7 +1992,9 @@ CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, flo
 	dev_(0),
 	deviceId_(device),
 	presetRGB_(presetRGB),
-	presetDepth_(presetDepth)
+	presetDepth_(presetDepth),
+	computeOdometry_(computeOdometry),
+	slam_(0)
 #endif
 {
 	UDEBUG("");
@@ -1990,18 +2002,89 @@ CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, flo
 
 CameraRealSense::~CameraRealSense()
 {
+	UDEBUG("");
 #ifdef RTABMAP_REALSENSE
+	UDEBUG("");
+	if(dev_)
+	{
+		if(slam_!=0)
+		{
+			dev_->stop(rs::source::all_sources);
+		}
+		else
+		{
+			dev_->stop();
+		}
+		dev_ = 0;
+	}
+	UDEBUG("");
 	if (ctx_)
 	{
 		delete ctx_;
 	}
+#ifdef RTABMAP_REALSENSE_SLAM
+	UDEBUG("");
+	if(slam_)
+	{
+		UScopeMutex lock(slamLock_);
+		slam_->flush_resources();
+		delete slam_;
+		slam_ = 0;
+	}
+#endif
 #endif
 }
 
+#ifdef RTABMAP_REALSENSE_SLAM
+bool setStreamConfigIntrin(
+		rs::core::stream_type stream,
+		std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics,
+		rs::core::video_module_interface::supported_module_config & supported_config,
+		rs::core::video_module_interface::actual_module_config & actual_config)
+{
+  auto & supported_stream_config = supported_config[stream];
+  if (!supported_stream_config.is_enabled || supported_stream_config.size.width != intrinsics[stream].width || supported_stream_config.size.height != intrinsics[stream].height)
+  {
+	  UERROR("size of stream is not supported by slam");
+	  			UERROR("  supported: stream %d, width: %d height: %d", (uint32_t) stream, supported_stream_config.size.width, supported_stream_config.size.height);
+	  			UERROR("  received: stream %d, width: %d height: %d", (uint32_t) stream, intrinsics[stream].width, intrinsics[stream].height);
+
+    return false;
+  }
+  rs::core::video_module_interface::actual_image_stream_config &actual_stream_config = actual_config[stream];
+  actual_config[stream].size.width = intrinsics[stream].width;
+  actual_config[stream].size.height = intrinsics[stream].height;
+  actual_stream_config.frame_rate = supported_stream_config.frame_rate;
+  actual_stream_config.intrinsics = intrinsics[stream];
+  actual_stream_config.is_enabled = true;
+  return true;
+}
+#endif
+
 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
 #ifdef RTABMAP_REALSENSE
+
+	if(dev_)
+	{
+		dev_->stop(rs::source::all_sources);
+		dev_ = 0;
+	}
+	bufferedFrames_.clear();
+
+#ifdef RTABMAP_REALSENSE_SLAM
+	motionSeq_[0] = motionSeq_[1] = 0;
+	if(slam_)
+	{
+		UScopeMutex lock(slamLock_);
+		UDEBUG("Flush slam");
+		slam_->flush_resources();
+		delete slam_;
+		slam_ = 0;
+	}
+#endif
+
 	if (ctx_ == 0)
 	{
 		ctx_ = new rs::context();
@@ -2021,12 +2104,23 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		UERROR("Cannot connect to device %d", deviceId_);
 		return false;
 	}
-	UINFO("Using device %d, an %s", deviceId_, dev_->get_name());
+	std::string name = dev_->get_name();
+	UINFO("Using device %d, an %s", deviceId_, name.c_str());
 	UINFO("    Serial number: %s", dev_->get_serial());
 	UINFO("    Firmware version: %s", dev_->get_firmware_version());
 	UINFO("    Preset RGB: %d", presetRGB_);
 	UINFO("    Preset Depth: %d", presetDepth_);
 
+	bool computeOdometry = false;
+#ifdef RTABMAP_REALSENSE_SLAM
+	if (name.find("ZR300") != std::string::npos && computeOdometry_)
+	{
+		// Only enable ZR300 functionality if fisheye stream is enabled.
+		// Accel/Gyro automatically enabled when fisheye requested
+		computeOdometry = true;
+	}
+#endif
+
 	// Configure depth and color to run with the device's preferred settings
 	UINFO("Enabling streams...");
 	// R200: 
@@ -2040,9 +2134,260 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 	rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
 	UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
 	UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
-	dev_->start();
 
-	dev_->wait_for_frames();
+#ifdef RTABMAP_REALSENSE_SLAM
+	UDEBUG("Setup frame callback");
+	// Define lambda callback for receiving stream data
+	std::function<void(rs::frame)> frameCallback = [this](rs::frame frame)
+	{
+		if(slam_ != 0)
+		{
+			const auto timestampDomain = frame.get_frame_timestamp_domain();
+			if (rs::timestamp_domain::microcontroller != timestampDomain)
+			{
+				UERROR("error: Junk time stamp in stream: %d\twith frame counter: %d",
+						(int)(frame.get_stream_type()), frame.get_frame_number());
+				return ;
+			}
+		}
+
+		int width = frame.get_width();
+		int height = frame.get_height();
+		rs::core::correlated_sample_set sample_set = {};
+
+		rs::core::image_info info =
+		{
+		  width,
+		  height,
+		  rs::utils::convert_pixel_format(frame.get_format()),
+		  frame.get_stride()
+		};
+		cv::Mat image;
+		if(frame.get_format() == rs::format::raw8)
+		{
+			image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
+		}
+		else if(frame.get_format() == rs::format::z16)
+		{
+			image = cv::Mat(height, width, CV_16UC1, (unsigned char*)frame.get_data());
+			if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
+			{
+				bufferedFrames_.find(frame.get_timestamp())->second.second = image.clone();
+				UScopeMutex lock(dataMutex_);
+				bool notify = lastSyncFrames_.first.empty();
+				lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
+				if(notify)
+				{
+					dataReady_.release();
+				}
+				bufferedFrames_.erase(frame.get_timestamp());
+			}
+			else
+			{
+				bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(cv::Mat(), image.clone())));
+			}
+			if(bufferedFrames_.size()>5)
+			{
+				UWARN("Frames cannot be synchronized!");
+				bufferedFrames_.clear();
+			}
+		}
+		else if(frame.get_format() == rs::format::rgb8)
+		{
+			image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
+			{
+				bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
+				UScopeMutex lock(dataMutex_);
+				bool notify = lastSyncFrames_.first.empty();
+				lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
+				if(notify)
+				{
+					dataReady_.release();
+				}
+				bufferedFrames_.erase(frame.get_timestamp());
+			}
+			else
+			{
+				bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
+			}
+			if(bufferedFrames_.size()>5)
+			{
+				UWARN("Frames cannot be synchronized!");
+				bufferedFrames_.clear();
+			}
+			return;
+		}
+		else
+		{
+			return;
+		}
+
+		if(slam_ != 0)
+		{
+			rs::core::stream_type stream = rs::utils::convert_stream_type(frame.get_stream_type());
+			sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data(
+								   & info,
+								   image.data,
+								   stream,
+								   rs::core::image_interface::flag::any,
+								   frame.get_timestamp(),
+								   (uint64_t)frame.get_frame_number(),
+								   rs::core::timestamp_domain::microcontroller);
+
+			UScopeMutex lock(slamLock_);
+			if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
+			{
+				UERROR("error: failed to process sample");
+			}
+			sample_set[stream]->release();
+		}
+	};
+
+	// Setup stream callback for stream
+	if(computeOdometry)
+	{
+		dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
+	}
+	dev_->set_frame_callback(rs::stream::depth, frameCallback);
+	dev_->set_frame_callback(rs::stream::color, frameCallback);
+
+	if (computeOdometry)
+	{
+		dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
+		rs::intrinsics fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+		UINFO("    Fish: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+
+		// Needed to align image timestamps to common clock-domain with the motion events
+		dev_->set_option(rs::option::fisheye_strobe, 1);
+		// This option causes the fisheye image to be aquired in-sync with the depth image.
+		dev_->set_option(rs::option::fisheye_external_trigger, 1);
+		dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
+
+		UDEBUG("Setup motion callback");
+		//define callback to the motion events and set it.
+		std::function<void(rs::motion_data)> motion_callback;
+		motion_callback = [this](rs::motion_data entry)
+		{
+			if ((entry.timestamp_data.source_id != RS_EVENT_IMU_GYRO) &&
+					(entry.timestamp_data.source_id != RS_EVENT_IMU_ACCEL))
+				return;
+
+			rs_event_source motionType = entry.timestamp_data.source_id;
+
+			rs::core::correlated_sample_set sample_set = {};
+			if (motionType == RS_EVENT_IMU_ACCEL)
+			{
+				sample_set[rs::core::motion_type::accel].timestamp = entry.timestamp_data.timestamp;
+				sample_set[rs::core::motion_type::accel].data[0] = (float)entry.axes[0];
+				sample_set[rs::core::motion_type::accel].data[1] = (float)entry.axes[1];
+				sample_set[rs::core::motion_type::accel].data[2] = (float)entry.axes[2];
+				sample_set[rs::core::motion_type::accel].type = rs::core::motion_type::accel;
+				++motionSeq_[0];
+				sample_set[rs::core::motion_type::accel].frame_number = motionSeq_[0];
+			}
+			else if (motionType == RS_EVENT_IMU_GYRO)
+			{
+				sample_set[rs::core::motion_type::gyro].timestamp = entry.timestamp_data.timestamp;
+				sample_set[rs::core::motion_type::gyro].data[0] = (float)entry.axes[0];
+				sample_set[rs::core::motion_type::gyro].data[1] = (float)entry.axes[1];
+				sample_set[rs::core::motion_type::gyro].data[2] = (float)entry.axes[2];
+				sample_set[rs::core::motion_type::gyro].type = rs::core::motion_type::gyro;
+				++motionSeq_[1];
+				sample_set[rs::core::motion_type::gyro].frame_number = motionSeq_[1];
+			}
+
+			UScopeMutex lock(slamLock_);
+			if (slam_->process_sample_set(sample_set) < rs::core::status_no_error)
+			{
+				UERROR("error: failed to process sample");
+			}
+		};
+
+		std::function<void(rs::timestamp_data)> timestamp_callback;
+		timestamp_callback = [](rs::timestamp_data entry) {};
+
+		dev_->enable_motion_tracking(motion_callback, timestamp_callback);
+		UINFO("  enabled accel and gyro stream");
+
+		rs::motion_intrinsics imuIntrinsics;
+		rs::extrinsics fisheye2ImuExtrinsics;
+		rs::extrinsics fisheye2DepthExtrinsics;
+		try
+		{
+			imuIntrinsics = dev_->get_motion_intrinsics();
+			fisheye2ImuExtrinsics = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
+			fisheye2DepthExtrinsics = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
+		}
+		catch (const rs::error & e) {
+			UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
+			return false;
+		}
+
+		UDEBUG("Setup SLAM");
+		UScopeMutex lock(slamLock_);
+		slam_ = new rs::slam::slam();
+		slam_->set_auto_occupancy_map_building(false);
+		slam_->force_relocalization_pose(false);
+
+		rs::core::video_module_interface::supported_module_config supported_config = {};
+		if (slam_->query_supported_module_config(0, supported_config) < rs::core::status_no_error)
+		{
+			UERROR("Failed to query the first supported module configuration");
+			return false;
+		}
+
+		rs::core::video_module_interface::actual_module_config actual_config = {};
+
+		// Set camera intrinsics
+		std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics;
+		intrinsics[rs::core::stream_type::fisheye] = rs::utils::convert_intrinsics(fisheye_intrin);
+		intrinsics[rs::core::stream_type::depth] = rs::utils::convert_intrinsics(depth_intrin);
+
+		if(!setStreamConfigIntrin(rs::core::stream_type::fisheye, intrinsics, supported_config, actual_config))
+		{
+			return false;
+		}
+		if(!setStreamConfigIntrin(rs::core::stream_type::depth, intrinsics, supported_config, actual_config))
+		{
+			return false;
+		}
+
+		// Set IMU intrinsics
+		actual_config[rs::core::motion_type::accel].is_enabled = true;
+		actual_config[rs::core::motion_type::gyro].is_enabled = true;
+		actual_config[rs::core::motion_type::gyro].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.gyro);
+		actual_config[rs::core::motion_type::accel].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.acc);
+
+		// Set extrinsics
+		actual_config[rs::core::stream_type::fisheye].extrinsics_motion = rs::utils::convert_extrinsics(fisheye2ImuExtrinsics);
+		actual_config[rs::core::stream_type::fisheye].extrinsics = rs::utils::convert_extrinsics(fisheye2DepthExtrinsics);
+
+		UDEBUG("Set SLAM config");
+		// Set actual config
+		if (slam_->set_module_config(actual_config) < rs::core::status_no_error)
+		{
+			UERROR("error : failed to set the enabled module configuration");
+			return false;
+		}
+
+		dev_->start(rs::source::all_sources);
+	}
+	else
+	{
+		dev_->start();
+	}
+#else
+	dev_->start();
+	try {
+		dev_->wait_for_frames();
+	}
+	catch (const rs::error & e)
+	{
+		UERROR("Exception: %s", e.what());
+	}
+#endif
+
 	uSleep(1000); // ignore the first frames
 	UINFO("Enabling streams...done!");
 
@@ -2074,26 +2419,75 @@ std::string CameraRealSense::getSerial() const
 	return "NA";
 }
 
+bool CameraRealSense::odomProvided() const
+{
+#ifdef RTABMAP_REALSENSE_SLAM
+	return slam_!=0;
+#else
+	return false;
+#endif
+}
+
+#ifdef RTABMAP_REALSENSE_SLAM
+Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose)
+{
+	return Transform(
+			pose.m_data[0], pose.m_data[1], pose.m_data[2], pose.m_data[3],
+			pose.m_data[4], pose.m_data[5], pose.m_data[6], pose.m_data[7],
+			pose.m_data[8], pose.m_data[9], pose.m_data[10], pose.m_data[11]);
+}
+#endif
+
 SensorData CameraRealSense::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_REALSENSE
 	if (dev_)
 	{
-		dev_->wait_for_frames();
-
-		// Retrieve our images
-		const uint16_t * depth_image = (const uint16_t *)dev_->get_frame_data(rs::stream::depth);
-		const uint8_t * color_image = (const uint8_t *)dev_->get_frame_data(rs::stream::color);
+		cv::Mat rgb;
+		cv::Mat depthIn;
 
 		// Retrieve camera parameters for mapping between depth and color
 		rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
 		rs::extrinsics depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::color);
 		rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+
+#ifdef RTABMAP_REALSENSE_SLAM
+		if(!dataReady_.acquire(1, 5000))
+		{
+			UWARN("Not received new frames since 5 seconds, end of stream reached!");
+			return data;
+		}
+		{
+			UScopeMutex lock(dataMutex_);
+			rgb = lastSyncFrames_.first;
+			depthIn = lastSyncFrames_.second;
+			lastSyncFrames_.first = cv::Mat();
+			lastSyncFrames_.second = cv::Mat();
+		}
+
+		if(rgb.empty() || depthIn.empty())
+		{
+			return data;
+		}
+#else
+		try {
+			dev_->wait_for_frames();
+		}
+		catch (const rs::error & e)
+		{
+			UERROR("Exception: %s", e.what());
+			return data;
+		}
+
+		// Retrieve our images
+		depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
+		rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
+#endif
+
 		float scale = dev_->get_depth_scale();
 
 		// factory registration...
-		cv::Mat rgb = cv::Mat(cv::Size(color_intrin.width, color_intrin.height), CV_8UC3, (void*)color_image);
 		cv::Mat bgr;
 		cv::cvtColor(rgb, bgr, CV_RGB2BGR);
 
@@ -2132,7 +2526,7 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 			for (int dx = 0; dx < depth_intrin.width; ++dx)
 			{
 				// Retrieve the 16-bit depth value and map it into a depth in meters
-				uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
+				uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
 				float depth_in_meters = depth_value * scale;
 
 				// Skip over pixels with a depth value of zero, which is used to indicate no data
@@ -2165,6 +2559,22 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 		if (!bgr.empty() && !depth.empty())
 		{
 			data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now());
+#ifdef RTABMAP_REALSENSE_SLAM
+			if(info && slam_)
+			{
+				UScopeMutex lock(slamLock_);
+				rs::slam::PoseMatrix4f pose;
+				if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
+				{
+					Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+					info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+				}
+				else
+				{
+					UERROR("Failed getting odometry pose");
+				}
+			}
+#endif
 		}
 	}
 	else

corelib/src/CameraStereo.cpp

@@ -49,7 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 
 #ifdef RTABMAP_ZED
-#include <zed/Camera.hpp>
+#include <sl/Camera.hpp>
 #endif
 
 namespace rtabmap
@@ -785,9 +785,9 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
-	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
-	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
-	UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
+	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
+	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
+	UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
 	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
 #endif
 }
@@ -819,9 +819,9 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
-	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
-	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
-	UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
+	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
+	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
+	UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
 	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
 #endif
 }
@@ -847,62 +847,79 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	}
 	
 	lost_ = true;
+
+	sl::InitParameters param;
+	param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
+	param.camera_fps=getImageRate();
+	param.camera_linux_id=usbDevice_;
+	param.depth_mode=(sl::DEPTH_MODE)quality_;
+	param.coordinate_units=sl::UNIT_METER;
+	param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
+	param.sdk_verbose=false;
+	param.sdk_gpu_id=-1;
+	param.depth_minimum_distance=-1;
+	param.camera_disable_self_calib=!selfCalibration_;
+
+	sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
 	if(src_ == CameraVideo::kVideoFile)
 	{
 		UINFO("svo file = %s", svoFilePath_.c_str());
-		zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
+		zed_ = new sl::Camera(); // Use in SVO playback mode
+		param.svo_input_filename=svoFilePath_.c_str();
+		r = zed_->open(param);
 	}
 	else
 	{
 		UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
-		zed_ = new sl::zed::Camera((sl::zed::ZEDResolution_mode)resolution_, getImageRate(), usbDevice_); // Use in Live Mode
+		zed_ = new sl::Camera(); // Use in Live Mode
+		r = zed_->open(param);
 	}
 
-	sl::zed::InitParams parameters(
-		(sl::zed::MODE)quality_, //MODE
-		(sl::zed::UNIT)sl::zed::METER,  //UNIT
-		(sl::zed::COORDINATE_SYSTEM)sl::zed::IMAGE,  //COORDINATE_SYSTEM
-		false,  // verbose
-		-1,     //device (GPU)
-		-1.,    //minDist
-		!selfCalibration_,  //disableSelfCalib: false = self calibrated
-		false); //vflip
-
-	UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
-			quality_, sl::zed::METER, sl::zed::IMAGE, selfCalibration_?"true":"false");
-	sl::zed::ERRCODE err = zed_->init(parameters);
-	UDEBUG("");
-
-	// Quit if an error occurred
-	if (err != sl::zed::SUCCESS)
+	if(r!=sl::ERROR_CODE::SUCCESS)
 	{
-		UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err).c_str());
+		UERROR("Camera initialization failed: \"%s\"", errorCode2str(r).c_str());
 		delete zed_;
 		zed_ = 0;
 		return false;
 	}
 
+
+	UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
+	      quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
+	UDEBUG("");
+
 	zed_->setConfidenceThreshold(confidenceThr_);
 
 	if (computeOdometry_)
 	{
-		Eigen::Matrix4f initPose;
-		initPose.setIdentity(4, 4);
-		zed_->enableTracking(initPose, false);
+		sl::TrackingParameters tparam;
+		tparam.enable_spatial_memory=false;
+		zed_->enableTracking(tparam);
 	}
 
-	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
-	sl::zed::resolution res = zed_->getImageSize();
+	sl::CameraInformation infos = zed_->getCameraInformation();
+	sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
+	sl::Resolution res = stereoParams->left_cam.image_size;
 				
 	stereoModel_ = StereoCameraModel(
-		stereoParams->LeftCam.fx, 
-		stereoParams->LeftCam.fy, 
-		stereoParams->LeftCam.cx, 
-		stereoParams->LeftCam.cy, 
-		stereoParams->baseline,
+		stereoParams->left_cam.fx, 
+		stereoParams->left_cam.fy, 
+		stereoParams->left_cam.cx, 
+		stereoParams->left_cam.cy, 
+		stereoParams->T[0],//baseline
 		this->getLocalTransform(),
 		cv::Size(res.width, res.height));
 
+	UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
+			stereoParams->left_cam.fx,
+			stereoParams->left_cam.fy,
+			stereoParams->left_cam.cx,
+			stereoParams->left_cam.cy,
+			stereoParams->T[0],//baseline
+			(int)res.width,
+			(int)res.height,
+			this->getLocalTransform().prettyPrint().c_str());
+
 	return true;
 #else
 	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
@@ -924,7 +941,7 @@ std::string CameraStereoZed::getSerial() const
 #ifdef RTABMAP_ZED
 	if(zed_)
 	{
-		return uFormat("%x", zed_->getZEDSerial());
+		return uFormat("%x", zed_->getCameraInformation ().serial_number);
 	}
 #endif
 	return "";
@@ -938,25 +955,55 @@ bool CameraStereoZed::odomProvided() const
 	return false;
 #endif
 }
+#ifdef RTABMAP_ZED
+static cv::Mat slMat2cvMat(sl::Mat& input) {
+	//convert MAT_TYPE to CV_TYPE
+	int cv_type = -1;
+	switch (input.getDataType()) {
+	case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
+	case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
+	case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
+	case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
+	case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
+	case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
+	case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
+	case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
+	default: break;
+	}	
+	// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
+	//cv::Mat and sl::Mat will share the same memory pointer
+	return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
+}
+
+Transform zedPoseToTransform(const sl::Pose & pose)
+{
+	return Transform(
+			pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
+			pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
+			pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
+}
+#endif
 
 SensorData CameraStereoZed::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_ZED
+	sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
 	if(zed_)
 	{
 		UTimer timer;
-		bool res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
+		bool res = zed_->grab(rparam);
 		while (src_ == CameraVideo::kUsbDevice && res && timer.elapsed() < 2.0)
 		{
 			// maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
 			uSleep(10);
-			res = zed_->grab((sl::zed::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, false);
+			res = zed_->grab(rparam);
 		}
 		if(!res)
 		{
 			// get left image
-			cv::Mat rgbaLeft = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::LEFT)));
+			sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_LEFT);
+			cv::Mat rgbaLeft = slMat2cvMat(tmp);
 
 			cv::Mat left;
 			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
@@ -965,14 +1012,17 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 			{
 				// get depth image
 				cv::Mat depth;
-				slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
+				sl::Mat tmp;
+				zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
+				slMat2cvMat(tmp).copyTo(depth);
 
 				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
 			}
 			else
 			{
 				// get right image
-				cv::Mat rgbaRight = sl::zed::slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::RIGHT)));
+				sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
+				cv::Mat rgbaRight = slMat2cvMat(tmp);
 				cv::Mat right;
 				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
 			
@@ -981,35 +1031,43 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 
 			if (computeOdometry_ && info)
 			{
-				Eigen::Matrix4f path;
-				int trackingConfidence = zed_->getTrackingConfidence();
-				if (trackingConfidence)
+				sl::Pose pose;
+				zed_->getPosition(pose);
+				int trackingConfidence = pose.pose_confidence;
+				// FIXME What does pose_confidence == -1 mean?
+				if (trackingConfidence>0)
 				{
-					zed_->getPosition(path);
-					info->odomPose = Transform::fromEigen4f(path);
+					info->odomPose = zedPoseToTransform(pose);
 					if (!info->odomPose.isNull())
 					{
 						//transform x->forward, y->left, z->up
 						Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 						info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
-					}
-					if (lost_)
-					{
-						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
-						lost_ = false;
-						UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+
+						if (lost_)
+						{
+							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+							lost_ = false;
+							UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+						}
+						else
+						{
+							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+							UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+						}
 					}
 					else
 					{
-						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
-						UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+						lost_ = true;
+						UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
 					}
 				}
 				else
 				{
 					info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
 					lost_ = true;
-					UWARN("ZED lost!");
+					UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
 				}
 			}
 		}

corelib/src/CameraThread.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/StereoDense.h"
+#include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
 
 #include <rtabmap/utilite/UTimer.h>
@@ -129,181 +130,9 @@ void CameraThread::mainLoop()
 	CameraInfo info;
 	SensorData data = _camera->takeImage(&info);
 
-	if(!data.imageRaw().empty())
+	if(!data.imageRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
-		if(_colorOnly && !data.depthRaw().empty())
-		{
-			data.setDepthOrRightRaw(cv::Mat());
-		}
-
-		if(_distortionModel && !data.depthRaw().empty())
-		{
-			UTimer timer;
-			if(_distortionModel->getWidth() == data.depthRaw().cols &&
-			   _distortionModel->getHeight() == data.depthRaw().rows	)
-			{
-				cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
-				_distortionModel->undistort(depth);
-				data.setDepthOrRightRaw(depth);
-			}
-			else
-			{
-				UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
-						_distortionModel->getWidth(), _distortionModel->getHeight(),
-						data.depthRaw().cols, data.depthRaw().rows);
-			}
-			info.timeUndistortDepth = timer.ticks();
-		}
-
-		if(_bilateralFiltering && !data.depthRaw().empty())
-		{
-			UTimer timer;
-			data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
-			info.timeBilateralFiltering = timer.ticks();
-		}
-
-		if(_imageDecimation>1 && !data.imageRaw().empty())
-		{
-			UDEBUG("");
-			UTimer timer;
-			if(!data.depthRaw().empty() &&
-			   !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
-			{
-				UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
-					   "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
-			}
-			else
-			{
-				data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
-				data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
-				std::vector<CameraModel> models = data.cameraModels();
-				for(unsigned int i=0; i<models.size(); ++i)
-				{
-					if(models[i].isValidForProjection())
-					{
-						models[i] = models[i].scaled(1.0/double(_imageDecimation));
-					}
-				}
-				data.setCameraModels(models);
-				StereoCameraModel stereoModel = data.stereoCameraModel();
-				if(stereoModel.isValidForProjection())
-				{
-					stereoModel.scale(1.0/double(_imageDecimation));
-					data.setStereoCameraModel(stereoModel);
-				}
-			}
-			info.timeImageDecimation = timer.ticks();
-		}
-		if(_mirroring && data.cameraModels().size() == 1)
-		{
-			UDEBUG("");
-			UTimer timer;
-			cv::Mat tmpRgb;
-			cv::flip(data.imageRaw(), tmpRgb, 1);
-			data.setImageRaw(tmpRgb);
-			UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
-			if(data.cameraModels().size() && data.cameraModels()[0].cx())
-			{
-				CameraModel tmpModel(
-						data.cameraModels()[0].fx(),
-						data.cameraModels()[0].fy(),
-						float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
-						data.cameraModels()[0].cy(),
-						data.cameraModels()[0].localTransform());
-				data.setCameraModel(tmpModel);
-			}
-			if(!data.depthRaw().empty())
-			{
-				cv::Mat tmpDepth;
-				cv::flip(data.depthRaw(), tmpDepth, 1);
-				data.setDepthOrRightRaw(tmpDepth);
-			}
-			info.timeMirroring = timer.ticks();
-		}
-		if(_stereoToDepth && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
-		{
-			UDEBUG("");
-			UTimer timer;
-			cv::Mat depth = util2d::depthFromDisparity(
-					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
-					data.stereoCameraModel().left().fx(),
-					data.stereoCameraModel().baseline());
-			// set Tx for stereo bundle adjustment (when used)
-			CameraModel model = CameraModel(
-					data.stereoCameraModel().left().fx(),
-					data.stereoCameraModel().left().fy(),
-					data.stereoCameraModel().left().cx(),
-					data.stereoCameraModel().left().cy(),
-					data.stereoCameraModel().localTransform(),
-					-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
-			data.setCameraModel(model);
-			data.setDepthOrRightRaw(depth);
-			data.setStereoCameraModel(StereoCameraModel());
-			info.timeDisparity = timer.ticks();
-			UDEBUG("Computing disparity = %f s", info.timeDisparity);
-		}
-		if(_scanFromDepth &&
-			data.cameraModels().size() &&
-			data.cameraModels().at(0).isValidForProjection() &&
-			!data.depthRaw().empty())
-		{
-			UDEBUG("");
-			if(data.laserScanRaw().empty())
-			{
-				UASSERT(_scanDecimation >= 1);
-				UTimer timer;
-				pcl::IndicesPtr validIndices(new std::vector<int>);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
-						data,
-						_scanDecimation,
-						_scanMaxDepth,
-						_scanMinDepth,
-						validIndices.get());
-				float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
-				cv::Mat scan;
-				const Transform & baseToScan = data.cameraModels()[0].localTransform();
-				if(validIndices->size())
-				{
-					if(_scanVoxelSize>0.0f)
-					{
-						cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
-						float ratio = float(cloud->size()) / float(validIndices->size());
-						maxPoints = ratio * maxPoints;
-					}
-					else if(!cloud->is_dense)
-					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
-						pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
-						cloud = denseCloud;
-					}
-
-					if(cloud->size())
-					{
-						if(_scanNormalsK>0)
-						{
-							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::concatenateFields(*cloud, *normals, *cloudNormals);
-							scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
-						}
-						else
-						{
-							scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
-						}
-					}
-				}
-				data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
-				info.timeScanFromDepth = timer.ticks();
-				UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
-			}
-			else
-			{
-				UWARN("Option to create laser scan from depth image is enabled, but "
-					  "there is already a laser scan in the captured sensor data. Scan from "
-					  "depth will not be created.");
-			}
-		}
+		postUpdate(&data, &info);
 
 		info.cameraName = _camera->getSerial();
 		info.timeTotal = totalTime.ticks();
@@ -343,4 +172,181 @@ void CameraThread::mainLoopKill()
 	}
 }
 
+void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
+{
+	UASSERT(dataPtr!=0);
+	SensorData & data = *dataPtr;
+	if(_colorOnly && !data.depthRaw().empty())
+	{
+		data.setDepthOrRightRaw(cv::Mat());
+	}
+
+	if(_distortionModel && !data.depthRaw().empty())
+	{
+		UTimer timer;
+		if(_distortionModel->getWidth() == data.depthRaw().cols &&
+		   _distortionModel->getHeight() == data.depthRaw().rows	)
+		{
+			cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
+			_distortionModel->undistort(depth);
+			data.setDepthOrRightRaw(depth);
+		}
+		else
+		{
+			UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
+					_distortionModel->getWidth(), _distortionModel->getHeight(),
+					data.depthRaw().cols, data.depthRaw().rows);
+		}
+		if(info) info->timeUndistortDepth = timer.ticks();
+	}
+
+	if(_bilateralFiltering && !data.depthRaw().empty())
+	{
+		UTimer timer;
+		data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
+		if(info) info->timeBilateralFiltering = timer.ticks();
+	}
+
+	if(_imageDecimation>1 && !data.imageRaw().empty())
+	{
+		UDEBUG("");
+		UTimer timer;
+		if(!data.depthRaw().empty() &&
+		   !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
+		{
+			UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
+				   "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
+		}
+		else
+		{
+			data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
+			data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
+			std::vector<CameraModel> models = data.cameraModels();
+			for(unsigned int i=0; i<models.size(); ++i)
+			{
+				if(models[i].isValidForProjection())
+				{
+					models[i] = models[i].scaled(1.0/double(_imageDecimation));
+				}
+			}
+			data.setCameraModels(models);
+			StereoCameraModel stereoModel = data.stereoCameraModel();
+			if(stereoModel.isValidForProjection())
+			{
+				stereoModel.scale(1.0/double(_imageDecimation));
+				data.setStereoCameraModel(stereoModel);
+			}
+		}
+		if(info) info->timeImageDecimation = timer.ticks();
+	}
+	if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
+	{
+		UDEBUG("");
+		UTimer timer;
+		cv::Mat tmpRgb;
+		cv::flip(data.imageRaw(), tmpRgb, 1);
+		data.setImageRaw(tmpRgb);
+		UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
+		if(data.cameraModels().size() && data.cameraModels()[0].cx())
+		{
+			CameraModel tmpModel(
+					data.cameraModels()[0].fx(),
+					data.cameraModels()[0].fy(),
+					float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
+					data.cameraModels()[0].cy(),
+					data.cameraModels()[0].localTransform());
+			data.setCameraModel(tmpModel);
+		}
+		if(!data.depthRaw().empty())
+		{
+			cv::Mat tmpDepth;
+			cv::flip(data.depthRaw(), tmpDepth, 1);
+			data.setDepthOrRightRaw(tmpDepth);
+		}
+		if(info) info->timeMirroring = timer.ticks();
+	}
+	if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
+	{
+		UDEBUG("");
+		UTimer timer;
+		cv::Mat depth = util2d::depthFromDisparity(
+				_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
+				data.stereoCameraModel().left().fx(),
+				data.stereoCameraModel().baseline());
+		// set Tx for stereo bundle adjustment (when used)
+		CameraModel model = CameraModel(
+				data.stereoCameraModel().left().fx(),
+				data.stereoCameraModel().left().fy(),
+				data.stereoCameraModel().left().cx(),
+				data.stereoCameraModel().left().cy(),
+				data.stereoCameraModel().localTransform(),
+				-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
+		data.setCameraModel(model);
+		data.setDepthOrRightRaw(depth);
+		data.setStereoCameraModel(StereoCameraModel());
+		if(info) info->timeDisparity = timer.ticks();
+	}
+	if(_scanFromDepth &&
+		data.cameraModels().size() &&
+		data.cameraModels().at(0).isValidForProjection() &&
+		!data.depthRaw().empty())
+	{
+		UDEBUG("");
+		if(data.laserScanRaw().empty())
+		{
+			UASSERT(_scanDecimation >= 1);
+			UTimer timer;
+			pcl::IndicesPtr validIndices(new std::vector<int>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+					data,
+					_scanDecimation,
+					_scanMaxDepth,
+					_scanMinDepth,
+					validIndices.get());
+			float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
+			cv::Mat scan;
+			const Transform & baseToScan = data.cameraModels()[0].localTransform();
+			if(validIndices->size())
+			{
+				if(_scanVoxelSize>0.0f)
+				{
+					cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
+					float ratio = float(cloud->size()) / float(validIndices->size());
+					maxPoints = ratio * maxPoints;
+				}
+				else if(!cloud->is_dense)
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
+					cloud = denseCloud;
+				}
+
+				if(cloud->size())
+				{
+					if(_scanNormalsK>0)
+					{
+						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::concatenateFields(*cloud, *normals, *cloudNormals);
+						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
+					}
+					else
+					{
+						scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
+					}
+				}
+			}
+			data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
+			if(info) info->timeScanFromDepth = timer.ticks();
+		}
+		else
+		{
+			UWARN("Option to create laser scan from depth image is enabled, but "
+				  "there is already a laser scan in the captured sensor data. Scan from "
+				  "depth will not be created.");
+		}
+	}
+}
+
 } // namespace rtabmap

corelib/src/Compression.cpp

@@ -51,32 +51,46 @@ CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
 {}
 void CompressionThread::mainLoop()
 {
-	if(compressMode_)
+	try
 	{
-		if(!uncompressedData_.empty())
+		if(compressMode_)
 		{
-			if(image_)
+			if(!uncompressedData_.empty())
 			{
-				compressedData_ = compressImage2(uncompressedData_, format_);
+				if(image_)
+				{
+					compressedData_ = compressImage2(uncompressedData_, format_);
+				}
+				else
+				{
+					compressedData_ = compressData2(uncompressedData_);
+				}
 			}
-			else
+		}
+		else // uncompress
+		{
+			if(!compressedData_.empty())
 			{
-				compressedData_ = compressData2(uncompressedData_);
+				if(image_)
+				{
+					uncompressedData_ = uncompressImage(compressedData_);
+				}
+				else
+				{
+					uncompressedData_ = uncompressData(compressedData_);
+				}
 			}
 		}
 	}
-	else // uncompress
-	{
-		if(!compressedData_.empty())
+	catch (cv::Exception & e) {
+		UERROR("Exception while compressing/uncompressing data: %s", e.what());
+		if(compressMode_)
 		{
-			if(image_)
-			{
-				uncompressedData_ = uncompressImage(compressedData_);
-			}
-			else
-			{
-				uncompressedData_ = uncompressData(compressedData_);
-			}
+			compressedData_ = cv::Mat();
+		}
+		else
+		{
+			uncompressedData_ = cv::Mat();
 		}
 	}
 	this->kill();

corelib/src/DBDriver.cpp

@@ -116,6 +116,22 @@ long DBDriver::getMemoryUsed() const
 	return bytes;
 }
 
+long DBDriver::getNodesMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getNodesMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
+long DBDriver::getLinksMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getLinksMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
 long DBDriver::getImagesMemoryUsed() const
 {
 	long bytes;
@@ -132,6 +148,22 @@ long DBDriver::getDepthImagesMemoryUsed() const
 	_dbSafeAccessMutex.unlock();
 	return bytes;
 }
+long DBDriver::getCalibrationsMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getCalibrationsMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
+long DBDriver::getGridsMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getGridsMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
 long DBDriver::getLaserScansMemoryUsed() const
 {
 	long bytes;
@@ -156,6 +188,22 @@ long DBDriver::getWordsMemoryUsed() const
 	_dbSafeAccessMutex.unlock();
 	return bytes;
 }
+long DBDriver::getFeaturesMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getFeaturesMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
+long DBDriver::getStatisticsMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getStatisticsMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
 int DBDriver::getLastNodesSize() const
 {
 	int nodes;
@@ -632,6 +680,30 @@ bool DBDriver::getCalibration(
 	return found;
 }
 
+bool DBDriver::getLaserScanInfo(
+		int signatureId,
+		LaserScanInfo & info) const
+{
+	UDEBUG("");
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		info = _trashSignatures.at(signatureId)->sensorData().laserScanInfo();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		found = this->getLaserScanInfoQuery(signatureId, info);
+		_dbSafeAccessMutex.unlock();
+	}
+	return found;
+}
+
 bool DBDriver::getNodeInfo(
 		int signatureId,
 		Transform & pose,
@@ -639,7 +711,8 @@ bool DBDriver::getNodeInfo(
 		int & weight,
 		std::string & label,
 		double & stamp,
-		Transform & groundTruthPose) const
+		Transform & groundTruthPose,
+		std::vector<float> & velocity) const
 {
 	bool found = false;
 	// look in the trash
@@ -659,7 +732,7 @@ bool DBDriver::getNodeInfo(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;
@@ -951,6 +1024,53 @@ void DBDriver::addStatistics(const Statistics & statistics) const
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::savePreviewImage(const cv::Mat & image) const
+{
+	_dbSafeAccessMutex.lock();
+	savePreviewImageQuery(image);
+	_dbSafeAccessMutex.unlock();
+}
+
+cv::Mat DBDriver::loadPreviewImage() const
+{
+	_dbSafeAccessMutex.lock();
+	cv::Mat image = loadPreviewImageQuery();
+	_dbSafeAccessMutex.unlock();
+	return image;
+}
+
+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,
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+			const cv::Mat & textures) const
+{
+	_dbSafeAccessMutex.lock();
+	saveOptimizedMeshQuery(cloud, poses, 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,
+#else
+				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+				cv::Mat * textures) const
+{
+	_dbSafeAccessMutex.lock();
+	cv::Mat cloud = loadOptimizedMeshQuery(poses, polygons, texCoords, textures);
+	_dbSafeAccessMutex.unlock();
+	return cloud;
+}
+
 void DBDriver::generateGraph(
 		const std::string & fileName,
 		const std::set<int> & idsInput,
@@ -1102,7 +1222,7 @@ void DBDriver::generateGraph(
 									 weightNeighbor,
 									 colorG);
 						 }
-						 else
+						 else if(iter->first != id)
 						 {
 							 //child
 							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",

corelib/src/DBDriverSqlite3.h

@@ -53,11 +53,17 @@ private:
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
+	virtual long getNodesMemoryUsedQuery() const;
+	virtual long getLinksMemoryUsedQuery() const;
 	virtual long getImagesMemoryUsedQuery() const;
 	virtual long getDepthImagesMemoryUsedQuery() const;
+	virtual long getCalibrationsMemoryUsedQuery() const;
+	virtual long getGridsMemoryUsedQuery() const;
 	virtual long getLaserScansMemoryUsedQuery() const;
 	virtual long getUserDataMemoryUsedQuery() const;
 	virtual long getWordsMemoryUsedQuery() const;
+	virtual long getFeaturesMemoryUsedQuery() const;
+	virtual long getStatisticsMemoryUsedQuery() const;
 	virtual int getLastNodesSizeQuery() const;
 	virtual int getLastDictionarySizeQuery() const;
 	virtual int getTotalNodesSizeQuery() const;
@@ -85,10 +91,31 @@ private:
 			const cv::Point3f & viewpoint) const;
 
 	virtual void updateDepthImageQuery(
-				int nodeId,
-				const cv::Mat & image) const;
+			int nodeId,
+			const cv::Mat & image) const;
 
 	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const;
+	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void 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,
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#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,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+			cv::Mat * textures) const;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
@@ -99,7 +126,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 getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) 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 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;

corelib/src/DBReader.cpp

@@ -267,7 +267,8 @@ SensorData DBReader::captureImage(CameraInfo * info)
 						double stamp;
 						int mapId;
 						Transform localTransform, pose, groundTruth;
-						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
+						std::vector<float> velocity;
+						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
 						if(previousStamp && stamp && stamp > previousStamp)
 						{
 							delay = stamp - previousStamp;
@@ -320,7 +321,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			std::string label;
 			double stamp;
 			Transform groundTruth;
-			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
+			std::vector<float> velocity;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
 
 			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
@@ -357,7 +359,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 
 			int seq = *_currentId;
 			++_currentId;
-			if(data.imageCompressed().empty())
+			if(data.imageCompressed().empty() && weight>=0)
 			{
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}

corelib/src/Features2d.cpp

@@ -414,8 +414,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	if(type == Feature2D::kFeatureFastBrief ||
 	   type == Feature2D::kFeatureFastFreak ||
 	   type == Feature2D::kFeatureGfttBrief ||
-	   type == Feature2D::kFeatureGfttFreak ||
-	   type == Feature2D::kFeatureFreak)
+	   type == Feature2D::kFeatureGfttFreak)
 	{
 		UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
 		type = Feature2D::kFeatureOrb;
@@ -424,10 +423,15 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #endif
 
 #if CV_MAJOR_VERSION < 3
-	if(type == Feature2D::kFeatureFreak)
+	if(type == Feature2D::kFeatureKaze)
 	{
-		UWARN("FREAK detector/descriptor can be used only with OpenCV3. GFTT/FREAK is used instead.");
-		type = Feature2D::kFeatureGfttFreak;
+#ifdef RTABMAP_NONFREE
+		UWARN("KAZE detector/descriptor can be used only with OpenCV3. SURF is used instead.");
+		type = Feature2D::kFeatureSurf;
+#else
+		UWARN("KAZE detector/descriptor can be used only with OpenCV3. ORB is used instead.");
+		type = Feature2D::kFeatureOrb;
+#endif
 	}
 #endif
 
@@ -461,6 +465,9 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureBrisk:
 		feature2D = new BRISK(parameters);
 		break;
+	case Feature2D::kFeatureKaze:
+		feature2D = new KAZE(parameters);
+		break;
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -567,11 +574,15 @@ cv::Mat Feature2D::generateDescriptors(
 		const cv::Mat & image,
 		std::vector<cv::KeyPoint> & keypoints) const
 {
-	UASSERT(!image.empty());
-	UASSERT(image.type() == CV_8UC1);
-	cv::Mat descriptors = generateDescriptorsImpl(image, keypoints);
-	UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
-	UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
+	cv::Mat descriptors;
+	if(keypoints.size())
+	{
+		UASSERT(!image.empty());
+		UASSERT(image.type() == CV_8UC1);
+		descriptors = generateDescriptorsImpl(image, keypoints);
+		UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
+		UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
+	}
 	return descriptors;
 }
 
@@ -580,47 +591,50 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 		const std::vector<cv::KeyPoint> & keypoints) const
 {
 	std::vector<cv::Point3f> keypoints3D;
-	if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
+	if(keypoints.size())
 	{
-		//stereo
-		cv::Mat imageMono;
-		// convert to grayscale
-		if(data.imageRaw().channels() > 1)
+		if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
 		{
-			cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
+			//stereo
+			cv::Mat imageMono;
+			// convert to grayscale
+			if(data.imageRaw().channels() > 1)
+			{
+				cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
+			}
+			else
+			{
+				imageMono = data.imageRaw();
+			}
+
+			std::vector<cv::Point2f> leftCorners;
+			cv::KeyPoint::convert(keypoints, leftCorners);
+			std::vector<unsigned char> status;
+
+			std::vector<cv::Point2f> rightCorners;
+			rightCorners = _stereo->computeCorrespondences(
+					imageMono,
+					data.rightRaw(),
+					leftCorners,
+					status);
+
+			keypoints3D = util3d::generateKeypoints3DStereo(
+					leftCorners,
+					rightCorners,
+					data.stereoCameraModel(),
+					status,
+					_minDepth,
+					_maxDepth);
 		}
-		else
+		else if(!data.depthRaw().empty() && data.cameraModels().size())
 		{
-			imageMono = data.imageRaw();
+			keypoints3D = util3d::generateKeypoints3DDepth(
+					keypoints,
+					data.depthOrRightRaw(),
+					data.cameraModels(),
+					_minDepth,
+					_maxDepth);
 		}
-
-		std::vector<cv::Point2f> leftCorners;
-		cv::KeyPoint::convert(keypoints, leftCorners);
-		std::vector<unsigned char> status;
-
-		std::vector<cv::Point2f> rightCorners;
-		rightCorners = _stereo->computeCorrespondences(
-				imageMono,
-				data.rightRaw(),
-				leftCorners,
-				status);
-
-		keypoints3D = util3d::generateKeypoints3DStereo(
-				leftCorners,
-				rightCorners,
-				data.stereoCameraModel(),
-				status,
-				_minDepth,
-				_maxDepth);
-	}
-	else if(!data.depthRaw().empty() && data.cameraModels().size())
-	{
-		keypoints3D = util3d::generateKeypoints3DDepth(
-				keypoints,
-				data.depthOrRightRaw(),
-				data.cameraModels(),
-				_minDepth,
-				_maxDepth);
 	}
 
 	return keypoints3D;
@@ -1462,71 +1476,66 @@ cv::Mat BRISK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Ke
 	return descriptors;
 }
 
-
 //////////////////////////
-//FREAK
+//KAZE
 //////////////////////////
-FREAK::FREAK(const ParametersMap & parameters) :
-	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
-	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
-	patternScale_(Parameters::defaultFREAKPatternScale()),
-	nOctaves_(Parameters::defaultFREAKNOctaves())
+KAZE::KAZE(const ParametersMap & parameters) :
+		extended_(Parameters::defaultKAZEExtended()),
+		upright_(Parameters::defaultKAZEUpright()),
+		threshold_(Parameters::defaultKAZEThreshold()),
+		nOctaves_(Parameters::defaultKAZENOctaves()),
+		nOctaveLayers_(Parameters::defaultKAZENOctaveLayers()),
+		diffusivity_(Parameters::defaultKAZEDiffusivity())
 {
 	parseParameters(parameters);
 }
 
-FREAK::~FREAK()
+KAZE::~KAZE()
 {
 }
 
-void FREAK::parseParameters(const ParametersMap & parameters)
+void KAZE::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
-	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
-	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
-	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+	Parameters::parse(parameters, Parameters::kKAZEExtended(), extended_);
+	Parameters::parse(parameters, Parameters::kKAZEUpright(), upright_);
+	Parameters::parse(parameters, Parameters::kKAZEThreshold(), threshold_);
+	Parameters::parse(parameters, Parameters::kKAZENOctaves(), nOctaves_);
+	Parameters::parse(parameters, Parameters::kKAZENOctaveLayers(), nOctaveLayers_);
+	Parameters::parse(parameters, Parameters::kKAZEDiffusivity(), diffusivity_);
 
-#if CV_MAJOR_VERSION < 3
-	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#if CV_MAJOR_VERSION > 2
+	kaze_ = cv::KAZE::create(extended_, upright_, threshold_, nOctaves_, nOctaveLayers_, diffusivity_);
 #else
-#ifdef HAVE_OPENCV_XFEATURES2D
-	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
-#else
-	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
-#endif
+	UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
 #endif
 }
 
-std::vector<cv::KeyPoint> FREAK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> KAZE::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#ifdef HAVE_OPENCV_XFEATURES2D
+#if CV_MAJOR_VERSION > 2
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if (!mask.empty())
 	{
 		maskRoi = cv::Mat(mask, roi);
 	}
-	_freak->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
+	kaze_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #else
-	UWARN("RTAB-Map is not built with OpenCV3 xfeatures2d module so Freak (for keypoint detection) cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
 #endif
 	return keypoints;
 }
 
-cv::Mat FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+cv::Mat KAZE::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#if CV_MAJOR_VERSION < 3
-	_freak->compute(image, keypoints, descriptors);
+#if CV_MAJOR_VERSION > 2
+	kaze_->compute(image, keypoints, descriptors);
 #else
-#ifdef HAVE_OPENCV_XFEATURES2D
-	_freak->compute(image, keypoints, descriptors);
-#else
-	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
-#endif
+	UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
 #endif
 	return descriptors;
 }

corelib/src/GainCompensator.cpp

@@ -342,7 +342,6 @@ void feedImpl(
 	cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
 	cv::solve(A, b, gainsGray);
 
-
 	cv::solve(AR, b, gainsR);
 	cv::solve(AG, b, gainsG);
 	cv::solve(AB, b, gainsB);
@@ -395,11 +394,12 @@ void applyImpl(
 		int index,
 		typename pcl::PointCloud<PointT>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		const cv::Mat_<double> & gains)
+		const cv::Mat_<double> & gains,
+		bool rgb)
 {
-	double gainR = gains(index, 1);
-	double gainG = gains(index, 2);
-	double gainB = gains(index, 3);
+	double gainR = gains(index, rgb?1:0);
+	double gainG = gains(index, rgb?2:0);
+	double gainB = gains(index, rgb?3:0);
 	UDEBUG("index=%d gain=%f (%f,%f,%f)", index, gains(index, 0), gainR, gainG, gainB);
 	if(indices->size())
 	{
@@ -425,34 +425,38 @@ void applyImpl(
 
 void GainCompensator::apply(
 		int id,
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) const
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		bool rgb) const
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	apply(id, cloud, indices);
+	apply(id, cloud, indices, rgb);
 }
 void GainCompensator::apply(
 		int id,
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const pcl::IndicesPtr & indices) const
+		const pcl::IndicesPtr & indices,
+		bool rgb) const
 {
 	UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
-	applyImpl<pcl::PointXYZRGB>(idToIndex_.at(id), cloud, indices, gains_);
+	applyImpl<pcl::PointXYZRGB>(idToIndex_.at(id), cloud, indices, gains_, rgb);
 }
 void GainCompensator::apply(
 		int id,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		const pcl::IndicesPtr & indices) const
+		const pcl::IndicesPtr & indices,
+		bool rgb) const
 {
 	UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
-	applyImpl<pcl::PointXYZRGBNormal>(idToIndex_.at(id), cloud, indices, gains_);
+	applyImpl<pcl::PointXYZRGBNormal>(idToIndex_.at(id), cloud, indices, gains_, rgb);
 }
 
 void GainCompensator::apply(
 		int id,
-		cv::Mat & image) const
+		cv::Mat & image,
+		bool rgb) const
 {
 	UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
-	if(image.channels() == 1)
+	if(image.channels() == 1 || !rgb)
 	{
 		cv::multiply(image, gains_(idToIndex_.at(id), 0), image);
 	}

corelib/src/Graph.cpp

@@ -425,6 +425,124 @@ bool importPoses(
 	return false;
 }
 
+// KITTI evaluation
+float lengths[] = {100,200,300,400,500,600,700,800};
+int32_t num_lengths = 8;
+
+struct errors {
+	int32_t first_frame;
+	float   r_err;
+	float   t_err;
+	float   len;
+	float   speed;
+	errors (int32_t first_frame,float r_err,float t_err,float len,float speed) :
+		first_frame(first_frame),r_err(r_err),t_err(t_err),len(len),speed(speed) {}
+};
+
+std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) {
+	std::vector<float> dist;
+	dist.push_back(0);
+	for (unsigned int i=1; i<poses.size(); i++) {
+		Transform P1 = poses[i-1];
+		Transform P2 = poses[i];
+		float dx = P1.x()-P2.x();
+		float dy = P1.y()-P2.y();
+		float dz = P1.z()-P2.z();
+		dist.push_back(dist[i-1]+sqrt(dx*dx+dy*dy+dz*dz));
+	}
+	return dist;
+}
+
+int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,float len) {
+	for (unsigned int i=first_frame; i<dist.size(); i++)
+		if (dist[i]>dist[first_frame]+len)
+			return i;
+	return -1;
+}
+
+inline float rotationError(const Transform &pose_error) {
+	float a = pose_error(0,0);
+	float b = pose_error(1,1);
+	float c = pose_error(2,2);
+	float d = 0.5*(a+b+c-1.0);
+	return std::acos(std::max(std::min(d,1.0f),-1.0f));
+}
+
+inline float translationError(const Transform &pose_error) {
+	float dx = pose_error.x();
+	float dy = pose_error.y();
+	float dz = pose_error.z();
+	return sqrt(dx*dx+dy*dy+dz*dz);
+}
+
+void calcKittiSequenceErrors (
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err) {
+
+	UASSERT(poses_gt.size() == poses_result.size());
+
+	// error vector
+	std::vector<errors> err;
+
+	// parameters
+	int32_t step_size = 10; // every second
+
+	// pre-compute distances (from ground truth as reference)
+	std::vector<float> dist = trajectoryDistances(poses_gt);
+
+	// for all start positions do
+	for (unsigned int first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
+
+		// for all segment lengths do
+		for (int32_t i=0; i<num_lengths; i++) {
+
+			// current length
+			float len = lengths[i];
+
+			// compute last frame
+			int32_t last_frame = lastFrameFromSegmentLength(dist,first_frame,len);
+
+			// continue, if sequence not long enough
+			if (last_frame==-1)
+				continue;
+
+			// compute rotational and translational errors
+			Transform pose_delta_gt     = poses_gt[first_frame].inverse()*poses_gt[last_frame];
+			Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
+			Transform pose_error        = pose_delta_result.inverse()*pose_delta_gt;
+			float r_err = rotationError(pose_error);
+			float t_err = translationError(pose_error);
+
+			// compute speed
+			float num_frames = (float)(last_frame-first_frame+1);
+			float speed = len/(0.1*num_frames);
+
+			// write to file
+			err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
+		}
+	}
+
+	t_err = 0;
+	r_err = 0;
+
+	// for all errors do => compute sum of t_err, r_err
+	for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
+	{
+		t_err += it->t_err;
+		r_err += it->r_err;
+	}
+
+	// save errors
+	float num = err.size();
+	t_err /= num;
+	r_err /= num;
+	t_err *= 100.0f;    // Translation error (%)
+	r_err *= 180/CV_PI; // Rotation error (deg/m)
+}
+// KITTI evaluation end
+
 
 ////////////////////////////////////////////
 // Graph utilities
@@ -1338,61 +1456,64 @@ std::list<std::pair<int, Transform> > computePath(
 		}
 		for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
-			Transform nextPose = currentNode->pose()*iter->second.transform();
-			float cost = 0.0f;
-			if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
+			if(iter->second.from() != iter->second.to())
 			{
-				// use distance only
-				cost = iter->second.transform().getNorm();
-			}
-			else // use time
-			{
-				if(linearVelocity > 0.0f)
+				Transform nextPose = currentNode->pose()*iter->second.transform();
+				float cost = 0.0f;
+				if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
 				{
-					cost += iter->second.transform().getNorm()/linearVelocity;
+					// use distance only
+					cost = iter->second.transform().getNorm();
 				}
-				if(angularVelocity > 0.0f)
+				else // use time
 				{
-					Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
-					Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
-					float angle = pcl::getAngle3D(v1, v2);
-					cost += angle / angularVelocity;
-				}
-			}
-
-			std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
-			if(nodeIter == nodes.end())
-			{
-				Node n(iter->second.to(), currentNode->id(), nextPose);
-
-				n.setCostSoFar(currentNode->costSoFar() + cost);
-				nodes.insert(std::make_pair(iter->second.to(), n));
-				if(updateNewCosts)
-				{
-					pqmap.insert(std::make_pair(n.totalCost(), n.id()));
-				}
-				else
-				{
-					pq.push(Pair(n.id(), n.totalCost()));
-				}
-			}
-			else if(updateNewCosts && nodeIter->second.isOpened())
-			{
-				float newCostSoFar = currentNode->costSoFar() + cost;
-				if(nodeIter->second.costSoFar() > newCostSoFar)
-				{
-					// update pose with new link
-					nodeIter->second.setPose(nextPose);
-
-					// update the cost in the priority queue
-					for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
+					if(linearVelocity > 0.0f)
 					{
-						if(mapIter->second == nodeIter->first)
+						cost += iter->second.transform().getNorm()/linearVelocity;
+					}
+					if(angularVelocity > 0.0f)
+					{
+						Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
+						Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
+						float angle = pcl::getAngle3D(v1, v2);
+						cost += angle / angularVelocity;
+					}
+				}
+
+				std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
+				if(nodeIter == nodes.end())
+				{
+					Node n(iter->second.to(), currentNode->id(), nextPose);
+
+					n.setCostSoFar(currentNode->costSoFar() + cost);
+					nodes.insert(std::make_pair(iter->second.to(), n));
+					if(updateNewCosts)
+					{
+						pqmap.insert(std::make_pair(n.totalCost(), n.id()));
+					}
+					else
+					{
+						pq.push(Pair(n.id(), n.totalCost()));
+					}
+				}
+				else if(updateNewCosts && nodeIter->second.isOpened())
+				{
+					float newCostSoFar = currentNode->costSoFar() + cost;
+					if(nodeIter->second.costSoFar() > newCostSoFar)
+					{
+						// update pose with new link
+						nodeIter->second.setPose(nextPose);
+
+						// update the cost in the priority queue
+						for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
 						{
-							pqmap.erase(mapIter);
-							nodeIter->second.setCostSoFar(newCostSoFar);
-							pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
-							break;
+							if(mapIter->second == nodeIter->first)
+							{
+								pqmap.erase(mapIter);
+								nodeIter->second.setCostSoFar(newCostSoFar);
+								pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
+								break;
+							}
 						}
 					}
 				}
@@ -1636,6 +1757,29 @@ float computePathLength(
 	return length;
 }
 
+float computePathLength(
+		const std::map<int, Transform> & path)
+{
+	float length = 0.0f;
+	if(path.size() > 1)
+	{
+		float x=0, y=0, z=0;
+		std::map<int, Transform>::const_iterator iter=path.begin();
+		Transform previousPose = iter->second;
+		++iter;
+		for(; iter!=path.end(); ++iter)
+		{
+			const Transform & currentPose = iter->second;
+			x += fabs(previousPose.x() - currentPose.x());
+			y += fabs(previousPose.y() - currentPose.y());
+			z += fabs(previousPose.z() - currentPose.z());
+			previousPose = currentPose;
+		}
+		length = sqrt(x*x + y*y + z*z);
+	}
+	return length;
+}
+
 // return all paths linked only by neighbor links
 std::list<std::map<int, Transform> > getPaths(
 		std::map<int, Transform> poses,

corelib/src/Link.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Link.h"
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/Compression.h>
 
 namespace rtabmap {
@@ -61,29 +62,6 @@ Link::Link(int from,
 		_userDataRaw = userData;
 	}
 }
-Link::Link(int from,
-		int to,
-		Type type,
-		const Transform & transform,
-		double rotVariance,
-		double transVariance,
-		const cv::Mat & userData) :
-	from_(from),
-	to_(to),
-	transform_(transform),
-	type_(type)
-{
-	setVariance(rotVariance, transVariance);
-
-	if(userData.type() == CV_8UC1) // Bytes
-	{
-		_userDataCompressed = userData; // assume compressed
-	}
-	else
-	{
-		_userDataRaw = userData;
-	}
-}
 
 double Link::rotVariance() const
 {
@@ -100,25 +78,14 @@ double Link::transVariance() const
 
 void Link::setInfMatrix(const cv::Mat & infMatrix) {
 	UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
-	UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(0,0)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(1,1)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(2,2)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(3,3)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(4,4)).c_str());
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(5,5)).c_str());
 	infMatrix_ = infMatrix;
 }
-void Link::setVariance(double rotVariance, double transVariance) {
-	UASSERT(uIsFinite(rotVariance) && rotVariance>0);
-	UASSERT(uIsFinite(transVariance) && transVariance>0);
-	infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
-	infMatrix_.at<double>(0,0) = 1.0/transVariance;
-	infMatrix_.at<double>(1,1) = 1.0/transVariance;
-	infMatrix_.at<double>(2,2) = 1.0/transVariance;
-	infMatrix_.at<double>(3,3) = 1.0/rotVariance;
-	infMatrix_.at<double>(4,4) = 1.0/rotVariance;
-	infMatrix_.at<double>(5,5) = 1.0/rotVariance;
-}
 
 void Link::uncompressUserData()
 {
@@ -150,7 +117,7 @@ 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_ + link.infMatrix());
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
 }
 
 Link Link::inverse() const

corelib/src/Memory.cpp

@@ -74,6 +74,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
 	_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
+	_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_reduceGraph(Parameters::defaultMemReduceGraph()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
@@ -425,6 +426,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
 	Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
 	Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
+	Parameters::parse(parameters, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -580,13 +582,14 @@ bool Memory::update(
 		const SensorData & data,
 		Statistics * stats)
 {
-	return update(data, Transform(), cv::Mat(), stats);
+	return update(data, Transform(), cv::Mat(), std::vector<float>(), stats);
 }
 
 bool Memory::update(
 		const SensorData & data,
 		const Transform & pose,
 		const cv::Mat & covariance,
+		const std::vector<float> & velocity,
 		Statistics * stats)
 {
 	UDEBUG("");
@@ -613,6 +616,10 @@ bool Memory::update(
 		UERROR("Failed to create a signature...");
 		return false;
 	}
+	if(velocity.size()==6)
+	{
+		signature->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
+	}
 
 	t=timer.ticks()*1000;
 	if(stats) stats->addStatistic(Statistics::kTimingMemSignature_creation(), t);
@@ -711,7 +718,15 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 				if(!signature->getPose().isNull() &&
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
+					UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
 					cv::Mat infMatrix = covariance.inv();
+					if((uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0.0) &&
+						!(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0.0))
+					{
+						UERROR("Failed to invert the covariance matrix! Covariance matrix should be invertible!");
+						std::cout << "Covariance: " << covariance << std::endl;
+						infMatrix = cv::Mat::eye(6,6,CV_64FC1);
+					}
 					motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
 					_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
 					signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));
@@ -798,6 +813,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 			if(!merge)
 			{
 				merge = iter->second.to() < s->id() && // should be a parent->child link
+						iter->second.to() != iter->second.from() &&
 						iter->second.type() != Link::kNeighbor &&
 						iter->second.type() != Link::kNeighborMerged &&
 						iter->second.userDataCompressed().empty() &&
@@ -951,6 +967,7 @@ std::map<int, Link> Memory::getLoopClosureLinks(
 		{
 			if(iter->second.type() != Link::kNeighbor &&
 			   iter->second.type() != Link::kNeighborMerged &&
+			   iter->second.type() != Link::kPosePrior &&
 			   iter->second.type() != Link::kUndef)
 			{
 				loopClosures.insert(*iter);
@@ -1642,6 +1659,56 @@ void Memory::saveStatistics(const Statistics & statistics)
 	}
 }
 
+void Memory::savePreviewImage(const cv::Mat & image) const
+{
+	if(_dbDriver)
+	{
+		_dbDriver->savePreviewImage(image);
+	}
+}
+cv::Mat Memory::loadPreviewImage() const
+{
+	if(_dbDriver)
+	{
+		return _dbDriver->loadPreviewImage();
+	}
+	return cv::Mat();
+}
+
+void Memory::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,
+#else
+		const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+		const cv::Mat & textures) const
+{
+	if(_dbDriver)
+	{
+		_dbDriver->saveOptimizedMesh(cloud, poses, polygons, texCoords, textures);
+	}
+}
+
+cv::Mat Memory::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,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+			cv::Mat * textures) const
+{
+	if(_dbDriver)
+	{
+		return _dbDriver->loadOptimizedMesh(poses, polygons, texCoords, textures);
+	}
+	return cv::Mat();
+}
+
 void Memory::emptyTrash()
 {
 	if(_dbDriver)
@@ -1836,27 +1903,30 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 			const std::map<int, Link> & links = s->getLinks();
 			for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
-				Signature * sTo = this->_getSignature(iter->first);
-				// neighbor to s
-				UASSERT_MSG(sTo!=0,
-							uFormat("A neighbor (%d) of the deleted location %d is "
-									"not found in WM/STM! Are you deleting a location "
-									"outside the STM?", iter->first, s->id()).c_str());
-
-				if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
+				if(iter->second.from() != iter->second.to())
 				{
-					UWARN("Link %d of %d is newer, removing neighbor link "
-						  "may split the map!",
-							iter->first, s->id());
-				}
+					Signature * sTo = this->_getSignature(iter->first);
+					// neighbor to s
+					UASSERT_MSG(sTo!=0,
+								uFormat("A neighbor (%d) of the deleted location %d is "
+										"not found in WM/STM! Are you deleting a location "
+										"outside the STM?", iter->first, s->id()).c_str());
 
-				// child
-				if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
-				{
-					sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
-				}
+					if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
+					{
+						UWARN("Link %d of %d is newer, removing neighbor link "
+							  "may split the map!",
+								iter->first, s->id());
+					}
 
-				sTo->removeLink(s->id());
+					// child
+					if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
+					{
+						sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
+					}
+
+					sTo->removeLink(s->id());
+				}
 
 			}
 			s->removeLinks(); // remove all links
@@ -2082,6 +2152,7 @@ void Memory::removeLink(int oldId, int newId)
 			{
 				if(iter->second.type() != Link::kNeighbor &&
 				   iter->second.type() != Link::kNeighborMerged &&
+				   iter->second.type() != Link::kPosePrior &&
 				   iter->first < newS->id())
 				{
 					noChildrenAnymore = false;
@@ -2262,14 +2333,6 @@ Transform Memory::computeTransform(
 					info->rejectedMsg = msg;
 				}
 			}
-			else if(info && !transform.isIdentity())
-			{
-				//normalize variance
-				info->varianceLin *= transform.getNorm();
-				info->varianceAng *= transform.getAngle();
-				info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
-				info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
-			}
 		}
 	}
 	return transform;
@@ -2317,15 +2380,6 @@ Transform Memory::computeIcpTransform(
 		// compute transform fromId -> toId
 		std::vector<int> inliersV;
 		t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
-
-		if(!t.isNull() && !t.isIdentity() && info)
-		{
-			// normalize variance
-			info->varianceLin *= t.getNorm();
-			info->varianceAng *= t.getAngle();
-			info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
-			info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
-		}
 	}
 	else
 	{
@@ -2704,7 +2758,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
 					{
 						childIds.insert(*iter);
 					}
-					else
+					else if(iter->second.from() != iter->second.to())
 					{
 						loopIds.insert(*iter);
 					}
@@ -2734,8 +2788,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
 void Memory::rehearsal(Signature * signature, Statistics * stats)
 {
 	UTimer timer;
-	if(signature->getLinks().size() != 1 ||
-	   signature->isBadSignature())
+	if(signature->isBadSignature())
 	{
 		return;
 	}
@@ -2800,7 +2853,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 		std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
 		if(iter != oldS->getLinks().end() &&
 		   iter->second.type() != Link::kNeighbor &&
-		   iter->second.type() != Link::kNeighborMerged)
+		   iter->second.type() != Link::kNeighborMerged &&
+		   iter->second.from() != iter->second.to())
 		{
 			// do nothing, already merged
 			UWARN("already merged, old=%d, new=%d", oldId, newId);
@@ -2814,7 +2868,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 
 		bool fullMerge;
 		bool intermediateMerge = false;
-		if(!newS->getLinks().begin()->second.transform().isNull())
+		if(!newS->getLinks().empty() && !newS->getLinks().begin()->second.transform().isNull())
 		{
 			// we are in metric SLAM mode:
 			// 1) Normal merge if not moving AND has direct link
@@ -2854,28 +2908,31 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				const std::map<int, Link> & links = oldS->getLinks();
 				for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 				{
-					Link link = iter->second;
-					Link mergedLink = newToOldLink.merge(link, link.type());
-					UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
-
-					Signature * s = this->_getSignature(link.to());
-					if(s)
+					if(iter->second.from() != iter->second.to())
 					{
-						// modify neighbor "from"
-						s->removeLink(oldS->id());
-						s->addLink(mergedLink.inverse());
+						Link link = iter->second;
+						Link mergedLink = newToOldLink.merge(link, link.type());
+						UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
 
-						newS->addLink(mergedLink);
-					}
-					else
-					{
-						UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
+						Signature * s = this->_getSignature(link.to());
+						if(s)
+						{
+							// modify neighbor "from"
+							s->removeLink(oldS->id());
+							s->addLink(mergedLink.inverse());
+
+							newS->addLink(mergedLink);
+						}
+						else
+						{
+							UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
+						}
 					}
 				}
 				newS->setLabel(oldS->getLabel());
 				oldS->setLabel("");
 				oldS->removeLinks(); // remove all links
-				oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location
+				oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
 
 				// Set old image to new signature
 				this->copyData(oldS, newS);
@@ -2890,7 +2947,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 			}
 			else
 			{
-				newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location
+				newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
 
 				// update weight
 				oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
@@ -2949,7 +3006,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
 	int mapId, weight;
 	std::string label;
 	double stamp;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
+	std::vector<float> velocity;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
 	return pose;
 }
 
@@ -2959,7 +3017,8 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
 	int mapId, weight;
 	std::string label;
 	double stamp;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
+	std::vector<float> velocity;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
 	return groundTruth;
 }
 
@@ -2970,6 +3029,7 @@ bool Memory::getNodeInfo(int signatureId,
 		std::string & label,
 		double & stamp,
 		Transform & groundTruth,
+		std::vector<float> & velocity,
 		bool lookInDatabase) const
 {
 	const Signature * s = this->getSignature(signatureId);
@@ -2981,11 +3041,12 @@ bool Memory::getNodeInfo(int signatureId,
 		label = s->getLabel();
 		stamp = s->getStamp();
 		groundTruth = s->getGroundTruthPose();
+		velocity = s->getVelocity();
 		return true;
 	}
 	else if(lookInDatabase && _dbDriver)
 	{
-		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth);
+		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity);
 	}
 	return false;
 }
@@ -3211,7 +3272,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 
 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().size() == 0 &&
-		!data.stereoCameraModel().isValidForProjection())
+		!data.stereoCameraModel().isValidForProjection() &&
+		!pose.isNull())
 	{
 		UERROR("Rectified images required! Calibrate your camera.");
 		return 0;
@@ -3669,7 +3731,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 
 	Signature * s;
-	if(this->isBinDataKept() && !isIntermediateNode)
+	if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
 	{
 		UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
 				image.empty()?0:1,
@@ -3853,6 +3915,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
 
+	// prior
+	if(!isIntermediateNode && !data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
+	{
+		s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
+	}
+
 	return s;
 }
 

corelib/src/OccupancyGrid.cpp

@@ -65,6 +65,10 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
 	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
 	projRayTracing_(Parameters::defaultGridProjRayTracing()),
+	fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
+	minMapSize_(Parameters::defaultGridGlobalMinSize()),
+	erode_(Parameters::defaultGridGlobalEroded()),
+	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
 	xMin_(0.0f),
 	yMin_(0.0f)
 {
@@ -94,26 +98,6 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
 	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
 	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
-	if(maxGroundHeight_ > 0 &&
-		maxObstacleHeight_ > 0 &&
-		maxObstacleHeight_ < maxGroundHeight_)
-	{
-		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
-				Parameters::kGridMaxGroundHeight().c_str(),
-				Parameters::kGridMaxObstacleHeight().c_str(),
-				Parameters::kGridMaxObstacleHeight().c_str());
-		maxObstacleHeight_ = 0;
-	}
-	if(maxGroundHeight_ > 0 &&
-			minGroundHeight_ > 0 &&
-		maxGroundHeight_ < minGroundHeight_)
-	{
-		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
-				Parameters::kGridMinGroundHeight().c_str(),
-				Parameters::kGridMaxGroundHeight().c_str(),
-				Parameters::kGridMinGroundHeight().c_str());
-		minGroundHeight_ = 0;
-	}
 	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
 	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
 	{
@@ -131,6 +115,12 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
 	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
 	Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
+	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
+	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
+	Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
+	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
+
+	UASSERT(minMapSize_ >= 0.0f);
 
 	// convert ROI from string to vector
 	ParametersMap::const_iterator iter;
@@ -165,13 +155,33 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 		}
 	}
 
-	if(maxGroundHeight_ <= 0.0f && !normalsSegmentation_)
+	if(maxGroundHeight_ == 0.0f && !normalsSegmentation_)
 	{
-		UWARN("\"%s\" should be greater than 0 if not using normals "
+		UWARN("\"%s\" should be not equal to 0 if not using normals "
 				"segmentation approach. Setting it to cell size (%f).",
 				Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
 		maxGroundHeight_ = cellSize_;
 	}
+	if(maxGroundHeight_ != 0.0f &&
+		maxObstacleHeight_ != 0.0f &&
+		maxObstacleHeight_ < maxGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str());
+		maxObstacleHeight_ = 0;
+	}
+	if(maxGroundHeight_ != 0.0f &&
+		minGroundHeight_ != 0.0f &&
+		maxGroundHeight_ < minGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMinGroundHeight().c_str(),
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMinGroundHeight().c_str());
+		minGroundHeight_ = 0;
+	}
 }
 
 void OccupancyGrid::setCellSize(float cellSize)
@@ -208,6 +218,7 @@ void OccupancyGrid::createLocalMap(
 
 		util3d::occupancy2DFromLaserScan(
 				util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
+				cv::Mat(),
 				viewPoint,
 				ground,
 				obstacles,
@@ -288,7 +299,8 @@ void OccupancyGrid::createLocalMap(
 			viewPoint.z = viewpointRotated.z();
 		}
 
-		if(cloud->size())
+		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>);
@@ -345,9 +357,12 @@ void OccupancyGrid::createLocalMap(
 					if(projRayTracing_)
 					{
 						cv::Mat laserScan = obstacles;
+						cv::Mat laserScanNoHit = ground;
 						obstacles = cv::Mat();
+						ground = cv::Mat();
 						util3d::occupancy2DFromLaserScan(
 								laserScan,
+								laserScanNoHit,
 								viewPoint,
 								ground,
 								obstacles,
@@ -373,44 +388,58 @@ void OccupancyGrid::clear()
 	addedNodes_.clear();
 }
 
+const cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
+{
+	xMin = xMin_;
+	yMin = yMin_;
+	if(erode_ && !map_.empty())
+	{
+		return util3d::erodeMap(map_);
+	}
+	return map_;
+}
+
 void OccupancyGrid::addToCache(
 		int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
 }
 
-void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMapSize, float footprintRadius)
+void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 {
 	UTimer timer;
 	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
 
-	float margin = cellSize_*10.0f+footprintRadius;
+	float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_;
 
-	float minX=-minMapSize/2.0f;
-	float minY=-minMapSize/2.0f;
-	float maxX=minMapSize/2.0f;
-	float maxY=minMapSize/2.0f;
-	bool undefinedSize = minMapSize == 0.0f;
+	float minX=-minMapSize_/2.0f;
+	float minY=-minMapSize_/2.0f;
+	float maxX=minMapSize_/2.0f;
+	float maxY=minMapSize_/2.0f;
+	bool undefinedSize = minMapSize_ == 0.0f;
 	std::map<int, cv::Mat> emptyLocalMaps;
 	std::map<int, cv::Mat> occupiedLocalMaps;
 
-	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
-	bool graphChanged = false;
+	// First, check of the graph has changed. If so, re-create the map by moving all occupied nodes (fullUpdate==false).
+	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;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
 		if(jter != posesIn.end())
 		{
+			graphChanged = false;
+
 			UASSERT(!iter->second.isNull() && !jter->second.isNull());
 			Transform t = Transform::getIdentity();
 			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
 			{
 				t = jter->second * iter->second.inverse();
-				graphChanged = true;
+				graphOptimized = true;
 			}
 			transforms.insert(std::make_pair(jter->first, t));
 
@@ -441,78 +470,89 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 		}
 	}
 
-	if(graphChanged && !map_.empty())
+	if(graphOptimized || graphChanged)
 	{
-		UINFO("Graph changed!");
-
-		// 1) recreate all local maps
-		UASSERT(map_.cols == mapInfo_.cols &&
-				map_.rows == mapInfo_.rows);
-		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(graphChanged)
 		{
-			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)));
+			UWARN("Graph has changed! The whole map should be rebuilt.");
 		}
-		for(int y=1; y<map_.rows-1; ++y)
+		else
 		{
-			for(int x=1; x<map_.cols-1; ++x)
+			UINFO("Graph optimized!");
+		}
+
+		if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells
+		{
+			// 1) recreate all local maps
+			UASSERT(map_.cols == mapInfo_.cols &&
+					map_.rows == mapInfo_.rows);
+			std::map<int, std::pair<int, int> > tmpIndices;
+			for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
 			{
-				float * info = mapInfo_.ptr<float>(y,x);
-				int nodeId = (int)info[0];
-				if(nodeId > 0 && map_.at<char>(y,x) >= 0)
+				if(iter->second.first)
 				{
-					std::map<int, Transform>::iterator tter = transforms.find(nodeId);
-					if(tter != transforms.end() && !uContains(cache_, nodeId))
+					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)));
+			}
+			for(int y=1; y<map_.rows-1; ++y)
+			{
+				for(int x=1; x<map_.cols-1; ++x)
+				{
+					float * info = mapInfo_.ptr<float>(y,x);
+					int nodeId = (int)info[0];
+					if(nodeId > 0 && map_.at<char>(y,x) >= 0)
 					{
-						cv::Point3f pt(info[1], info[2], 0.0f);
-						pt = util3d::transformPoint(pt, tter->second);
-
-						if(minX > pt.x)
-							minX = pt.x;
-						else if(maxX < pt.x)
-							maxX = pt.x;
-
-						if(minY > pt.y)
-							minY = pt.y;
-						else if(maxY < pt.y)
-							maxY = pt.y;
-
-						std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
-						if(map_.at<char>(y, x) == 0)
+						std::map<int, Transform>::iterator tter = transforms.find(nodeId);
+						if(tter != transforms.end() && !uContains(cache_, nodeId))
 						{
-							// ground
-							std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
-							UASSERT(iter != emptyLocalMaps.end());
-							UASSERT(jter->second.first < iter->second.cols);
-							float * ptf = iter->second.ptr<float>(0,jter->second.first++);
-							ptf[0] = pt.x;
-							ptf[1] = pt.y;
-						}
-						else
-						{
-							// obstacle
-							std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
-							UASSERT(iter != occupiedLocalMaps.end());
-							UASSERT(iter!=occupiedLocalMaps.end());
-							UASSERT(jter->second.second < iter->second.cols);
-							float * ptf = iter->second.ptr<float>(0,jter->second.second++);
-							ptf[0] = pt.x;
-							ptf[1] = pt.y;
+							cv::Point3f pt(info[1], info[2], 0.0f);
+							pt = util3d::transformPoint(pt, tter->second);
+
+							if(minX > pt.x)
+								minX = pt.x;
+							else if(maxX < pt.x)
+								maxX = pt.x;
+
+							if(minY > pt.y)
+								minY = pt.y;
+							else if(maxY < pt.y)
+								maxY = pt.y;
+
+							std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
+							if(map_.at<char>(y, x) == 0)
+							{
+								// ground
+								std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
+								UASSERT(iter != emptyLocalMaps.end());
+								UASSERT(jter->second.first < iter->second.cols);
+								float * ptf = iter->second.ptr<float>(0,jter->second.first++);
+								ptf[0] = pt.x;
+								ptf[1] = pt.y;
+							}
+							else
+							{
+								// obstacle
+								std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
+								UASSERT(iter != occupiedLocalMaps.end());
+								UASSERT(iter!=occupiedLocalMaps.end());
+								UASSERT(jter->second.second < iter->second.cols);
+								float * ptf = iter->second.ptr<float>(0,jter->second.second++);
+								ptf[0] = pt.x;
+								ptf[1] = pt.y;
+							}
 						}
 					}
 				}
 			}
+
+			UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
 		}
 
-		UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
 		addedNodes_.clear();
 		map_ = cv::Mat();
 		mapInfo_ = cv::Mat();
@@ -523,25 +563,36 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 	else if(!map_.empty())
 	{
 		// update
-		minX=xMin_+margin;
-		minY=yMin_+margin;
+		minX=xMin_+margin+cellSize_/2.0f;
+		minY=yMin_+margin+cellSize_/2.0f;
 		maxX=xMin_+float(map_.cols)*cellSize_ - margin;
 		maxY=yMin_+float(map_.rows)*cellSize_ - margin;
 		undefinedSize = false;
 	}
 
+	bool incrementalGraphUpdate = graphOptimized && !fullUpdate_ && !graphChanged;
+
 	std::list<std::pair<int, Transform> > poses;
-	// place negative poses at the end
-	for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
+	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
+	UDEBUG("Last id = %d", lastId);
+	if(lastId >= 0)
 	{
-		if(iter->first>0)
-		{
-			poses.push_front(*iter);
-		}
-		else
+		for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter)
 		{
 			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);
+			}
+			else
+			{
+				break;
+			}
+		}
 	}
 	for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
@@ -658,9 +709,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 	{
 		//Get map size
 		float xMin = minX-margin;
+		xMin -= cellSize_/2.0f;
 		float yMin = minY-margin;
+		yMin -= cellSize_/2.0f;
 		float xMax = maxX+margin;
 		float yMax = maxY+margin;
+
 		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
 		   fabs((xMax - xMin) / cellSize_) > 99999)
 		{
@@ -671,7 +725,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 		else
 		{
 			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
-			cv::Size newMapSize((xMax - xMin) / cellSize_ + 0.5f, (yMax - yMin) / cellSize_ + 0.5f);
+			cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
 			if(map_.empty())
 			{
 				UDEBUG("Map empty!");
@@ -691,6 +745,11 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 				}
 				else
 				{
+					UASSERT_MSG(xMin <= xMin_+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, xMin_, cellSize_).c_str());
+					UASSERT_MSG(yMin <= yMin_+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, yMin_, cellSize_).c_str());
+					UASSERT_MSG(xMax >= xMin_+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, xMin_, map_.cols, cellSize_).c_str());
+					UASSERT_MSG(yMax >= yMin_+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, yMin_, map_.rows, cellSize_).c_str());
+
 					UDEBUG("Copy map");
 					// copy the old map in the new map
 					// make sure the translation is cellSize
@@ -707,8 +766,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 						yMin = yMin_-float(deltaY)*cellSize_;
 					}
 					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
-					newMapSize.width = (xMax - xMin) / cellSize_ + 0.5f;
-					newMapSize.height = (yMax - yMin) / cellSize_ + 0.5f;
+					newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
+					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);
 					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)));
@@ -739,12 +800,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 					for(int i=0; i<iter->second.cols; ++i)
 					{
 						float * ptf = iter->second.ptr<float>(0,i);
-						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						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());
 						char & value = map.at<char>(pt.y, pt.x);
-						if(value != -2)
+						if(value != -2 && (!incrementalGraphUpdate || value==-1))
 						{
 							float * info = mapInfo.ptr<float>(pt.y, pt.x);
 							int nodeId = (int)info[0];
@@ -785,11 +846,11 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 					}
 				}
 
-				if(footprintRadius >= cellSize_*1.5f)
+				if(footprintRadius_ >= cellSize_*1.5f)
 				{
 					// place free space under the footprint of the robot
-					cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize_ + 0.5f);
-					cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize_ + 0.5f);
+					cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
+					cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
 					if(ptBegin.x < 0)
 						ptBegin.x = 0;
 					if(ptEnd.x >= map.cols)
@@ -835,8 +896,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 							if(kter->first > 0)
 							{
 								info[0] = (float)kter->first;
-								info[1] = float(i) * cellSize_ + xMin_ + 0.5f;
-								info[2] = float(j) * cellSize_ + yMin_ + 0.5f;
+								info[1] = float(i) * cellSize_ + xMin;
+								info[2] = float(j) * cellSize_ + yMin;
 								cter->second.first+=1;
 							}
 							value = -2; // free space (footprint)
@@ -849,7 +910,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 					for(int i=0; i<jter->second.cols; ++i)
 					{
 						float * ptf = jter->second.ptr<float>(0,i);
-						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						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());
@@ -896,36 +957,110 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 				}
 			}
 
-			// fill holes and put footprint values to empty (0)
-			//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-			//cloud->resize(map.rows*map.cols);
-			//int oi=0;
-			for(int i=1; i<map.rows-1; ++i)
+			if(footprintRadius_ >= cellSize_*1.5f || incrementalGraphUpdate)
 			{
-				for(int j=1; j<map.cols-1; ++j)
+				for(int i=1; i<map.rows-1; ++i)
 				{
-					char & value = map.at<char>(i, j);
-					if(value == -2)
+					for(int j=1; j<map.cols-1; ++j)
 					{
-						value = 0;
-					}
+						char & value = map.at<char>(i, j);
+						if(value == -2)
+						{
+							value = 0;
+						}
 
-					char sum =  (map.at<char>(i+1, j) != -1?1:0) +
-								(map.at<char>(i-1, j) != -1?1:0) +
-								(map.at<char>(i, j+1) != -1?1:0) +
-								(map.at<char>(i, j-1) != -1?1:0);
-					if(value == -1 && sum >=3)
-					{
-						value = 0;
-					}
+						if(incrementalGraphUpdate && value == -1)
+						{
+							float * info = mapInfo.ptr<float>(i, j);
 
-					//float * info = mapInfo.ptr<float>(i,j);
-					//if(info[0] > 0)
-					//{
-					//	cloud->at(oi).x = info[1];
-					//	cloud->at(oi).y = info[2];
-					//	oi++;
-					//}
+							// fill obstacle
+							if(map.at<char>(i+1, j) == 100 && map.at<char>(i-1, j) == 100)
+							{
+								value = 100;
+								// associate with the nearest pose
+								if(mapInfo.ptr<float>(i+1, j)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i+1, j)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+								else if(mapInfo.ptr<float>(i-1, j)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i-1, j)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+							}
+							else if(map.at<char>(i, j+1) == 100 && map.at<char>(i, j-1) == 100)
+							{
+								value = 100;
+								// associate with the nearest pose
+								if(mapInfo.ptr<float>(i, j+1)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i, j+1)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+								else if(mapInfo.ptr<float>(i, j-1)[0]>0.0f)
+								{
+									info[0] = mapInfo.ptr<float>(i, j-1)[0];
+									info[1] = float(j) * cellSize_ + xMin;
+									info[2] = float(i) * cellSize_ + yMin;
+									std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+									UASSERT(cter!=cellCount_.end());
+									cter->second.second+=1;
+								}
+							}
+							else
+							{
+								// fill empty
+								char sum =  (map.at<char>(i+1, j) == 0?1:0) +
+											(map.at<char>(i-1, j) == 0?1:0) +
+											(map.at<char>(i, j+1) == 0?1:0) +
+											(map.at<char>(i, j-1) == 0?1:0);
+								if(sum >=3)
+								{
+									value = 0;
+									// associate with the nearest pose, only check two cases (as 3 are required)
+									if(map.at<char>(i+1, j) != -1 && mapInfo.ptr<float>(i+1, j)[0]>0.0f)
+									{
+										info[0] = mapInfo.ptr<float>(i+1, j)[0];
+										info[1] = float(j) * cellSize_ + xMin;
+										info[2] = float(i) * cellSize_ + yMin;
+										std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+										UASSERT(cter!=cellCount_.end());
+										cter->second.first+=1;
+									}
+									else if(map.at<char>(i-1, j) != -1 && mapInfo.ptr<float>(i-1, j)[0]>0.0f)
+									{
+										info[0] = mapInfo.ptr<float>(i-1, j)[0];
+										info[1] = float(j) * cellSize_ + xMin;
+										info[2] = float(i) * cellSize_ + yMin;
+										std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(int(info[0]));
+										UASSERT(cter!=cellCount_.end());
+										cter->second.first+=1;
+									}
+								}
+							}
+						}
+
+						//float * info = mapInfo.ptr<float>(i,j);
+						//if(info[0] > 0)
+						//{
+						//	cloud->at(oi).x = info[1];
+						//	cloud->at(oi).y = info[2];
+						//	oi++;
+						//}
+					}
 				}
 			}
 			//if(graphChanged)
@@ -956,7 +1091,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMa
 		}
 	}
 
-	cache_.clear();
+	if(!fullUpdate_)
+	{
+		cache_.clear();
+	}
 
 	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
 }

corelib/src/OctoMap.cpp

@@ -35,9 +35,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-OctoMap::OctoMap(float voxelSize, float occupancyThr) :
+OctoMap::OctoMap(float voxelSize, float occupancyThr, bool fullUpdate) :
 		octree_(new octomap::ColorOcTree(voxelSize)),
-		hasColor_(false)
+		hasColor_(false),
+		fullUpdate_(fullUpdate)
 {
 	octree_->setOccupancyThres(occupancyThr);
 	UASSERT(voxelSize>0.0f);
@@ -67,8 +68,8 @@ void OctoMap::addToCache(int nodeId,
 		const pcl::PointXYZ & viewPoint)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-	cacheViewPoints_.insert(std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
+	uInsert(cacheClouds_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cacheViewPoints_, std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
 }
 void OctoMap::addToCache(int nodeId,
 		const cv::Mat & ground,
@@ -78,8 +79,8 @@ void OctoMap::addToCache(int nodeId,
 	UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
 	UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
 	UDEBUG("nodeId=%d", nodeId);
-	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-	cacheViewPoints_.insert(std::make_pair(nodeId, viewPoint));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
 }
 
 void OctoMap::update(const std::map<int, Transform> & poses)
@@ -87,7 +88,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
 
 	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
-	bool graphChanged = false;
+	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;
 	std::map<int, Transform> updatedAddedNodes;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
@@ -95,83 +97,104 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
 		if(jter != poses.end())
 		{
+			graphChanged = false;
 			UASSERT(!iter->second.isNull() && !jter->second.isNull());
 			Transform t = Transform::getIdentity();
 			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
 			{
 				t = jter->second * iter->second.inverse();
-				graphChanged = true;
+				graphOptimized = true;
 			}
 			transforms.insert(std::make_pair(jter->first, t));
 			updatedAddedNodes.insert(std::make_pair(jter->first, jter->second));
 		}
 		else
 		{
-			UWARN("Updated pose for node %d is not found, some points may not be copied.", jter->first);
+			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
 		}
 	}
-	if(graphChanged)
+	if(graphOptimized || graphChanged)
 	{
-		UINFO("Graph changed!");
-		octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
-		std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
-		int copied=0;
-		for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
-				iter!=occupiedCells_.end();
-				++iter)
+		if(graphChanged)
 		{
-			std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
-			if(jter != transforms.end())
+			UWARN("Graph has changed! The whole map should be rebuilt.");
+		}
+		else
+		{
+			UINFO("Graph optimized!");
+		}
+
+		if(fullUpdate_ || graphChanged)
+		{
+			// clear all but keep cache
+			octree_->clear();
+			occupiedCells_.clear();
+			addedNodes_.clear();
+			keyRay_ = octomap::KeyRay();
+			hasColor_ = false;
+		}
+		else
+		{
+			octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
+			std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
+			int copied=0;
+			for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
+					iter!=occupiedCells_.end();
+					++iter)
 			{
-				octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
-				std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
-				UASSERT(pter != addedNodes_.end());
-
-				cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
-				cvPt = util3d::transformPoint(cvPt, jter->second);
-
-				octomap::OcTreeKey key;
-				if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
+				std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
+				if(jter != transforms.end())
 				{
-					octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
-					if(n)
+					octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
+					std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
+					UASSERT(pter != addedNodes_.end());
+
+					cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
+					cvPt = util3d::transformPoint(cvPt, jter->second);
+
+					octomap::OcTreeKey key;
+					if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
 					{
-						++copied;
-						uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
-						newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+						octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
+						if(n)
+						{
+							++copied;
+							uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
+							newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+						}
+						else
+						{
+							UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+						}
 					}
 					else
 					{
-						UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+						UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
 					}
 				}
-				else
+				else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
 				{
-					UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+					UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
 				}
 			}
-			else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
-			{
-				UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
-			}
-		}
-		UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
-		delete octree_;
-		octree_ = newOcTree;
-		occupiedCells_ = newOccupiedCells;
+			UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
+			delete octree_;
+			octree_ = newOcTree;
+			occupiedCells_ = newOccupiedCells;
 
-		//update added poses
-		addedNodes_ = updatedAddedNodes;
+			//update added poses
+			addedNodes_ = updatedAddedNodes;
+		}
 	}
 
 	// Original version from A. Hornung:
 	// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
 	//
+	std::list<std::pair<int, Transform> > orderedPoses;
 	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
 	UDEBUG("Last id = %d", lastId);
 	if(lastId >= 0)
 	{
-		std::list<std::pair<int, Transform> > orderedPoses;
 		for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(lastId); iter!=poses.end(); ++iter)
 		{
 			orderedPoses.push_back(*iter);
@@ -188,173 +211,176 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				break;
 			}
 		}
+	}
 
-		UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
-		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
+	UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
+	for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
+	{
+		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
+		std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
+		std::map<int, cv::Point3f>::iterator viewPointIter;
+		cloudIter = cacheClouds_.find(iter->first);
+		occupancyIter = cache_.find(iter->first);
+		viewPointIter = cacheViewPoints_.find(iter->first);
+		if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
 		{
-			std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
-			std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
-			std::map<int, cv::Point3f>::iterator viewPointIter;
-			cloudIter = cacheClouds_.find(iter->first);
-			occupancyIter = cache_.find(iter->first);
-			viewPointIter = cacheViewPoints_.find(iter->first);
-			if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
+			UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
+
+			UASSERT(viewPointIter != cacheViewPoints_.end());
+			octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
+			sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
+
+			octomap::OcTreeKey tmpKey;
+			if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
+					|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
 			{
-				UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
-
-				UASSERT(viewPointIter != cacheViewPoints_.end());
-				octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
-				sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
-
-				octomap::OcTreeKey tmpKey;
-				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
-						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
-				{
-					UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
-				}
-
-				// instead of direct scan insertion, compute update to filter ground:
-				octomap::KeySet free_cells, occupied_cells, ground_cells;
-				// insert ground points only as free:
-				unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
-				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
-				Eigen::Affine3f t = iter->second.toEigen3f();
-				for (unsigned int i=0; i<maxGroundPts; ++i)
-				{
-					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
-					}
-
-					octomap::point3d point(pt.x, pt.y, pt.z);
-
-					// only clear space (ground points)
-					if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
-					{
-						free_cells.insert(keyRay_.begin(), keyRay_.end());
-					}
-					// occupied endpoint
-					octomap::OcTreeKey key;
-					if (octree_->coordToKeyChecked(point, key))
-					{
-						ground_cells.insert(key);
-
-						octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
-						if(n)
-						{
-							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
-							{
-								hasColor_ = true;
-							}
-							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
-							if(iter->first > 0)
-							{
-								uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
-							}
-							else
-							{
-								occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
-							}
-						}
-					}
-				}
-				UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
-
-				// all other points: free on ray, occupied on endpoint:
-				unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
-				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
-				for (unsigned int i=0; i<maxObstaclePts; ++i)
-				{
-					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
-					}
-
-					octomap::point3d point(pt.x, pt.y, pt.z);
-
-					// free cells
-					if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
-					{
-						free_cells.insert(keyRay_.begin(), keyRay_.end());
-					}
-					// occupied endpoint
-					octomap::OcTreeKey key;
-					if (octree_->coordToKeyChecked(point, key))
-					{
-						occupied_cells.insert(key);
-
-						octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
-						if(n)
-						{
-							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
-							{
-								hasColor_ = true;
-							}
-							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
-							if(iter->first > 0)
-							{
-								uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
-							}
-							else
-							{
-								occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
-							}
-						}
-					}
-				}
-				UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
-
-
-				// mark free cells only if not seen occupied in this cloud
-				for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
-				{
-					if (occupied_cells.find(*it) == occupied_cells.end() &&
-						ground_cells.find(*it) == ground_cells.end())
-					{
-						octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
-						if(n)
-						{
-							std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
-							gter = occupiedCells_.find(n);
-							if(gter != occupiedCells_.end() && gter->second.isObstacle_)
-							{
-								occupiedCells_.erase(gter);
-							}
-						}
-					}
-				}
-
-				// compress map
-				//octree_->prune();
-
-				// ignore negative ids as they are temporary clouds
-				if(iter->first > 0)
-				{
-					addedNodes_.insert(*iter);
-				}
-				UDEBUG("%d: end", iter->first);
+				UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
 			}
-			else
+
+			// instead of direct scan insertion, compute update to filter ground:
+			octomap::KeySet free_cells, occupied_cells, ground_cells;
+			// insert ground points only as free:
+			unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
+			UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
+			Eigen::Affine3f t = iter->second.toEigen3f();
+			for (unsigned int i=0; i<maxGroundPts; ++i)
 			{
-				UDEBUG("Did not find %d in cache", iter->first);
+				pcl::PointXYZRGB pt;
+				if(occupancyIter != cache_.end())
+				{
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
+					pt = pcl::transformPoint(pt, t);
+				}
+				else
+				{
+					pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
+				}
+
+				octomap::point3d point(pt.x, pt.y, pt.z);
+
+				// only clear space (ground points)
+				if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
+				{
+					free_cells.insert(keyRay_.begin(), keyRay_.end());
+				}
+				// occupied endpoint
+				octomap::OcTreeKey key;
+				if (octree_->coordToKeyChecked(point, key))
+				{
+					ground_cells.insert(key);
+
+					octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
+					if(n)
+					{
+						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+						{
+							hasColor_ = true;
+						}
+						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
+						if(iter->first > 0)
+						{
+							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+						}
+						else
+						{
+							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+						}
+					}
+				}
 			}
+			UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
+
+			// all other points: free on ray, occupied on endpoint:
+			unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
+			UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
+			for (unsigned int i=0; i<maxObstaclePts; ++i)
+			{
+				pcl::PointXYZRGB pt;
+				if(occupancyIter != cache_.end())
+				{
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
+					pt = pcl::transformPoint(pt, t);
+				}
+				else
+				{
+					pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
+				}
+
+				octomap::point3d point(pt.x, pt.y, pt.z);
+
+				// free cells
+				if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
+				{
+					free_cells.insert(keyRay_.begin(), keyRay_.end());
+				}
+				// occupied endpoint
+				octomap::OcTreeKey key;
+				if (octree_->coordToKeyChecked(point, key))
+				{
+					occupied_cells.insert(key);
+
+					octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
+					if(n)
+					{
+						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+						{
+							hasColor_ = true;
+						}
+						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
+						if(iter->first > 0)
+						{
+							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+						}
+						else
+						{
+							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+						}
+					}
+				}
+			}
+			UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
+
+
+			// mark free cells only if not seen occupied in this cloud
+			for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
+			{
+				if (occupied_cells.find(*it) == occupied_cells.end() &&
+					ground_cells.find(*it) == ground_cells.end())
+				{
+					octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
+					if(n)
+					{
+						std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
+						gter = occupiedCells_.find(n);
+						if(gter != occupiedCells_.end() && gter->second.isObstacle_)
+						{
+							occupiedCells_.erase(gter);
+						}
+					}
+				}
+			}
+
+			// compress map
+			//octree_->prune();
+
+			// ignore negative ids as they are temporary clouds
+			if(iter->first > 0)
+			{
+				addedNodes_.insert(*iter);
+			}
+			UDEBUG("%d: end", iter->first);
+		}
+		else
+		{
+			UDEBUG("Did not find %d in cache", iter->first);
 		}
 	}
-	cache_.clear();
-	cacheClouds_.clear();
-	cacheViewPoints_.clear();
+	if(!fullUpdate_)
+	{
+		cache_.clear();
+		cacheClouds_.clear();
+		cacheViewPoints_.clear();
+	}
 }
 
 void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
@@ -494,9 +520,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	return cloud;
 }
 
-cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize)
+cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
 {
-	gridCellSize = octree_->getResolution();
+	UASSERT(treeDepth <= octree_->getTreeDepth());
+	if(treeDepth == 0)
+	{
+		treeDepth = octree_->getTreeDepth();
+	}
+
+	gridCellSize = octree_->getNodeSize(treeDepth);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
@@ -505,16 +537,15 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	obstacles->resize(octree_->size());
 	int gi=0;
 	int oi=0;
-	for (octomap::ColorOcTree::iterator it = octree_->begin(octree_->getTreeDepth()); it != octree_->end(); ++it)
+	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
+		octomap::point3d pt = octree_->keyToCoord(it.getKey());
 		if(octree_->isNodeOccupied(*it))
 		{
-			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
 		}
 		else
 		{
-			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			(*ground)[gi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
 		}
 	}
@@ -531,17 +562,19 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	}
 
 	cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
+	cv::Vec2f * ptr = obstaclesMat.ptr<cv::Vec2f>(0,0);
 	for(unsigned int i=0;i<obstacles->size(); ++i)
 	{
-		obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
-		obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
+		ptr[i][0] = obstacles->at(i).x;
+		ptr[i][1] = obstacles->at(i).y;
 	}
 
 	cv::Mat groundMat = cv::Mat(1, (int)ground->size(), CV_32FC2);
+	ptr = groundMat.ptr<cv::Vec2f>(0,0);
 	for(unsigned int i=0;i<ground->size(); ++i)
 	{
-		groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;
-		groundMat.at<cv::Vec2f>(i)[1] = ground->at(i).y;
+		ptr[i][0] = ground->at(i).x;
+		ptr[i][1] = ground->at(i).y;
 	}
 
 	std::map<int, Transform> poses;

corelib/src/Odometry.cpp

@@ -28,6 +28,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OdometryF2M.h>
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryF2F.h"
+#include "rtabmap/core/OdometryFovis.h"
+#include "rtabmap/core/OdometryViso2.h"
+#include "rtabmap/core/OdometryDVO.h"
+#include "rtabmap/core/OdometryORBSLAM2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"
@@ -56,6 +60,18 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	Odometry * odometry = 0;
 	switch(type)
 	{
+	case Odometry::kTypeORBSLAM2:
+		odometry = new OdometryORBSLAM2(parameters);
+		break;
+	case Odometry::kTypeDVO:
+		odometry = new OdometryDVO(parameters);
+		break;
+	case Odometry::kTypeViso2:
+		odometry = new OdometryViso2(parameters);
+		break;
+	case Odometry::kTypeFovis:
+		odometry = new OdometryFovis(parameters);
+		break;
 	case Odometry::kTypeF2F:
 		odometry = new OdometryF2F(parameters);
 		break;
@@ -269,8 +285,20 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	}
 
 	double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
-	Transform guess = dt && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
-	UASSERT_MSG(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()), uFormat("dt=%f previous transform=%s", dt, previousVelocityTransform_.prettyPrint().c_str()).c_str());
+	Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
+	if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
+	{
+		if(guessFromMotion_)
+		{
+			UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
+		}
+		else if(_filteringStrategy==1)
+		{
+			UERROR("Kalman filtering is enalbed but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
+		}
+		dt=0;
+		previousVelocityTransform_.setNull();
+	}
 	if(!previousVelocityTransform_.isNull())
 	{
 		if(guessFromMotion_)
@@ -517,11 +545,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			info->distanceTravelled = distanceTravelled_;
 		}
 
-		info->varianceLin *= t.getNorm();
-		info->varianceAng *= t.getAngle();
-		info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
-		info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
-
 		return _pose *= t; // update
 	}
 	else if(_resetCurrentCount > 0)

corelib/src/OdometryDVO.cpp

@@ -0,0 +1,284 @@
+/*
+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/OdometryDVO.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/Version.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+
+#ifdef RTABMAP_DVO
+#include <dvo/dense_tracking.h>
+#include <dvo/core/surface_pyramid.h>
+#include <dvo/core/rgbd_image.h>
+#endif
+
+namespace rtabmap {
+
+OdometryDVO::OdometryDVO(const ParametersMap & parameters) :
+	Odometry(parameters),
+	dvo_(0),
+	reference_(0),
+	camera_(0),
+	lost_(false),
+	motionFromKeyFrame_(Transform::getIdentity())
+{
+}
+
+OdometryDVO::~OdometryDVO()
+{
+#ifdef RTABMAP_DVO
+	if(dvo_)
+	{
+		delete dvo_;
+	}
+	if(reference_)
+	{
+		delete reference_;
+	}
+	if(camera_)
+	{
+		delete camera_;
+	}
+#endif
+}
+
+void OdometryDVO::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_DVO
+	if(dvo_)
+	{
+		delete dvo_;
+		dvo_ = 0;
+	}
+	if(reference_)
+	{
+		delete reference_;
+		reference_ = 0;
+	}
+	if(camera_)
+	{
+		delete camera_;
+		camera_ = 0;
+	}
+	lost_ = false;
+	motionFromKeyFrame_.setIdentity();
+	previousLocalTransform_.setNull();
+#endif
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryDVO::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+
+#ifdef RTABMAP_DVO
+	UTimer timer;
+
+	if(data.imageRaw().empty() ||
+		data.imageRaw().rows != data.depthOrRightRaw().rows ||
+		data.imageRaw().cols != data.depthOrRightRaw().cols)
+	{
+		UERROR("Not supported input!");
+		return t;
+	}
+
+	if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()))
+	{
+		UERROR("Invalid camera model! Only single RGB-D camera supported by DVO. Try another odometry approach.");
+		return t;
+	}
+
+	if(dvo_ == 0)
+	{
+		dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
+
+		dvo_ = new dvo::DenseTracker(cfg);
+	}
+
+	cv::Mat grey, grey_s16, depth_inpainted, depth_mask, depth_mono, depth_float;
+	if(data.imageRaw().type() != CV_32FC1)
+	{
+		if(data.imageRaw().type() == CV_8UC3)
+		{
+			cv::cvtColor(data.imageRaw(), grey, CV_BGR2GRAY);
+		}
+		else
+		{
+			grey = data.imageRaw();
+		}
+
+		grey.convertTo(grey_s16, CV_32F);
+	}
+	else
+	{
+		grey_s16 = data.imageRaw();
+	}
+
+	// make sure all zeros are NAN
+	if(data.depthRaw().type() == CV_32FC1)
+	{
+		depth_float = data.depthRaw();
+		for(int i=0; i<depth_float.rows; ++i)
+		{
+			for(int j=0; j<depth_float.cols; ++j)
+			{
+				float & d = depth_float.at<float>(i,j);
+				if(d == 0.0f)
+				{
+					d = NAN;
+				}
+			}
+		}
+	}
+	else if(data.depthRaw().type() == CV_16UC1)
+	{
+		depth_float = cv::Mat(data.depthRaw().size(), CV_32FC1);
+		for(int i=0; i<data.depthRaw().rows; ++i)
+		{
+			for(int j=0; j<data.depthRaw().cols; ++j)
+			{
+				float d = float(data.depthRaw().at<unsigned short>(i,j))/1000.0f;
+				depth_float.at<float>(i, j) = d==0.0f?NAN:d;
+			}
+		}
+	}
+	else
+	{
+		UFATAL("Unknown depth format!");
+	}
+
+	if(camera_ == 0)
+	{
+		dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(
+						data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy(),
+						data.cameraModels()[0].cx(),
+						data.cameraModels()[0].cy());
+		camera_ = new dvo::core::RgbdCameraPyramid(
+				data.cameraModels()[0].imageWidth(),
+				data.cameraModels()[0].imageHeight(),
+				intrinsics);
+	}
+
+	dvo::core::RgbdImagePyramid * current = new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);
+
+	const Transform & localTransform = data.cameraModels()[0].localTransform();
+	cv::Mat covariance;
+	if(reference_ == 0)
+	{
+		reference_ = current;
+		if(!lost_)
+		{
+			t.setIdentity();
+		}
+		covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
+	}
+	else
+	{
+		dvo::DenseTracker::Result result;
+		dvo_->match(*reference_, *current, result);
+		t = Transform::fromEigen3d(result.Transformation);
+
+		if(result.Information(0,0) > 0.0 && result.Information(0,0) != 1.0)
+		{
+			lost_ = false;
+			cv::Mat information = cv::Mat::eye(6,6, CV_64FC1);
+			memcpy(information.data, result.Information.data(), 36*sizeof(double));
+			covariance = information.inv();
+			covariance *= 100.0; // to be in the same scale than loop closure detection
+
+			Transform currentMotion = t;
+			t = motionFromKeyFrame_.inverse() * t;
+
+			// TODO make parameters?
+			if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle() > 0.01)
+			{
+				if(info)
+				{
+					info->keyFrameAdded = true;
+				}
+				// new keyframe
+				delete reference_;
+				reference_ = current;
+				motionFromKeyFrame_.setIdentity();
+			}
+			else
+			{
+				delete current;
+				motionFromKeyFrame_ = currentMotion;
+			}
+		}
+		else
+		{
+			lost_ = true;
+			delete reference_;
+			delete current;
+			reference_ = 0; // this will make restart from the next frame
+			motionFromKeyFrame_.setIdentity();
+			t.setNull();
+			previousLocalTransform_.setNull();
+			covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
+			UWARN("dvo failed to estimate motion, tracking will be reinitialized on next frame.");
+		}
+
+		if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+		{
+			// from camera frame to base frame
+			if(!previousLocalTransform_.isNull())
+			{
+				t = previousLocalTransform_ * t * localTransform.inverse();
+			}
+			else
+			{
+				t = localTransform * t * localTransform.inverse();
+			}
+			previousLocalTransform_ = localTransform;
+		}
+	}
+
+	if(info)
+	{
+		info->type = (int)kTypeDVO;
+		info->covariance = covariance;
+	}
+
+	UINFO("Odom update time = %fs", timer.elapsed());
+
+#else
+	UERROR("RTAB-Map is not built with DVO support! Select another visual odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap

corelib/src/OdometryF2F.cpp

@@ -168,8 +168,7 @@ Transform OdometryF2F::computeTransform(
 		//return Identity
 		output = Transform::getIdentity();
 		// a very high variance tells that the new pose is not linked with the previous one
-		regInfo.varianceLin = 9999;
-		regInfo.varianceAng = 9999;
+		regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
 	}
 
 	if(!output.isNull())
@@ -245,8 +244,7 @@ Transform OdometryF2F::computeTransform(
 	if(info)
 	{
 		info->type = 1;
-		info->varianceLin = regInfo.varianceLin;
-		info->varianceAng = regInfo.varianceAng;
+		info->covariance = regInfo.covariance;
 		info->inliers = regInfo.inliers;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;